summaryrefslogtreecommitdiff
path: root/digital/io/src/top.fsm
blob: 4bebcdaa1a07fb60f7d4fa0b1cdf556c0523a710 (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
# Top FSM.
top
  Main FSM calling other FSM.

States:
 *IDLE
  waiting for the beginning of the top FSM.
 WAIT_INIT_TO_FINISH
  waiting for the end of init.
 FIRST_GO_BEGIN_OF_LINE_FAST
  first sequence, go to start of diagonal line
 FIRST_GO_END_OF_LINE_FAST
  first sequence, go to end of line
 FIRST_GO_END_OF_LINE_SLOW
  slow down to take elements
 UNLOAD
  go to unloading position
 UNLOAD_LOADER_UP
  move loader to up position
 UNLOAD_FACE_BIN
  orient robot to unload angle
 UNLOAD_FACE_BIN_UNBLOCK
  move backward to unblock
 UNLOAD_BACK_BIN
  move back to unload bin
 UNLOAD_UNLOAD[timeout=225]
  unloading, wait
 COLLECT
  collecting elements
 COLLECT_SLOW_MOTION
  slowly move toward element to collect
 COLLECT_BLACK
  moving backward after trying to collect a black corn

Events:
 start
  initialize the FSM.
 move_fsm_succeed
  when the move FSM returns with success.
 move_fsm_failed
  when the move FSM returns with an error.
 bot_move_succeed
  when the bot has succeed its asserv movement.
 bot_move_failed
  when the bot has failed its asserv movement.
 init_match_is_started
  when the init FSM has finished and the match start.
 in_field
  the robot is in the zone containing interesting elements.
 state_timeout
  timeout in cycle count.
 asserv_last_cmd_ack
  last command sent to the asserv board has been acquitted.

import init.fsm
import loader.fsm
import move.fsm

IDLE:
 start -> WAIT_INIT_TO_FINISH
  nothing to do.

WAIT_INIT_TO_FINISH:
 init_match_is_started -> FIRST_GO_BEGIN_OF_LINE_FAST
  the match start
  set fast speed
  go to first sequence begin of line

FIRST_GO_BEGIN_OF_LINE_FAST:
 move_fsm_succeed -> FIRST_GO_END_OF_LINE_FAST
  go to end of line
  loader down
 move_fsm_failed -> .
  set slow speed
  retry

FIRST_GO_END_OF_LINE_FAST:
 in_field -> FIRST_GO_END_OF_LINE_SLOW
  set slow speed
 move_fsm_succeed -> UNLOAD_LOADER_UP
  set slow speed
  move loader up
 move_fsm_failed -> FIRST_GO_END_OF_LINE_SLOW
  set slow speed
  retry

FIRST_GO_END_OF_LINE_SLOW:
 move_fsm_succeed -> UNLOAD_LOADER_UP
  move loader up
 move_fsm_failed -> FIRST_GO_END_OF_LINE_SLOW
  retry

UNLOAD:
 move_fsm_succeed -> UNLOAD_LOADER_UP
  move loader up
 move_fsm_failed -> .
  retry

UNLOAD_LOADER_UP:
 loader_uped -> UNLOAD_FACE_BIN
  turn toward bin
 loader_errored -> UNLOAD_FACE_BIN
  turn toward bin

UNLOAD_FACE_BIN:
 bot_move_succeed -> UNLOAD_BACK_BIN
  go backward to bin
 bot_move_failed -> UNLOAD_FACE_BIN_UNBLOCK
  move backward

UNLOAD_FACE_BIN_UNBLOCK:
 bot_move_succeed -> UNLOAD_FACE_BIN
  turn toward bin
 bot_move_failed -> UNLOAD_FACE_BIN
  turn toward bin

UNLOAD_BACK_BIN:
 bot_move_succeed -> UNLOAD_UNLOAD
  unload
 bot_move_failed -> UNLOAD_UNLOAD
  unload

UNLOAD_UNLOAD:
 state_timeout -> COLLECT
  close gate
  loader down
  choose best food to collect

COLLECT:
 move_fsm_succeed: unload -> UNLOAD
 move_fsm_succeed: collect -> COLLECT
 move_fsm_succeed: slow_motion -> COLLECT_SLOW_MOTION
  slow down
  move to food
 move_fsm_failed: unload -> UNLOAD
 move_fsm_failed: collect -> COLLECT

COLLECT_SLOW_MOTION:
 bot_move_succeed: unload -> UNLOAD
 bot_move_succeed: collect -> COLLECT
  speed up
  collect
 bot_move_failed: unload -> UNLOAD
 bot_move_failed: collect -> COLLECT
  same as above
 loader_black -> COLLECT_BLACK
  speed up
  move backward
  mark as black

COLLECT_BLACK:
 bot_move_succeed: unload -> UNLOAD
 bot_move_succeed: collect -> COLLECT
 bot_move_failed: unload -> UNLOAD
 bot_move_failed: collect -> COLLECT