From 3ee60de086963a418bb2104da3f9dded851a6b50 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 14 May 2010 16:25:13 +0200 Subject: digital/io/src: give up if path obstructed several times --- digital/io/src/ai_move_cb.c | 39 +++++++++++++++++++++++++++------------ digital/io/src/move.c | 4 ++++ digital/io/src/move.fsm | 13 +++++++++---- 3 files changed, 40 insertions(+), 16 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 e250d010..0c54de7c 100644 --- a/digital/io/src/ai_move_cb.c +++ b/digital/io/src/ai_move_cb.c @@ -146,8 +146,6 @@ move_go_to_next (vect_t dst) move_data.backward_movement_allowed); } TRACE (TRACE_MOVE__GO_TO, dst.x, dst.y); - /* Reset try counter. */ - move_data.try_again_counter = 3; /* Next time, do not use slow. */ move_data.slow = 0; return r; @@ -322,16 +320,25 @@ ai__MOVE_MOVING__bot_move_failed (void) /* * MOVE_MOVING =obstacle_in_front=> - * => MOVE_WAIT_FOR_CLEAR_PATH + * tryagain => MOVE_WAIT_FOR_CLEAR_PATH * reset final_move. * stop the bot. + * tryout => MOVE_IDLE + * stop the bot. + * post failure event. */ fsm_branch_t ai__MOVE_MOVING__obstacle_in_front (void) { move_data.final_move = 0; asserv_stop_motor (); - return ai_next (MOVE_MOVING, obstacle_in_front); + if (--move_data.try_again_counter == 0) + { + main_post_event (AI_EVENT_move_fsm_failed); + return ai_next_branch (MOVE_MOVING, obstacle_in_front, tryout); + } + else + return ai_next_branch (MOVE_MOVING, obstacle_in_front, tryagain); } /* @@ -381,8 +388,10 @@ ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_succeed (void) * rotate towards next position. * path_found => MOVE_MOVING * move to next position. - * no_path_found => MOVE_WAIT_FOR_CLEAR_PATH + * no_path_found_tryagain => MOVE_WAIT_FOR_CLEAR_PATH * nothing to do. + * no_path_found_tryout => MOVE_IDLE + * post failure event. */ fsm_branch_t ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_failed (void) @@ -396,7 +405,15 @@ ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_failed (void) 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); + { + 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); + } } /* @@ -405,9 +422,9 @@ ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_failed (void) * rotate towards next position. * path_found => MOVE_MOVING * move to next position. - * no_path_found_and_try_again => MOVE_WAIT_FOR_CLEAR_PATH + * no_path_found_tryagain => MOVE_WAIT_FOR_CLEAR_PATH * decrement counter. - * no_path_found_and_no_try_again => MOVE_IDLE + * no_path_found_tryout => MOVE_IDLE * post failure. */ fsm_branch_t @@ -428,12 +445,10 @@ ai__MOVE_WAIT_FOR_CLEAR_PATH__state_timeout (void) if (--move_data.try_again_counter == 0) { main_post_event (AI_EVENT_move_fsm_failed); - return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, - no_path_found_and_no_try_again); + return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, no_path_found_tryout); } else - return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, - no_path_found_and_try_again); + return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, no_path_found_tryagain); } } diff --git a/digital/io/src/move.c b/digital/io/src/move.c index 27fc93fb..e4f59c39 100644 --- a/digital/io/src/move.c +++ b/digital/io/src/move.c @@ -47,6 +47,8 @@ move_start (position_t position, uint8_t backward) move_data.backward_movement_allowed = backward; move_data.final_move = 0; move_data.shorten = 0; + /* Reset try counter. */ + move_data.try_again_counter = 3; /* Start the FSM. */ fsm_handle_event (&ai_fsm, AI_EVENT_move_start); } @@ -60,6 +62,8 @@ move_start_noangle (vect_t position, uint8_t backward, int16_t shorten) move_data.backward_movement_allowed = backward; move_data.final_move = 0; move_data.shorten = shorten; + /* Reset try counter. */ + move_data.try_again_counter = 3; /* Start the FSM. */ fsm_handle_event (&ai_fsm, AI_EVENT_move_start); } diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm index 2812f686..abedd8dd 100644 --- a/digital/io/src/move.fsm +++ b/digital/io/src/move.fsm @@ -54,9 +54,12 @@ MOVE_MOVING: bot_move_failed -> MOVE_MOVING_BACKWARD_TO_TURN_FREELY reset final_move. move backward to turn freely. - obstacle_in_front -> MOVE_WAIT_FOR_CLEAR_PATH + obstacle_in_front: tryagain -> MOVE_WAIT_FOR_CLEAR_PATH reset final_move. stop the bot. + obstacle_in_front: tryout -> MOVE_IDLE + stop the bot. + post failure event. loader_errored -> MOVE_LOADER_UNBLOCKING_UPING move backward loader up @@ -72,17 +75,19 @@ MOVE_MOVING_BACKWARD_TO_TURN_FREELY: rotate towards next position. bot_move_failed: path_found -> MOVE_MOVING move to next position. - bot_move_failed: no_path_found -> MOVE_WAIT_FOR_CLEAR_PATH + bot_move_failed: no_path_found_tryagain -> MOVE_WAIT_FOR_CLEAR_PATH nothing to do. + bot_move_failed: no_path_found_tryout -> MOVE_IDLE + post failure event. MOVE_WAIT_FOR_CLEAR_PATH: state_timeout: path_found_rotate -> MOVE_ROTATING rotate towards next position. state_timeout: path_found -> MOVE_MOVING move to next position. - state_timeout: no_path_found_and_try_again -> . + state_timeout: no_path_found_tryagain -> . decrement counter. - state_timeout: no_path_found_and_no_try_again -> MOVE_IDLE + state_timeout: no_path_found_tryout -> MOVE_IDLE post failure. MOVE_LOADER_UNBLOCKING_UPING: -- cgit v1.2.3