summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/robospierre
diff options
context:
space:
mode:
authorNicolas Schodet2011-06-25 17:27:07 +0200
committerNicolas Schodet2011-06-25 17:27:07 +0200
commit284ebff6aab7beed71c78e58996b267c4c0dca08 (patch)
tree2eb0dbd39068ca6e910a6be4fe2581ea8e1648e3 /digital/io-hub/src/robospierre
parent04d27a86b1225e02f80d20962f9168fc9c0e76a7 (diff)
digital/io-hub/src/robospierre: do not drop everything on blocking
Diffstat (limited to 'digital/io-hub/src/robospierre')
-rw-r--r--digital/io-hub/src/robospierre/bot.h2
-rw-r--r--digital/io-hub/src/robospierre/clamp.c35
-rw-r--r--digital/io-hub/src/robospierre/logistic.c8
-rw-r--r--digital/io-hub/src/robospierre/logistic.h4
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