summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src/loader.fsm
blob: 4ffafd262323289b7000c10bda3ad57f13d501bb (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
# 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_ELEVATOR_UP
  move the elevator up so that the robot can go to the wall
 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_LOAD_CLOSING
  close clamp
 LOADER_LOAD_UPING
  move load up to the unload position
 LOADER_LOAD_UNLOADING
  open clamp to unload
 LOADER_LOAD_EMPTY_OPEN
  open clamp due to no elements to pick

Events:
 elevator_succeed
  asserv success result
 elevator_failed
  asserv failure result
 clamp_succeed
  asserv success result, clamp movement cannot fail
 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_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

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_ELEVATOR_UP

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

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_DOWNING
  move down
 loader_up -> LOADER_UPING
  move up

LOADER_DOWNING:
 elevator_succeed -> LOADER_DOWN
  post loader_downed event
 elevator_failed -> LOADER_UPING
  something is blocking, move it up

LOADER_UPING:
 elevator_succeed -> LOADER_UP
  post loader_uped event
 elevator_failed -> LOADER_ERROR
  post loader_errored event

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

LOADER_LOAD_UPING:
 elevator_succeed -> LOADER_LOAD_UNLOADING
  open clamp

LOADER_LOAD_UNLOADING:
 clamp_succeed -> LOADER_DOWNING
  move down

LOADER_LOAD_EMPTY_OPEN:
 clamp_succeed -> LOADER_DOWN