summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src/move.fsm
blob: fad6b6b7b0c781cac9af6bc4f21d9bebfeaae4ed (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
# 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
  rotating towards next point.
 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.
 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.
 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.
 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: 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: 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.