From cbf2a5beed9a75707f4a2fe00f685c2afd8dfba4 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 1 Jun 2011 16:20:26 +0200 Subject: digital/io-hub: add demo mode --- digital/io-hub/src/robospierre/clamp.c | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) (limited to 'digital/io-hub/src/robospierre/clamp.c') diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 3e7fa9fd..812d4890 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -447,10 +447,21 @@ FSM_TRANS (CLAMP_INIT_FINDING_ROTATION_EDGE, clamp_rotation_success, } FSM_TRANS (CLAMP_INIT_FINDING_TOP, clamp_elevation_success, - CLAMP_INIT_GOING_REST) + rest, CLAMP_INIT_GOING_REST, + demo, CLAMP_GOING_IDLE) { - clamp_move (CLAMP_BAY_SIDE_ENTER_LEAVE); - return FSM_NEXT (CLAMP_INIT_FINDING_TOP, clamp_elevation_success); + if (IO_GET (CONTACT_STRAT)) + { + clamp_move (CLAMP_BAY_SIDE_ENTER_LEAVE); + return FSM_NEXT (CLAMP_INIT_FINDING_TOP, clamp_elevation_success, + rest); + } + else + { + clamp_move (logistic_global.clamp_pos_idle); + return FSM_NEXT (CLAMP_INIT_FINDING_TOP, clamp_elevation_success, + demo); + } } FSM_TRANS (CLAMP_INIT_GOING_REST, clamp_move_success, CLAMP_INIT_READY) -- cgit v1.2.3