From 7a66e70e4944d47ecb699e2979dca601d1faca38 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 29 May 2010 13:59:35 +0200 Subject: digital/io/src: add tryout on blocking detection --- digital/io/src/ai_move_cb.c | 56 ++++++++++++++++++++++++++++++--------------- digital/io/src/move.fsm | 4 ++++ 2 files changed, 42 insertions(+), 18 deletions(-) (limited to 'digital/io/src') diff --git a/digital/io/src/ai_move_cb.c b/digital/io/src/ai_move_cb.c index 69a46a3c..32cdd221 100644 --- a/digital/io/src/ai_move_cb.c +++ b/digital/io/src/ai_move_cb.c @@ -394,6 +394,8 @@ ai__MOVE_MOVING__loader_errored (void) /* * MOVE_MOVING_BACKWARD_TO_TURN_FREELY =bot_move_succeed=> + * tryout => MOVE_IDLE + * post failure event. * path_found_rotate => MOVE_ROTATING * rotate towards next position. * path_found => MOVE_MOVING @@ -404,23 +406,33 @@ ai__MOVE_MOVING__loader_errored (void) fsm_branch_t ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_succeed (void) { - uint8_t next = move_path_init (); - if (next) + if (--move_data.try_again_counter == 0) { - if (next == 2) - return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, path_found_rotate); - else - return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, path_found); + main_post_event (AI_EVENT_move_fsm_failed); + return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, tryout); } else { - main_post_event (AI_EVENT_move_fsm_failed); - return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, no_path_found); + uint8_t next = move_path_init (); + if (next) + { + if (next == 2) + return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, path_found_rotate); + else + return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, path_found); + } + else + { + main_post_event (AI_EVENT_move_fsm_failed); + return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, no_path_found); + } } } /* * MOVE_MOVING_BACKWARD_TO_TURN_FREELY =bot_move_failed=> + * tryout => MOVE_IDLE + * post failure event. * path_found_rotate => MOVE_ROTATING * rotate towards next position. * path_found => MOVE_MOVING @@ -433,23 +445,31 @@ ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_succeed (void) fsm_branch_t ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_failed (void) { - uint8_t next = move_path_init (); - if (next) + if (--move_data.try_again_counter == 0) { - if (next == 2) - return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, path_found_rotate); - else - return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, path_found); + main_post_event (AI_EVENT_move_fsm_failed); + return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, tryout); } else { - if (--move_data.try_again_counter == 0) + uint8_t next = move_path_init (); + if (next) { - main_post_event (AI_EVENT_move_fsm_failed); - return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_path_found_tryout); + if (next == 2) + return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, path_found_rotate); + else + return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, path_found); } else - return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_path_found_tryagain); + { + if (--move_data.try_again_counter == 0) + { + main_post_event (AI_EVENT_move_fsm_failed); + return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_path_found_tryout); + } + else + return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_path_found_tryagain); + } } } diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm index 05f0c9d0..add29b05 100644 --- a/digital/io/src/move.fsm +++ b/digital/io/src/move.fsm @@ -72,12 +72,16 @@ MOVE_MOVING: loader up MOVE_MOVING_BACKWARD_TO_TURN_FREELY: + bot_move_succeed: tryout -> MOVE_IDLE + post failure event. bot_move_succeed: path_found_rotate -> MOVE_ROTATING rotate towards next position. bot_move_succeed: path_found -> MOVE_MOVING move to next position. bot_move_succeed: no_path_found -> MOVE_IDLE post failure event. + bot_move_failed: tryout -> MOVE_IDLE + post failure event. bot_move_failed: path_found_rotate -> MOVE_ROTATING rotate towards next position. bot_move_failed: path_found -> MOVE_MOVING -- cgit v1.2.3