summaryrefslogtreecommitdiff
path: root/digital/io/src/loader.fsm
blob: 7cc192b31258e5ee0912dcbbeee5a728fbf04f20 (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
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
# loader FSM
loader
  Handle Eurobot 2010 robot clamp, elevator and output door.

States:
 *LOADER_IDLE
  wait start
 LOADER_WAIT_JACK_IN
  wait until the jack is inserted for the first time
 LOADER_WAIT_JACK_OUT
  wait until the jack is removed to initialise robot
 LOADER_INIT_ELEVATOR_ZERO
  move the elevator down to find zero position
 LOADER_INIT_CLAMP_CLOSE
  test clamp closing
 LOADER_INIT_CLAMP_ZERO
  find clamp zero position
 LOADER_INIT_GATE_ZERO
  find gate zero
 LOADER_INIT_ELEVATOR_UP
  move the elevator up so that the robot can go to the wall
 LOADER_INIT_GATE_WAIT[timeout=225]
  wait with gate open
 LOADER_INIT_GATE_CLOSE
  close gate before start
 LOADER_UP
  up position, clamp is protected
 LOADER_DOWN
  down position, ready to take an element
 LOADER_UPING
  moving to the up position
 LOADER_DOWNING
  moving to the down position
 LOADER_ERROR
  error while moving up, stop in an unknown state
 LOADER_ERROR_DOWNING
  error recovery moving down
 LOADER_ERROR_DOWNING_OPEN
  error recovery moving down, open clamp
 LOADER_ERROR_UPING
  error recovery moving up
 LOADER_ERROR_UPING_OPEN
  error recovery moving up, open clamp
 LOADER_LOAD_CLOSING
  close clamp
 LOADER_LOAD_UPING
  move load up to the unload position
 LOADER_LOAD_UNLOADING
  open clamp to unload, wait until unload position
 LOADER_LOAD_UNLOADING_OPEN
  check clamp is open
 LOADER_LOAD_EMPTY_OPEN
  open clamp due to no elements to pick

Events:
 elevator_succeed
  asserv success result
 elevator_failed
  asserv failure result
 elevator_unload_position
  elevator above unloading position
 clamp_succeed
  asserv success result
 clamp_failed
  asserv failure result
 gate_succeed
  asserv success result
 gate_failed
  asserv failure result
 loader_element
  element sensed between clamp to be loaded
 loader_down
  make the loader ready to load an element
 loader_up
  protect the clamp by moving it up
 loader_downed
  posted when loader successfully moved down
 loader_uped
  posted when loader successfully moved up
 loader_errored
  posted when loader in an unknown state due to error while moving up
 loader_black
  loader blocked, due to a black corn

LOADER_IDLE:
 start -> LOADER_WAIT_JACK_IN

LOADER_WAIT_JACK_IN:
 jack_inserted_into_bot -> LOADER_WAIT_JACK_OUT

LOADER_WAIT_JACK_OUT:
 jack_removed_from_bot -> LOADER_INIT_ELEVATOR_ZERO
  find elevator zero
  close clamp
  find gate zero

LOADER_INIT_ELEVATOR_ZERO:
 elevator_succeed -> LOADER_INIT_CLAMP_CLOSE

LOADER_INIT_CLAMP_CLOSE:
 clamp_succeed -> LOADER_INIT_CLAMP_ZERO
  move elevator up
  find clamp zero

LOADER_INIT_CLAMP_ZERO:
 clamp_succeed -> LOADER_INIT_GATE_ZERO

LOADER_INIT_GATE_ZERO:
 gate_succeed -> LOADER_INIT_ELEVATOR_UP

LOADER_INIT_ELEVATOR_UP:
 elevator_succeed -> LOADER_INIT_GATE_WAIT
 elevator_failed -> LOADER_IDLE
  initialisation failure

LOADER_INIT_GATE_WAIT:
 state_timeout -> LOADER_INIT_GATE_CLOSE
  close gate

LOADER_INIT_GATE_CLOSE:
 gate_succeed -> LOADER_UP

LOADER_UP:
 loader_down -> LOADER_DOWNING
  move down

LOADER_DOWN:
 loader_up -> LOADER_UPING
  move up
 loader_element -> LOADER_LOAD_CLOSING
  clamp

LOADER_ERROR:
 loader_down -> LOADER_ERROR_DOWNING
  move down
 loader_up -> LOADER_ERROR_UPING
  move up

LOADER_ERROR_DOWNING:
 elevator_succeed -> LOADER_ERROR_DOWNING_OPEN
  release elevator motor
  open clamp
 elevator_failed -> LOADER_ERROR
  post loader_errored event
 loader_element -> LOADER_ERROR
  emergency stop
  post loader_errored event

LOADER_ERROR_DOWNING_OPEN:
 clamp_succeed -> LOADER_DOWN
  post loader_downed event
 clamp_failed -> LOADER_ERROR
  post loader_errored event

LOADER_ERROR_UPING:
 elevator_succeed -> LOADER_ERROR_UPING_OPEN
  open clamp
 elevator_failed -> LOADER_ERROR
  post loader_errored event

LOADER_ERROR_UPING_OPEN:
 clamp_succeed -> LOADER_UP
  post loader_uped event
 clamp_failed -> LOADER_ERROR
  post loader_errored event

LOADER_DOWNING:
 elevator_succeed -> LOADER_DOWN
  release elevator motor
  post loader_downed event
 elevator_failed -> LOADER_ERROR
  post loader_errored event
 loader_element -> LOADER_ERROR
  emergency stop
  post loader_errored event
 loader_up -> LOADER_UPING
  move up

LOADER_UPING:
 elevator_succeed -> LOADER_UP
  post loader_uped event
 elevator_failed -> LOADER_ERROR
  post loader_errored event
 loader_down -> LOADER_DOWNING
  move down

LOADER_LOAD_CLOSING:
 clamp_succeed: full -> LOADER_LOAD_UPING
  move up
  count one element
 clamp_succeed: empty -> LOADER_LOAD_EMPTY_OPEN
  open clamp

LOADER_LOAD_UPING:
 elevator_unload_position -> LOADER_LOAD_UNLOADING
  open clamp
 elevator_failed -> LOADER_ERROR
  post loader_errored or loader_black event
  open clamp

LOADER_LOAD_UNLOADING:
 elevator_succeed -> LOADER_LOAD_UNLOADING_OPEN
 elevator_failed -> LOADER_ERROR
  post loader_errored event
  open clamp

LOADER_LOAD_UNLOADING_OPEN:
 clamp_succeed: down -> LOADER_DOWNING
  move down
 clamp_succeed: up -> LOADER_UPING
  move up
 clamp_failed -> LOADER_ERROR
  post loader_errored event

LOADER_LOAD_EMPTY_OPEN:
 clamp_succeed: down -> LOADER_DOWN
 clamp_succeed: up -> LOADER_UPING
  move up
 clamp_failed -> LOADER_ERROR
  post loader_errored event