From 5620e71a2078bf504cebbbe146cbb33f73e63314 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 2 May 2008 11:30:59 +0200 Subject: * digital/io/src: - added get sample timeout. --- digital/io/src/getsamples.fsm | 5 +++++ digital/io/src/getsamples_cb.c | 18 ++++++++++++++++++ 2 files changed, 23 insertions(+) diff --git a/digital/io/src/getsamples.fsm b/digital/io/src/getsamples.fsm index 0628448b..154bb489 100644 --- a/digital/io/src/getsamples.fsm +++ b/digital/io/src/getsamples.fsm @@ -61,6 +61,7 @@ APPROACH_DISTRIBUTOR: MOVE_BACKWARD_FROM_DISTRIBUTOR: bot_move_succeed -> TAKE_SAMPLES start taking some samples + setup a timeout TAKE_SAMPLES: arm_pass_noted_position: more -> . @@ -68,6 +69,9 @@ TAKE_SAMPLES: take a new one arm_pass_noted_position: no_more -> MOVE_AWAY_FROM_DISTRIBUTOR go backward + wait_finished -> MOVE_AWAY_FROM_DISTRIBUTOR + timed out, give up + go backward MOVE_AWAY_FROM_DISTRIBUTOR: bot_move_succeed -> CLOSE_INPUT_HOLE @@ -79,3 +83,4 @@ CLOSE_INPUT_HOLE: tell the top FSM we have finished wait_finished -> IDLE timed out, give up + tell the top FSM we have finished diff --git a/digital/io/src/getsamples_cb.c b/digital/io/src/getsamples_cb.c index 49eda8d1..62543f1a 100644 --- a/digital/io/src/getsamples_cb.c +++ b/digital/io/src/getsamples_cb.c @@ -95,6 +95,7 @@ getsamples_count_samples (void) * MOVE_BACKWARD_FROM_DISTRIBUTOR =bot_move_succeed=> * => TAKE_SAMPLES * start taking some samples + * setup a timeout */ fsm_branch_t getsamples__MOVE_BACKWARD_FROM_DISTRIBUTOR__bot_move_succeed (void) @@ -105,6 +106,8 @@ getsamples__MOVE_BACKWARD_FROM_DISTRIBUTOR__bot_move_succeed (void) asserv_arm_set_position_reached (getsamples_data_.arm_noted_position); asserv_move_arm (getsamples_count_samples () * BOT_ARM_THIRD_ROUND, BOT_ARM_SPEED); + /* Post an event for the top FSM to be waked up later */ + main_getsamples_wait_cycle = GET_SAMPLES_ARM_TIMEOUT; return getsamples_next (MOVE_BACKWARD_FROM_DISTRIBUTOR, bot_move_succeed); } @@ -138,6 +141,7 @@ getsamples__OPEN_INPUT_HOLE__arm_move_succeed (void) * CLOSE_INPUT_HOLE =wait_finished=> * => IDLE * timed out, give up + * tell the top FSM we have finished */ fsm_branch_t getsamples__CLOSE_INPUT_HOLE__wait_finished (void) @@ -160,6 +164,20 @@ getsamples__CLOSE_INPUT_HOLE__arm_move_succeed (void) return getsamples_next (CLOSE_INPUT_HOLE, arm_move_succeed); } +/* + * TAKE_SAMPLES =wait_finished=> + * => MOVE_AWAY_FROM_DISTRIBUTOR + * timed out, give up + * go backward + */ +fsm_branch_t +getsamples__TAKE_SAMPLES__wait_finished (void) +{ + /* Go backward */ + asserv_move_linearly (-PG_DISTANCE_DISTRIBUTOR); + return getsamples_next (TAKE_SAMPLES, wait_finished); +} + /* * TAKE_SAMPLES =arm_pass_noted_position=> * no_more => MOVE_AWAY_FROM_DISTRIBUTOR -- cgit v1.2.3