summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2011-06-03 12:53:24 +0200
committerNicolas Schodet2011-06-03 12:53:24 +0200
commitc8cfe9c4b5498a6d993f224935f38869e782352b (patch)
tree649e05070599ee14354e83248216a8b52024dfaa
parent7f3014a642702c61a5c2e619a8c2f9d7ca7ec3bb (diff)
digital/io-hub: give the right element to element module
-rw-r--r--digital/io-hub/src/robospierre/clamp.c11
-rw-r--r--digital/io-hub/src/robospierre/top.c12
2 files changed, 12 insertions, 11 deletions
diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c
index 0c6573d7..f91af6a0 100644
--- a/digital/io-hub/src/robospierre/clamp.c
+++ b/digital/io-hub/src/robospierre/clamp.c
@@ -425,6 +425,14 @@ clamp_route (void)
ctx.pos_current = pos_new;
}
+static void
+clamp_taken_pawn (uint8_t element_type)
+{
+ position_t robot_pos;
+ asserv_get_position (&robot_pos);
+ element_taken (element_nearest_element_id (robot_pos), element_type);
+}
+
/* When lifting an element, we can discover it is actually a head. In this
* case, change destination. */
void
@@ -443,6 +451,7 @@ clamp_change (void)
if (contact_head)
{
logistic_element_change (from, ELEMENT_KING);
+ clamp_taken_pawn (ELEMENT_HEAD);
if (logistic_global.moving_from != from)
/* Cancel move. */
ctx.pos_request = ctx.moving_to = from;
@@ -617,6 +626,7 @@ FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME,
done, CLAMP_IDLE)
{
logistic_element_new (ctx.pos_new, ctx.new_element_type);
+ clamp_taken_pawn (ctx.new_element_type);
if (logistic_global.moving_from != CLAMP_SLOT_NB)
{
clamp_move_element (logistic_global.moving_from,
@@ -747,6 +757,7 @@ FSM_TRANS (CLAMP_LOCKED, clamp_new_element, CLAMP_LOCKED)
pwm_set_timed (clamp_slot_door[ctx.pos_new],
BOT_PWM_DOOR_CLOSE (ctx.pos_new));
logistic_element_new (ctx.pos_new, ctx.new_element_type);
+ clamp_taken_pawn (ctx.new_element_type);
return FSM_NEXT (CLAMP_LOCKED, clamp_new_element);
}
diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c
index 52bb05ae..09de28f6 100644
--- a/digital/io-hub/src/robospierre/top.c
+++ b/digital/io-hub/src/robospierre/top.c
@@ -207,14 +207,6 @@ 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,
@@ -281,7 +273,6 @@ 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);
}
@@ -291,7 +282,7 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success,
drop, TOP_GOING_TO_DROP,
element, TOP_GOING_TO_ELEMENT)
{
- element_taken (ctx.target_element_id, ELEMENT_PAWN);
+ element_failure (ctx.target_element_id); /* Do not take this one again. */
if (clamp_working ())
return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, clamp_working);
switch (top_decision ())
@@ -318,7 +309,6 @@ 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);
}