summaryrefslogtreecommitdiffhomepage
path: root/digital/io-hub/src/robospierre/move.c
diff options
context:
space:
mode:
authorNicolas Schodet2011-06-02 07:09:25 +0200
committerNicolas Schodet2011-06-02 07:09:25 +0200
commitbecf1a33459af05d60f70a9b18bdeb05a987c558 (patch)
treed0b595a78f98e6e7579c6af03e12f05e41d012af /digital/io-hub/src/robospierre/move.c
parentc47f5e0c5a741f16ea115e6912ced21e53daf94f (diff)
digital/io-hub: stop robot when a element is taken
Diffstat (limited to 'digital/io-hub/src/robospierre/move.c')
-rw-r--r--digital/io-hub/src/robospierre/move.c32
1 files changed, 32 insertions, 0 deletions
diff --git a/digital/io-hub/src/robospierre/move.c b/digital/io-hub/src/robospierre/move.c
index 623dabdf..a44b5715 100644
--- a/digital/io-hub/src/robospierre/move.c
+++ b/digital/io-hub/src/robospierre/move.c
@@ -102,6 +102,13 @@ move_start_noangle (vect_t position, uint8_t backward, int16_t shorten)
}
void
+move_stop (void)
+{
+ /* Stop the FSM. */
+ fsm_queue_post_event (FSM_EVENT (AI, move_stop));
+}
+
+void
move_obstacles_update (void)
{
uint8_t i;
@@ -144,6 +151,8 @@ FSM_EVENTS (
robot_move_failure,
/* Initialize the FSM and start the movement directly. */
move_start,
+ /* Stop movement. */
+ move_stop,
/* Movement success. */
move_success,
/* Movement failure. */
@@ -343,6 +352,12 @@ FSM_TRANS_TIMEOUT (MOVE_ROTATING, 1250,
return FSM_NEXT_TIMEOUT (MOVE_ROTATING);
}
+FSM_TRANS (MOVE_ROTATING, move_stop, MOVE_IDLE)
+{
+ asserv_stop_motor ();
+ return FSM_NEXT (MOVE_ROTATING, move_stop);
+}
+
FSM_TRANS (MOVE_MOVING, robot_move_success,
done, MOVE_IDLE,
path_found_rotate, MOVE_ROTATING,
@@ -415,6 +430,12 @@ FSM_TRANS (MOVE_MOVING, obstacle_in_front,
return FSM_NEXT (MOVE_MOVING, obstacle_in_front, tryagain);
}
+FSM_TRANS (MOVE_MOVING, move_stop, MOVE_IDLE)
+{
+ asserv_stop_motor ();
+ return FSM_NEXT (MOVE_MOVING, move_stop);
+}
+
FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success,
tryout, MOVE_IDLE,
path_found_rotate, MOVE_ROTATING,
@@ -479,6 +500,12 @@ FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure,
}
}
+FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, move_stop, MOVE_IDLE)
+{
+ asserv_stop_motor ();
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, move_stop);
+}
+
FSM_TRANS_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, 250,
path_found_rotate, MOVE_ROTATING,
path_found, MOVE_MOVING,
@@ -507,3 +534,8 @@ FSM_TRANS_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, 250,
}
}
+FSM_TRANS (MOVE_WAIT_FOR_CLEAR_PATH, move_stop, MOVE_IDLE)
+{
+ return FSM_NEXT (MOVE_WAIT_FOR_CLEAR_PATH, move_stop);
+}
+