From 6980d8540e567a0955dc17d84c9d2e2166550a2b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 2 May 2008 07:14:41 +0200 Subject: * digital/io/src: - added arm close timeout. --- digital/io/src/getsamples_cb.c | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) (limited to 'digital/io/src/getsamples_cb.c') diff --git a/digital/io/src/getsamples_cb.c b/digital/io/src/getsamples_cb.c index 3099452f..49eda8d1 100644 --- a/digital/io/src/getsamples_cb.c +++ b/digital/io/src/getsamples_cb.c @@ -41,6 +41,11 @@ */ #define GET_SAMPLES_MOVE_BACKWARD_DISTANCE 7 +/** + * Arm time out. + */ +#define GET_SAMPLES_ARM_TIMEOUT (12 * 225 + 225 / 2) + /** * 'Private' get samples data used internaly by the FSM. */ @@ -129,6 +134,19 @@ getsamples__OPEN_INPUT_HOLE__arm_move_succeed (void) return getsamples_next (OPEN_INPUT_HOLE, arm_move_succeed); } +/* + * CLOSE_INPUT_HOLE =wait_finished=> + * => IDLE + * timed out, give up + */ +fsm_branch_t +getsamples__CLOSE_INPUT_HOLE__wait_finished (void) +{ + /* Give up, tell the top FSM we have finished */ + main_post_event_for_top_fsm = TOP_EVENT_get_samples_fsm_finished; + return getsamples_next (CLOSE_INPUT_HOLE, wait_finished); +} + /* * CLOSE_INPUT_HOLE =arm_move_succeed=> * => IDLE @@ -189,12 +207,15 @@ getsamples__IDLE__start (void) * MOVE_AWAY_FROM_DISTRIBUTOR =bot_move_succeed=> * => CLOSE_INPUT_HOLE * close input hole + * setup a close timeout */ fsm_branch_t getsamples__MOVE_AWAY_FROM_DISTRIBUTOR__bot_move_succeed (void) { /* Move the arm to close the input hole */ asserv_move_arm (BOT_ARM_MIN_TO_OPEN + 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_AWAY_FROM_DISTRIBUTOR, bot_move_succeed); } -- cgit v1.2.3