From c8cfe9c4b5498a6d993f224935f38869e782352b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 12:53:24 +0200 Subject: digital/io-hub: give the right element to element module --- digital/io-hub/src/robospierre/clamp.c | 11 +++++++++++ digital/io-hub/src/robospierre/top.c | 12 +----------- 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); } -- cgit v1.2.3