summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/robospierre/clamp.c
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/clamp.c
parent04d27a86b1225e02f80d20962f9168fc9c0e76a7 (diff)
digital/io-hub/src/robospierre: do not drop everything on blocking
Diffstat (limited to 'digital/io-hub/src/robospierre/clamp.c')
-rw-r--r--digital/io-hub/src/robospierre/clamp.c35
1 files changed, 30 insertions, 5 deletions
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));
}