summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src/move.fsm
diff options
context:
space:
mode:
authorJérémy Dufour2009-05-21 03:25:04 +0200
committerJérémy Dufour2009-05-21 03:25:04 +0200
commit3051e493cb014bbcf7f375cf488a34b9b025a11e (patch)
treefd9164742c802249f6540b9bcfe87ba2475602f4 /digital/io/src/move.fsm
parentd23d5d67bc7d78b318e81f6cd5703823a08dbbc0 (diff)
* digital/io/src:
- rewrite move FSM.
Diffstat (limited to 'digital/io/src/move.fsm')
-rw-r--r--digital/io/src/move.fsm129
1 files changed, 45 insertions, 84 deletions
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.