From 5ad4761bb3fc9b4d96d470630ee89b5a5bbd14e7 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 31 May 2011 22:42:33 +0200 Subject: digital/io-hub: move clamp to idle position once match started --- digital/io-hub/src/robospierre/clamp.c | 17 ++++++++++++++++- digital/io-hub/src/robospierre/logistic.c | 3 ++- 2 files changed, 18 insertions(+), 2 deletions(-) (limited to 'digital/io-hub/src/robospierre') diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index a9a1680b..979cc3c4 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -59,6 +59,10 @@ FSM_STATES ( CLAMP_INIT_FINDING_ROTATION_EDGE, /* Initialisation sequence: finding top switch. */ CLAMP_INIT_FINDING_TOP, + /* Initialisation sequence: going to rest position. */ + CLAMP_INIT_GOING_REST, + /* Clamp ready, waiting in rest position. */ + CLAMP_INIT_READY, /* Returning to idle position. */ CLAMP_GOING_IDLE, @@ -434,12 +438,23 @@ FSM_TRANS (CLAMP_INIT_FINDING_ROTATION_EDGE, clamp_rotation_success, } FSM_TRANS (CLAMP_INIT_FINDING_TOP, clamp_elevation_success, - CLAMP_GOING_IDLE) + CLAMP_INIT_GOING_REST) { clamp_move (CLAMP_BAY_SIDE_ENTER_LEAVE); return FSM_NEXT (CLAMP_INIT_FINDING_TOP, clamp_elevation_success); } +FSM_TRANS (CLAMP_INIT_GOING_REST, clamp_move_success, CLAMP_INIT_READY) +{ + return FSM_NEXT (CLAMP_INIT_GOING_REST, clamp_move_success); +} + +FSM_TRANS (CLAMP_INIT_READY, init_start_round, CLAMP_GOING_IDLE) +{ + clamp_move (logistic_global.clamp_pos_idle); + return FSM_NEXT (CLAMP_INIT_READY, init_start_round); +} + FSM_TRANS (CLAMP_GOING_IDLE, clamp_move_success, CLAMP_IDLE) { ctx.working = 0; diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index a1b2a705..4fff865d 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -419,7 +419,8 @@ logistic_init (void) ctx.slots[i] = 0; ctx.moving_from = ctx.moving_to = CLAMP_SLOT_NB; ctx.collect_direction = DIRECTION_FORWARD; - ctx.clamp_pos_idle = CLAMP_SLOT_FRONT_MIDDLE; + ctx.clamp_pos_idle = ctx.collect_direction == DIRECTION_FORWARD + ? CLAMP_SLOT_FRONT_MIDDLE : CLAMP_SLOT_BACK_MIDDLE; ctx.construct_possible = 0; ctx.prepare = 1; ctx.ready = 0; -- cgit v1.2.3