From 284ebff6aab7beed71c78e58996b267c4c0dca08 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 25 Jun 2011 17:27:07 +0200 Subject: digital/io-hub/src/robospierre: do not drop everything on blocking --- digital/io-hub/src/robospierre/bot.h | 2 ++ digital/io-hub/src/robospierre/clamp.c | 35 ++++++++++++++++++++++++++----- digital/io-hub/src/robospierre/logistic.c | 8 ++++--- digital/io-hub/src/robospierre/logistic.h | 4 ++-- 4 files changed, 39 insertions(+), 10 deletions(-) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 3996f10e..499046e2 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -99,6 +99,7 @@ BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP # define BOT_CLAMP_BAY_SIDE_ROTATION_STEP \ (BOT_CLAMP_BAY_BACK_ROTATION_STEP / 2) +# define BOT_CLAMP_BAY_SIDE_MARGIN_ROTATION_STEP 1000 #define BOT_CLAMP_CLOSED_FRONT_ROTATION_OFFSET 0 #define BOT_CLAMP_CLOSED_BACK_ROTATION_OFFSET 0 @@ -130,6 +131,7 @@ # define BOT_CLAMP_BAY_BACK_ROTATION_STEP \ BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP # define BOT_CLAMP_BAY_SIDE_ROTATION_STEP (0x1183 + 120) +# define BOT_CLAMP_BAY_SIDE_MARGIN_ROTATION_STEP 1000 #define BOT_CLAMP_CLOSED_FRONT_ROTATION_OFFSET -129 #define BOT_CLAMP_CLOSED_BACK_ROTATION_OFFSET -60 diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index b42226cb..68fbd44f 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -537,13 +537,38 @@ clamp_blocked (void) { /* Free everything. */ clamp_openclose (1); - clamp_door (CLAMP_SLOT_FRONT_BOTTOM, 1); - clamp_door (CLAMP_SLOT_FRONT_TOP, 1); - clamp_door (CLAMP_SLOT_BACK_BOTTOM, 1); - clamp_door (CLAMP_SLOT_BACK_TOP, 1); + uint16_t rotation_position = mimot_get_motor1_position (); + uint16_t elevation_position = mimot_get_motor0_position (); + if (rotation_position < BOT_CLAMP_BAY_SIDE_ROTATION_STEP + - BOT_CLAMP_BAY_SIDE_MARGIN_ROTATION_STEP) + { + clamp_door (CLAMP_SLOT_FRONT_BOTTOM, 1); + if (elevation_position + > (BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP + + BOT_CLAMP_SLOT_FRONT_TOP_ELEVATION_STEP) / 2) + { + clamp_door (CLAMP_SLOT_FRONT_TOP, 1); + logistic_dump (DIRECTION_FORWARD, 1); + } + else + logistic_dump (DIRECTION_FORWARD, 0); + } + else if (rotation_position > BOT_CLAMP_BAY_SIDE_ROTATION_STEP + + BOT_CLAMP_BAY_SIDE_MARGIN_ROTATION_STEP) + { + clamp_door (CLAMP_SLOT_BACK_BOTTOM, 1); + if (elevation_position + > (BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP + + BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP) / 2) + { + clamp_door (CLAMP_SLOT_BACK_TOP, 1); + logistic_dump (DIRECTION_BACKWARD, 1); + } + else + logistic_dump (DIRECTION_BACKWARD, 0); + } mimot_motor0_free (); mimot_motor1_free (); - logistic_dump (); /* Signal problem. */ fsm_queue_post_event (FSM_EVENT (AI, clamp_move_failure)); } diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index db0c8b94..82f5a4fa 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -582,11 +582,13 @@ logistic_drop_element_type (uint8_t direction) } void -logistic_dump (void) +logistic_dump (uint8_t direction, uint8_t drop_top) { + /* Drop. */ uint8_t i; - /* Drop all except side. */ - for (i = 0; i < CLAMP_SLOT_SIDE; i++) + uint8_t bay = direction == DIRECTION_FORWARD + ? CLAMP_SLOT_FRONT_BOTTOM : CLAMP_SLOT_BACK_BOTTOM; + for (i = bay; i < bay + 2 + drop_top; i++) ctx.slots[i] = 0; } diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h index 0a076477..c6bc0210 100644 --- a/digital/io-hub/src/robospierre/logistic.h +++ b/digital/io-hub/src/robospierre/logistic.h @@ -132,9 +132,9 @@ logistic_drop (uint8_t direction); uint8_t logistic_drop_element_type (uint8_t direction); -/** Dump every element. */ +/** Dump every element on one direction. */ void -logistic_dump (void); +logistic_dump (uint8_t direction, uint8_t drop_top); /** Is path clear between two positions? */ uint8_t -- cgit v1.2.3