summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2011-06-02 07:09:25 +0200
committerNicolas Schodet2011-06-02 07:09:25 +0200
commitbecf1a33459af05d60f70a9b18bdeb05a987c558 (patch)
treed0b595a78f98e6e7579c6af03e12f05e41d012af
parentc47f5e0c5a741f16ea115e6912ced21e53daf94f (diff)
digital/io-hub: stop robot when a element is taken
-rw-r--r--digital/io-hub/src/robospierre/move.c32
-rw-r--r--digital/io-hub/src/robospierre/move.h4
-rw-r--r--digital/io-hub/src/robospierre/top.c22
3 files changed, 58 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);
+}
+
diff --git a/digital/io-hub/src/robospierre/move.h b/digital/io-hub/src/robospierre/move.h
index 877002fd..72af5b84 100644
--- a/digital/io-hub/src/robospierre/move.h
+++ b/digital/io-hub/src/robospierre/move.h
@@ -48,6 +48,10 @@ move_start (position_t position, uint8_t backward);
void
move_start_noangle (vect_t position, uint8_t backward, int16_t shorten);
+/** Stop movement. */
+void
+move_stop (void);
+
/** To be called when obstacles positions are computed. */
void
move_obstacles_update (void);
diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c
index 4554c12b..05568099 100644
--- a/digital/io-hub/src/robospierre/top.c
+++ b/digital/io-hub/src/robospierre/top.c
@@ -160,6 +160,14 @@ top_decision (void)
return top_go_element ();
}
+static void
+top_taken_pawn (void)
+{
+ position_t robot_pos;
+ asserv_get_position (&robot_pos);
+ element_taken (element_nearest_element_id (robot_pos), ELEMENT_PAWN);
+}
+
FSM_TRANS (TOP_GOING_OUT2, robot_move_success,
clamp_working, TOP_WAITING_CLAMP,
drop, TOP_GOING_TO_DROP,
@@ -224,6 +232,13 @@ FSM_TRANS (TOP_GOING_TO_DROP, move_failure,
}
}
+FSM_TRANS (TOP_GOING_TO_DROP, clamp_working, TOP_WAITING_CLAMP)
+{
+ top_taken_pawn ();
+ move_stop ();
+ return FSM_NEXT (TOP_GOING_TO_DROP, clamp_working);
+}
+
FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success,
clamp_working, TOP_WAITING_CLAMP,
drop, TOP_GOING_TO_DROP,
@@ -254,6 +269,13 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, move_failure,
}
}
+FSM_TRANS (TOP_GOING_TO_ELEMENT, clamp_working, TOP_WAITING_CLAMP)
+{
+ top_taken_pawn ();
+ move_stop ();
+ return FSM_NEXT (TOP_GOING_TO_ELEMENT, clamp_working);
+}
+
FSM_TRANS (TOP_WAITING_CLAMP, clamp_done,
drop, TOP_GOING_TO_DROP,
element, TOP_GOING_TO_ELEMENT)