summaryrefslogtreecommitdiff
path: root/digital/io/src/move.fsm
blob: 2812f686850c1e6394805bc82559d63d297e07f5 (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
# Move FSM
# This FSM is responsible to move the bot from the current position to the
# desired one. It will try to avoid obstacle and manage the case when the
# asserv is blocked.
move
  move with avoid obstacle.

States:
 *MOVE_IDLE
  waiting for the beginning of the move FSM.
 MOVE_ROTATING
  rotating towards next point.
 MOVE_MOVING
  moving to a position (intermediate or final).
 MOVE_MOVING_BACKWARD_TO_TURN_FREELY
  moving backward to go away from what is blocking the bot.
 MOVE_WAIT_FOR_CLEAR_PATH [timeout=255]
  waiting for obstacle to disappear.
 MOVE_LOADER_UNBLOCKING_UPING
  moving loader up and move backward to unblock loader.
 MOVE_LOADER_UNBLOCKING_DOWNING
  moving loader down.

Events:
 move_start
  initialize the FSM and start the movement directly.
 obstacle_in_front
  the bot has seen something (front is the same when going backward).

MOVE_IDLE:
 move_start: path_found_rotate -> MOVE_ROTATING
  rotate towards next position.
 move_start: path_found -> MOVE_MOVING
  move to next position.
 move_start: no_path_found -> MOVE_IDLE
  post failure event.

MOVE_ROTATING:
 bot_move_succeed -> MOVE_MOVING
  move to next position.
 loader_errored -> MOVE_LOADER_UNBLOCKING_UPING
  move backward
  loader up

MOVE_MOVING:
 bot_move_succeed: done -> MOVE_IDLE
  post success event.
 bot_move_succeed: path_found_rotate -> MOVE_ROTATING
  rotate towards next position.
 bot_move_succeed: path_found -> MOVE_MOVING
  move to next position.
 bot_move_succeed: no_path_found -> MOVE_IDLE
  post failure event.
 bot_move_failed -> MOVE_MOVING_BACKWARD_TO_TURN_FREELY
  reset final_move.
  move backward to turn freely.
 obstacle_in_front -> MOVE_WAIT_FOR_CLEAR_PATH
  reset final_move.
  stop the bot.
 loader_errored -> MOVE_LOADER_UNBLOCKING_UPING
  move backward
  loader up

MOVE_MOVING_BACKWARD_TO_TURN_FREELY:
 bot_move_succeed: path_found_rotate -> MOVE_ROTATING
  rotate towards next position.
 bot_move_succeed: path_found -> MOVE_MOVING
  move to next position.
 bot_move_succeed: no_path_found -> MOVE_IDLE
  post failure event.
 bot_move_failed: path_found_rotate -> MOVE_ROTATING
  rotate towards next position.
 bot_move_failed: path_found -> MOVE_MOVING
  move to next position.
 bot_move_failed: no_path_found -> MOVE_WAIT_FOR_CLEAR_PATH
  nothing to do.

MOVE_WAIT_FOR_CLEAR_PATH:
 state_timeout: path_found_rotate -> MOVE_ROTATING
  rotate towards next position.
 state_timeout: path_found -> MOVE_MOVING
  move to next position.
 state_timeout: no_path_found_and_try_again -> .
  decrement counter.
 state_timeout: no_path_found_and_no_try_again -> MOVE_IDLE
  post failure.

MOVE_LOADER_UNBLOCKING_UPING:
 bot_move_succeed -> MOVE_LOADER_UNBLOCKING_DOWNING
  loader down
 bot_move_failed -> MOVE_LOADER_UNBLOCKING_DOWNING
  loader down

MOVE_LOADER_UNBLOCKING_DOWNING:
 loader_downed: rotate -> MOVE_ROTATING
  repeat last rotate
 loader_downed: move -> MOVE_MOVING
  repeat last move
 loader_errored -> MOVE_LOADER_UNBLOCKING_UPING
  move backward
  loader up