summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src/move.fsm
blob: add29b05b4205f2b0210a1882aeff62f9f15e8c6 (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
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
# 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_ROTATING [timeout=1125]
  rotating towards next point.
 MOVE_MOVING [timeout=2250]
  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.
 MOVE_LOADER_UNBLOCKING_UPING
  moving loader up and move backward to unblock loader.
 MOVE_LOADER_UNBLOCKING_DOWNING [timeout=450]
  moving loader down.

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

MOVE_IDLE:
 move_start: path_found_rotate -> MOVE_ROTATING
  rotate towards next position.
 move_start: path_found -> MOVE_MOVING
  move to next position.
 move_start: no_path_found -> MOVE_IDLE
  post failure event.

MOVE_ROTATING:
 bot_move_succeed -> MOVE_MOVING
  move to next position.
 bot_move_failed -> MOVE_MOVING
  move to next position.
 state_timeout -> MOVE_MOVING
  move to next position.
 loader_errored -> MOVE_LOADER_UNBLOCKING_UPING
  move backward
  loader up

MOVE_MOVING:
 bot_move_succeed: done -> MOVE_IDLE
  post success event.
 bot_move_succeed: path_found_rotate -> MOVE_ROTATING
  rotate towards next position.
 bot_move_succeed: path_found -> MOVE_MOVING
  move to next position.
 bot_move_succeed: no_path_found -> MOVE_IDLE
  post failure event.
 bot_move_failed -> MOVE_MOVING_BACKWARD_TO_TURN_FREELY
  reset final_move.
  move backward to turn freely.
 state_timeout -> MOVE_MOVING_BACKWARD_TO_TURN_FREELY
  reset final_move.
  move backward to turn freely.
 obstacle_in_front: tryagain -> MOVE_WAIT_FOR_CLEAR_PATH
  reset final_move.
  stop the bot.
 obstacle_in_front: tryout -> MOVE_IDLE
  stop the bot.
  post failure event.
 loader_errored -> MOVE_LOADER_UNBLOCKING_UPING
  move backward
  loader up

MOVE_MOVING_BACKWARD_TO_TURN_FREELY:
 bot_move_succeed: tryout -> MOVE_IDLE
  post failure event.
 bot_move_succeed: path_found_rotate -> MOVE_ROTATING
  rotate towards next position.
 bot_move_succeed: path_found -> MOVE_MOVING
  move to next position.
 bot_move_succeed: no_path_found -> MOVE_IDLE
  post failure event.
 bot_move_failed: tryout -> MOVE_IDLE
  post failure event.
 bot_move_failed: path_found_rotate -> MOVE_ROTATING
  rotate towards next position.
 bot_move_failed: path_found -> MOVE_MOVING
  move to next position.
 bot_move_failed: no_path_found_tryagain -> MOVE_WAIT_FOR_CLEAR_PATH
  nothing to do.
 bot_move_failed: no_path_found_tryout -> MOVE_IDLE
  post failure event.

MOVE_WAIT_FOR_CLEAR_PATH:
 state_timeout: path_found_rotate -> MOVE_ROTATING
  rotate towards next position.
 state_timeout: path_found -> MOVE_MOVING
  move to next position.
 state_timeout: no_path_found_tryagain -> .
  decrement counter.
 state_timeout: no_path_found_tryout -> MOVE_IDLE
  post failure.

MOVE_LOADER_UNBLOCKING_UPING:
 bot_move_succeed -> MOVE_LOADER_UNBLOCKING_DOWNING
  loader down
  reset unblocking retry counter
 bot_move_failed -> MOVE_LOADER_UNBLOCKING_DOWNING
  loader down
  reset unblocking retry counter

MOVE_LOADER_UNBLOCKING_DOWNING:
 loader_downed: path_found_rotate -> MOVE_ROTATING
  rotate towards next position.
 loader_downed: path_found -> MOVE_MOVING
  move to next position.
 loader_downed: no_path_found -> MOVE_IDLE
  post failure.
 loader_errored: tryagain -> MOVE_LOADER_UNBLOCKING_UPING
  move backward
  loader up
 loader_errored: tryout_path_found_rotate -> MOVE_ROTATING
  rotate towards next position.
 loader_errored: tryout_path_found -> MOVE_ROTATING
  move to next position.
 loader_errored: tryout_no_path_found -> MOVE_IDLE
  post failure.
 state_timeout: tryagain -> MOVE_LOADER_UNBLOCKING_UPING
  move backward
  loader up
 state_timeout: tryout_path_found_rotate -> MOVE_ROTATING
  rotate towards next position.
 state_timeout: tryout_path_found -> MOVE_ROTATING
  move to next position.
 state_timeout: tryout_no_path_found -> MOVE_IDLE
  post failure.