summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src/init.fsm
blob: 89ec26e63b730c4ab74c7cc4294c795fb8e1ffc9 (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
# init FSM
# This FSM is used to place the bot at a "start position" for a match. It is
# more precise than a human hand and reset the asserv board to the start
# position.
# Some remarks:
#  - for security reason, we need to insert the jack then removed it, then
#  insert it back. Why? Because it can already be inserted.
#  - other FSM can do a similar thing by using the first jack out to
#  initialize.
init
  place the bot in the start position for a match.

States:
 IDLE
  waiting for the beginning of the top FSM.
 WAIT_FIRST_JACK_IN
  wait for the jack to be inserted into the bot for the first time.
 WAIT_FIRST_JACK_OUT
  wait the jack to be removed from the bot for the first time.
 WAIT_SECOND_JACK_IN
  wait for the jack to be inserted into the bot for the second time.
 WAIT_FOR_HANDS_OUT[timeout=450]
  just wait for operator hands to be removed from the jack.
 GO_TO_THE_WALL
  go to the wall for the first time.
 SET_Y_POSITION
  reset the Y position of the bot.
 SET_ANGULAR_POSITION
  reset the angular position of the bot.
 GO_AWAY_FROM_THE_WALL
  go backward from the wall for a specific distance.
 FACE_OTHER_WALL
  make the bot face the other wall.
 WAIT_AFTER_ROTATION [timeout=100]
  make sure rotation is really finished before continuing.
 GO_TO_THE_WALL_AGAIN
  go to the wall for the second time.
 SET_X_POSITION
  reset the X position of the bot.
 GO_AWAY_FROM_THE_WALL_AGAIN
  go backward from the wall for a specific distance.
 GO_TO_START_POSITION
  go to the start position.
 WAIT_SECOND_JACK_OUT
  wait the jack to be removed from the bot for the second time.

Events:
 start
  initialize the FSM.
 jack_inserted_into_bot
  the jack is inserted into the bot.
 jack_removed_from_bot
  the jack is removed from the bot.
 bot_move_succeed
  the move is finished.
 state_timeout
  timeout.
 asserv_last_cmd_ack
  last command sent to the asserv board has been acquitted.

IDLE:
 start -> WAIT_FIRST_JACK_IN
  do nothing.

WAIT_FIRST_JACK_IN:
 jack_inserted_into_bot -> WAIT_FIRST_JACK_OUT
  do nothing.

WAIT_FIRST_JACK_OUT:
 jack_removed_from_bot -> WAIT_SECOND_JACK_IN
  start trace module.
  get and store the color of the bot.

WAIT_SECOND_JACK_IN:
 jack_inserted_into_bot -> WAIT_FOR_HANDS_OUT
  do nothing.

WAIT_FOR_HANDS_OUT:
 state_timeout -> GO_TO_THE_WALL
  fuck the wall in front.

GO_TO_THE_WALL:
 bot_move_succeed -> SET_Y_POSITION
  reset the Y position of the bot.

SET_Y_POSITION:
 asserv_last_cmd_ack -> SET_ANGULAR_POSITION
  reset the angular position of the bot.

SET_ANGULAR_POSITION:
 asserv_last_cmd_ack -> GO_AWAY_FROM_THE_WALL
  move away from the wall (linear move).

GO_AWAY_FROM_THE_WALL:
 bot_move_succeed -> FACE_OTHER_WALL
  turn to face the other wall.

FACE_OTHER_WALL:
 bot_move_succeed -> WAIT_AFTER_ROTATION
  nothing to do.

WAIT_AFTER_ROTATION:
 state_timeout -> GO_TO_THE_WALL_AGAIN
  fuck the wall in front.

GO_TO_THE_WALL_AGAIN:
 bot_move_succeed -> SET_X_POSITION
  reset the X position of the bot.

SET_X_POSITION:
 asserv_last_cmd_ack -> GO_AWAY_FROM_THE_WALL_AGAIN
  move away from the wall (linear move).

GO_AWAY_FROM_THE_WALL_AGAIN:
 bot_move_succeed -> GO_TO_START_POSITION
  go to the start position with a go to movement.

GO_TO_START_POSITION:
 bot_move_succeed -> WAIT_SECOND_JACK_OUT
  nothing to do, the bot is at the start position.

WAIT_SECOND_JACK_OUT:
 jack_removed_from_bot -> IDLE
  tell other FSM the match begins.
  start the chrono.