From 3051e493cb014bbcf7f375cf488a34b9b025a11e Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Thu, 21 May 2009 03:25:04 +0200 Subject: * digital/io/src: - rewrite move FSM. --- digital/io/src/move.fsm | 129 +++++++++++++++++------------------------------- 1 file changed, 45 insertions(+), 84 deletions(-) (limited to 'digital/io/src/move.fsm') diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm index f5a56253..50f104e2 100644 --- a/digital/io/src/move.fsm +++ b/digital/io/src/move.fsm @@ -3,104 +3,65 @@ # desired one. It will try to avoid obstacle and manage the case when the # asserv is blocked. move + move with avoid obstacle. States: IDLE - waiting for the beginning of the move FSM - MOVING_TO_FINAL_POSITION - moving to the final position - MOVING_BACKWARD - moving backward to go away from what is blocking the bot - MOVING_TO_INTERMEDIATE_POSITION - moving to an intermediate position to try avoiding the obstacle - WAIT_FOR_CLEAR_PATH - waiting for obstacle to disapear + waiting for the beginning of the move FSM. + MOVING + moving to a position (intermediate or final). + MOVING_BACKWARD_TO_TURN_FREELY + moving backward to go away from what is blocking the bot. + WAIT_FOR_CLEAR_PATH [timeout=255] + waiting for obstacle to disappear. Events: start - initialize the FSM + initialize the FSM and start the movement directly. bot_move_failed - the bot movement failed (blocked by something for example) + the bot movement failed (blocked by something for example). bot_move_succeed - the bot has finished to moved and arrived at the desired position - bot_move_obstacle - the bot has seen something (with the sharps) - wait_finished - we have wait the desired time + the bot has finished to moved and arrived at the desired position. + obstacle_in_front + the bot has seen something (with the sharps) (front is the same when going backward). state_timeout + timeout. IDLE: - start: intermediate_path_found -> MOVING_TO_INTERMEDIATE_POSITION - ask the asserv to go to the intermediate position - start: no_intermediate_path_found -> MOVING_TO_FINAL_POSITION - ask the asserv to go to the final position + start -> MOVING + ask the asserv to go to the computed position. -MOVING_TO_FINAL_POSITION: - bot_move_succeed -> IDLE - post an event for the top FSM to tell we have finished - bot_move_failed -> MOVING_BACKWARD - compute the obstacle position - store current moving direction (for possible failed of path module) - move backward to turn freely - bot_move_obstacle: intermediate_path_found -> MOVING_TO_INTERMEDIATE_POSITION - compute the obstacle position - get next intermediate position from path module - go to next intermediate position - bot_move_obstacle: no_intermediate_path_found -> WAIT_FOR_CLEAR_PATH - compute the obstacle position - get next intermediate position from path module failed - store current moving direction - stop the bot - post an event for the top FSM to be waked up later +MOVING: + bot_move_succeed: we_are_at_final_position -> 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 -> IDLE + post an event for the top FSM to generate a failure. + bot_move_failed -> MOVING_BACKWARD_TO_TURN_FREELY + compute the obstacle position. + move backward to turn freely. + obstacle_in_front -> WAIT_FOR_CLEAR_PATH + stop the bot. -MOVING_BACKWARD: -# TODO -# We ignore obstacle when moving backward - bot_move_succeed: intermediate_path_found -> MOVING_TO_INTERMEDIATE_POSITION - get next intermediate position from path module - bot_move_succeed: no_intermediate_path_found -> WAIT_FOR_CLEAR_PATH - get next intermediate position from path module, failed - stop the bot - post an event for the top FSM to be waked up later -# Do the same as when we succeed - bot_move_failed: intermediate_path_found -> MOVING_TO_INTERMEDIATE_POSITION +MOVING_BACKWARD_TO_TURN_FREELY: + bot_move_succeed: intermediate_path_found -> MOVING + get next intermediate position from path module. + bot_move_succeed: no_intermediate_path_found -> IDLE + post an event for the top FSM to generate a failure. + bot_move_failed: intermediate_path_found -> MOVING get next intermediate position from path module bot_move_failed: no_intermediate_path_found -> WAIT_FOR_CLEAR_PATH - get next intermediate position from path module, failed - stop the bot - post an event for the top FSM to be waked up later - -MOVING_TO_INTERMEDIATE_POSITION: - bot_move_obstacle: intermediate_path_found -> . - compute the obstacle position - get next intermediate position from path module - go to next intermediate position - bot_move_obstacle: no_intermediate_path_found -> WAIT_FOR_CLEAR_PATH - compute the obstacle position - get next intermediate position from path module failed - store current moving direction - stop the bot - post an event for the top FSM to be waked up later - bot_move_succeed: final_position -> IDLE - post an event for the top FSM to tell we have finished - bot_move_succeed: position_intermediary -> . - go to the next intermediate position computed by the path module - bot_move_succeed: no_intermediate_path_found -> WAIT_FOR_CLEAR_PATH - store current moving direction - stop the bot - post an event for the top FSM to be waked up later - bot_move_failed -> MOVING_BACKWARD - store the current position of the obstacle - move backward to turn freely + nothing to do. WAIT_FOR_CLEAR_PATH: - wait_finished: no_obstacle -> MOVING_TO_FINAL_POSITION - ask the asserv to go to the final position - wait_finished: obstacle_and_intermediate_path_found -> MOVING_TO_INTERMEDIATE_POSITION - compute the obstacle position - get next intermediate position from path module - go to next intermediate position - wait_finished: obstacle_and_no_intermediate_path_found -> . - compute the obstacle position - get next intermediate position from path module failed - post an event for the top FSM to be waked up later + state_timeout: no_more_obstacle_or_next_position -> 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 -> . + decrement counter. + state_timeout: obstacle_and_no_intermediate_path_found_and_no_try_again -> IDLE + post an event for the top FSM to generate a failure. -- cgit v1.2.3