summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src/move.fsm
blob: c278c748916846dc678306a3113f827fb3c5b720 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
# Move FSM
# This FSM is responsible to move the bot from the current position to the
# desired one. It will try to avoid obstacle and manage the case when the
# asserv is blocked.
move
  move with avoid obstacle.

States:
 *MOVE_IDLE
  waiting for the beginning of the move FSM.
 MOVE_MOVING
  moving to a position (intermediate or final).
 MOVE_MOVING_BACKWARD_TO_TURN_FREELY
  moving backward to go away from what is blocking the bot.
 MOVE_WAIT_FOR_CLEAR_PATH [timeout=255]
  waiting for obstacle to disappear.

Events:
 move_start
  initialize the FSM and start the movement directly.
 obstacle_in_front
  the bot has seen something (with the sharps) (front is the same when going backward).

MOVE_IDLE:
 move_start -> MOVE_MOVING
  ask the asserv to go to the computed position.

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_failed -> MOVE_MOVING_BACKWARD_TO_TURN_FREELY
  compute the obstacle position.
  move backward to turn freely.
 obstacle_in_front -> MOVE_WAIT_FOR_CLEAR_PATH
  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
  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 -> .
  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.