From ed51ec3c5eb23aecb3a93b02814acf655ff62a0c Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Thu, 1 May 2008 01:08:56 +0200 Subject: * digital/io/src - enhance move FSM with support to stop when there is no valid path found to avoid the obstacle ; - add a flag to be notified from the main loop after a certain amount of time. --- digital/io/src/move.fsm | 66 ++++++++++++++++++++++++++++++++++++------------- 1 file changed, 49 insertions(+), 17 deletions(-) (limited to 'digital/io/src/move.fsm') diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm index ab55d1d4..317f25d8 100644 --- a/digital/io/src/move.fsm +++ b/digital/io/src/move.fsm @@ -13,6 +13,8 @@ States: 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 Events: start @@ -23,6 +25,8 @@ Events: 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 IDLE: start -> MOVING_TO_FINAL_POSITION @@ -32,36 +36,64 @@ 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 - store the current position of the obstacle + compute the obstacle position + store current moving direction (for possible failed of path module) move backward to turn freely - bot_move_obstacle -> MOVING_TO_INTERMEDIATE_POSITION + 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 - give the obstacle position to the path module - give the current position of the bot to the path module - get the intermediate position from path module + 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_BACKWARD: # TODO # We ignore obstacle when moving backward - bot_move_succeed -> MOVING_TO_INTERMEDIATE_POSITION - give the obstacle position to the path module - give the current position of the bot to the path module - get the intermediate position from path module - bot_move_failed -> MOVING_TO_INTERMEDIATE_POSITION - do the same as when we succeed - give the obstacle position to the path module - give the current position of the bot to the path module - get the intermediate position from path module + 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 + 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 -> . + bot_move_obstacle: intermediate_path_found -> . compute the obstacle position - give the obstacle position to the path module - go to the next intermediate position computed by the path module + 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 + +WAIT_FOR_CLEAR_PATH: + wait_finished: obstacle -> . + check for obstacle using stored moving direction + post an event for the top FSM to be waked up later + wait_finished: no_obstacle -> MOVING_TO_FINAL_POSITION + check for obstacle using stored moving direction + try to go the final position -- cgit v1.2.3