From 5cd1edab4210108f0753202deea45fd40b44efa5 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 13 Apr 2010 00:22:32 +0200 Subject: digital/io/src: update move FSM Update branch names. Fix FSM lock up when no path found on FSM state. Fix FSM lock up when obstacle found and was going to final destination. --- digital/io/src/ai_move_cb.c | 113 +++++++++++++++++++------------------------- digital/io/src/move.fsm | 49 ++++++++++--------- 2 files changed, 72 insertions(+), 90 deletions(-) diff --git a/digital/io/src/ai_move_cb.c b/digital/io/src/ai_move_cb.c index b52d8279..de68fef0 100644 --- a/digital/io/src/ai_move_cb.c +++ b/digital/io/src/ai_move_cb.c @@ -82,15 +82,11 @@ move_obstacle_in_table (vect_t pos) * @return * - 0 if no path could be found ; * - 1 if a path has been found. - * - 2 already at final place. */ uint8_t move_get_next_position (void) { vect_t dst; - /* Are we at the final position. */ - if (move_data.final_move) - return 2; /* Get the current position */ position_t current_pos; asserv_get_position (¤t_pos); @@ -193,60 +189,61 @@ move_obstacle_here (void) /* * MOVE_IDLE =move_start=> - * => MOVE_MOVING - * ask the asserv to go to the computed position. + * path_found => MOVE_MOVING + * move to next position. + * no_path_found => MOVE_IDLE + * post failure event. */ fsm_branch_t ai__MOVE_IDLE__move_start (void) { - /* ask the asserv to go to the computed position. */ - move_get_next_position (); - return ai_next (MOVE_IDLE, move_start); + if (move_get_next_position ()) + return ai_next_branch (MOVE_IDLE, move_start, path_found); + else + { + main_post_event_for_top_fsm = AI_EVENT_move_fsm_failed; + return ai_next_branch (MOVE_IDLE, move_start, no_path_found); + } } /* * MOVE_MOVING =bot_move_succeed=> - * we_are_at_final_position => MOVE_IDLE - * post an event for the top FSM to tell it we have finished. - * position_intermediary => MOVE_MOVING - * get next position computed by the path module. - * if next position is the final, use a goto_xya. - * otherwise go to the next intermediate position with goto. - * no_intermediate_path_found => MOVE_IDLE - * post an event for the top FSM to generate a failure. + * done => MOVE_IDLE + * post success event. + * path_found => MOVE_MOVING + * move to next position. + * no_path_found => MOVE_IDLE + * post failure event. */ fsm_branch_t ai__MOVE_MOVING__bot_move_succeed (void) { - uint8_t ret = move_get_next_position (); - if (ret == 2) + if (move_data.final_move) { - /* Post an event for the top FSM to tell it we have finished. */ main_post_event_for_top_fsm = AI_EVENT_move_fsm_succeed; - return ai_next_branch (MOVE_MOVING, bot_move_succeed, we_are_at_final_position); + return ai_next_branch (MOVE_MOVING, bot_move_succeed, done); } - else if (ret == 1) + else if (move_get_next_position ()) { - /* Nothing to do. */ - return ai_next_branch (MOVE_MOVING, bot_move_succeed, position_intermediary); + return ai_next_branch (MOVE_MOVING, bot_move_succeed, path_found); } else { - /* Post an event for the top FSM to generate a failure. */ main_post_event_for_top_fsm = AI_EVENT_move_fsm_failed; - return ai_next_branch (MOVE_MOVING, bot_move_succeed, no_intermediate_path_found); + return ai_next_branch (MOVE_MOVING, bot_move_succeed, no_path_found); } } /* * MOVE_MOVING =bot_move_failed=> * => MOVE_MOVING_BACKWARD_TO_TURN_FREELY - * compute the obstacle position. + * reset final_move. * move backward to turn freely. */ fsm_branch_t ai__MOVE_MOVING__bot_move_failed (void) { + move_data.final_move = 0; /* Compute the obstacle position. */ move_obstacle_here (); /* Move backward to turn freely. */ @@ -258,97 +255,83 @@ ai__MOVE_MOVING__bot_move_failed (void) /* * MOVE_MOVING =obstacle_in_front=> * => MOVE_WAIT_FOR_CLEAR_PATH + * reset final_move. * stop the bot. */ fsm_branch_t ai__MOVE_MOVING__obstacle_in_front (void) { - /* Stop the bot. */ + move_data.final_move = 0; asserv_stop_motor (); return ai_next (MOVE_MOVING, obstacle_in_front); } /* * MOVE_MOVING_BACKWARD_TO_TURN_FREELY =bot_move_succeed=> - * intermediate_path_found => MOVE_MOVING - * get next intermediate position from path module. - * no_intermediate_path_found => MOVE_IDLE - * post an event for the top FSM to generate a failure. + * path_found => MOVE_MOVING + * move to next position. + * no_path_found => MOVE_IDLE + * post failure event. */ fsm_branch_t ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_succeed (void) { - uint8_t ret = move_get_next_position (); - if (ret == 1) + if (move_get_next_position ()) { - /* Nothing to do. */ - return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, intermediate_path_found); + return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, path_found); } else { - /* Post an event for the top FSM to generate a failure. */ main_post_event_for_top_fsm = AI_EVENT_move_fsm_failed; - return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, no_intermediate_path_found); + 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=> - * intermediate_path_found => MOVE_MOVING - * get next intermediate position from path module - * no_intermediate_path_found => MOVE_WAIT_FOR_CLEAR_PATH + * path_found => MOVE_MOVING + * move to next position. + * no_path_found => MOVE_WAIT_FOR_CLEAR_PATH * nothing to do. */ fsm_branch_t ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_failed (void) { - uint8_t ret = move_get_next_position (); - if (ret == 1) - { - /* Nothing to do. */ - return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, intermediate_path_found); - } + if (move_get_next_position ()) + return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, path_found); else - { - /* Nothing to do. */ - return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_intermediate_path_found); - } + return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_path_found); } /* * MOVE_WAIT_FOR_CLEAR_PATH =state_timeout=> - * no_more_obstacle_or_next_position => MOVE_MOVING - * get next position computed by the path module. - * if next position is the final, use a goto_xya. - * otherwise go to the next intermediate position with goto. - * obstacle_and_no_intermediate_path_found_and_try_again => MOVE_WAIT_FOR_CLEAR_PATH + * path_found => MOVE_MOVING + * move to next position. + * no_path_found_and_try_again => MOVE_WAIT_FOR_CLEAR_PATH * decrement counter. - * obstacle_and_no_intermediate_path_found_and_no_try_again => MOVE_IDLE - * post an event for the top FSM to generate a failure. + * no_path_found_and_no_try_again => MOVE_IDLE + * post failure. */ fsm_branch_t ai__MOVE_WAIT_FOR_CLEAR_PATH__state_timeout (void) { - uint8_t ret = move_get_next_position (); - if (ret == 1) + if (move_get_next_position ()) { - /* Go to position. */ return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, - no_more_obstacle_or_next_position); + path_found); } else { /* Error, no new position, should we try again? */ if (--move_data.try_again_counter == 0) { - /* Post an event for the top FSM to generate a failure. */ main_post_event_for_top_fsm = AI_EVENT_move_fsm_failed; return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, - obstacle_and_no_intermediate_path_found_and_no_try_again); + no_path_found_and_no_try_again); } else return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, - obstacle_and_no_intermediate_path_found_and_try_again); + no_path_found_and_try_again); } } diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm index c1bfa9e9..d96cb349 100644 --- a/digital/io/src/move.fsm +++ b/digital/io/src/move.fsm @@ -22,40 +22,39 @@ Events: the bot has seen something (front is the same when going backward). MOVE_IDLE: - move_start -> MOVE_MOVING - ask the asserv to go to the computed position. + move_start: path_found -> MOVE_MOVING + move to next position. + move_start: no_path_found -> MOVE_IDLE + post failure event. MOVE_MOVING: - bot_move_succeed: we_are_at_final_position -> MOVE_IDLE - post an event for the top FSM to tell it we have finished. - bot_move_succeed: position_intermediary -> . - get next position computed by the path module. - if next position is the final, use a goto_xya. - otherwise go to the next intermediate position with goto. - bot_move_succeed: no_intermediate_path_found -> MOVE_IDLE - post an event for the top FSM to generate a failure. + bot_move_succeed: done -> MOVE_IDLE + post success event. + bot_move_succeed: path_found -> . + move to next position. + bot_move_succeed: no_path_found -> MOVE_IDLE + post failure event. bot_move_failed -> MOVE_MOVING_BACKWARD_TO_TURN_FREELY - compute the obstacle position. + reset final_move. move backward to turn freely. obstacle_in_front -> MOVE_WAIT_FOR_CLEAR_PATH + reset final_move. stop the bot. MOVE_MOVING_BACKWARD_TO_TURN_FREELY: - bot_move_succeed: intermediate_path_found -> MOVE_MOVING - get next intermediate position from path module. - bot_move_succeed: no_intermediate_path_found -> MOVE_IDLE - post an event for the top FSM to generate a failure. - bot_move_failed: intermediate_path_found -> MOVE_MOVING - get next intermediate position from path module - bot_move_failed: no_intermediate_path_found -> MOVE_WAIT_FOR_CLEAR_PATH + 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: path_found -> MOVE_MOVING + move to next position. + bot_move_failed: no_path_found -> MOVE_WAIT_FOR_CLEAR_PATH nothing to do. MOVE_WAIT_FOR_CLEAR_PATH: - state_timeout: no_more_obstacle_or_next_position -> MOVE_MOVING - get next position computed by the path module. - if next position is the final, use a goto_xya. - otherwise go to the next intermediate position with goto. - state_timeout: obstacle_and_no_intermediate_path_found_and_try_again -> . + state_timeout: path_found -> MOVE_MOVING + move to next position. + state_timeout: no_path_found_and_try_again -> . decrement counter. - state_timeout: obstacle_and_no_intermediate_path_found_and_no_try_again -> MOVE_IDLE - post an event for the top FSM to generate a failure. + state_timeout: no_path_found_and_no_try_again -> MOVE_IDLE + post failure. -- cgit v1.2.3