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/clamp.c | 35 +++++++++++++++++++++++++++++----- 1 file changed, 30 insertions(+), 5 deletions(-) (limited to 'digital/io-hub/src/robospierre/clamp.c') 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)); } -- cgit v1.2.3