summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src/move.fsm
blob: ab55d1d405ef377be83ff4775eff0a9b831a5034 (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
# 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

States:
 IDLE
  waiting for the beginning of the move FSM
 MOVING_TO_FINAL_POSITION
  moving to the final position
 MOVING_BACKWARD
  moving backward to go away from what is blocking the bot
 MOVING_TO_INTERMEDIATE_POSITION
  moving to an intermediate position to try avoiding the obstacle

Events:
 start
  initialize the FSM
 bot_move_failed
  the bot movement failed (blocked by something for example)
 bot_move_succeed
  the bot has finished to moved and arrived at the desired position
 bot_move_obstacle
  the bot has seen something (with the sharps)

IDLE:
 start -> MOVING_TO_FINAL_POSITION
  ask the asserv to go to the final position

MOVING_TO_FINAL_POSITION:
 bot_move_succeed -> IDLE
  post an event for the top FSM to tell we have finished
 bot_move_failed -> MOVING_BACKWARD
  store the current position of the obstacle
  move backward to turn freely
 bot_move_obstacle -> MOVING_TO_INTERMEDIATE_POSITION
  compute the obstacle position
  give the obstacle position to the path module
  give the current position of the bot to the path module
  get the intermediate position from path module

MOVING_BACKWARD:
# TODO
# We ignore obstacle when moving backward
 bot_move_succeed -> MOVING_TO_INTERMEDIATE_POSITION
  give the obstacle position to the path module
  give the current position of the bot to the path module
  get the intermediate position from path module
 bot_move_failed -> MOVING_TO_INTERMEDIATE_POSITION
  do the same as when we succeed
  give the obstacle position to the path module
  give the current position of the bot to the path module
  get the intermediate position from path module

MOVING_TO_INTERMEDIATE_POSITION:
 bot_move_obstacle -> .
  compute the obstacle position
  give the obstacle position to the path module
  go to the next intermediate position computed by the path module
 bot_move_succeed: final_position -> IDLE
  post an event for the top FSM to tell we have finished
 bot_move_succeed: position_intermediary -> .
  go to the next intermediate position computed by the path module
 bot_move_failed -> MOVING_BACKWARD
  store the current position of the obstacle
  move backward to turn freely