summaryrefslogtreecommitdiff
path: root/digital/io/src/move.fsm
blob: c08d0454f2e9c42aae1f8724c4951daca2796d00 (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
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
# 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

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

Events:
 start
  initialize the FSM
 bot_move_failed
  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

IDLE:
 start -> MOVING_TO_FINAL_POSITION
  ask the asserv to go to the final 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_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
  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

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