summaryrefslogtreecommitdiff
path: root/digital/io/src/top.fsm
blob: a105e87dc892d52d04decff228eceb1b42634709 (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
# 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_BACK_BIN
  move back to unload bin
 UNLOAD_UNLOAD[timeout=225]
  unloading, wait
 COLLECT
  collecting elements

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

UNLOAD_FACE_BIN:
 bot_move_succeed -> UNLOAD_BACK_BIN
  go backward to 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_failed: unload -> UNLOAD
 move_fsm_failed: collect -> COLLECT