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.fsm | 5 +++++ digital/io/src/getsamples_cb.c | 21 +++++++++++++++++++++ digital/io/src/main.c | 14 ++++++++++++++ digital/io/src/main.h | 8 ++++++++ 4 files changed, 48 insertions(+) diff --git a/digital/io/src/getsamples.fsm b/digital/io/src/getsamples.fsm index 2f495626..0628448b 100644 --- a/digital/io/src/getsamples.fsm +++ b/digital/io/src/getsamples.fsm @@ -39,6 +39,8 @@ Events: the arm has reached the desired position arm_pass_noted_position the arm has just passed the 'noted' position + wait_finished + we have wait the desired time IDLE: start -> FACE_DISTRIBUTOR @@ -70,7 +72,10 @@ TAKE_SAMPLES: MOVE_AWAY_FROM_DISTRIBUTOR: bot_move_succeed -> CLOSE_INPUT_HOLE close input hole + setup a close timeout CLOSE_INPUT_HOLE: arm_move_succeed -> IDLE tell the top FSM we have finished + wait_finished -> IDLE + timed out, give up 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); } diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 0866642b..3c045368 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -87,6 +87,11 @@ uint8_t main_always_stop_for_obstacle = 1; */ uint16_t main_move_wait_cycle; +/** + * Get samples timeout. + */ +uint16_t main_getsamples_wait_cycle; + /** * Sharps stats counters. */ @@ -206,6 +211,9 @@ main_loop (void) /* Update wait flag for move FSM */ if (main_move_wait_cycle) main_move_wait_cycle--; + /* Update wait flag for getsamples FSM */ + if (main_getsamples_wait_cycle) + main_getsamples_wait_cycle--; /* Update sharp module if required and only every * MAIN_SHARP_UPDATE_FREQ cycles */ if (++main_sharp_freq_counter_ == MAIN_SHARP_UPDATE_FREQ) @@ -313,6 +321,12 @@ main_loop (void) { FSM_HANDLE_EVENT (&move_fsm, MOVE_EVENT_wait_finished); } + /* Wait flag for getsamples FSM */ + if (!main_getsamples_wait_cycle) + { + FSM_HANDLE_EVENT (&getsamples_fsm, + GETSAMPLES_EVENT_wait_finished); + } /* TODO: Check other sensors */ } diff --git a/digital/io/src/main.h b/digital/io/src/main.h index 51ab2c9a..1b09a9f8 100644 --- a/digital/io/src/main.h +++ b/digital/io/src/main.h @@ -48,6 +48,14 @@ extern uint16_t main_sharp_ignore_event; */ extern uint16_t main_move_wait_cycle; +/** + * Post an event for the main loop to wake up the getsamples FSM in a certain + * count of cycles. + * This is used by the getsamples FSM to timeout if the arm is completely + * blocked. + */ +extern uint16_t main_getsamples_wait_cycle; + /** * Flag for homologation, to disable the path finding and always stop in front * of an obstacle and wait. -- cgit v1.2.3