From 3fa20141eb2d87df11ded1f05ff7ddf17882c86b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 14 May 2011 12:48:49 +0200 Subject: digital/io-hub: add hardware offset when turning clamp --- digital/io-hub/src/robospierre/clamp.c | 29 ++++++++++++++++++++++++++++- 1 file changed, 28 insertions(+), 1 deletion(-) (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 1ee020e9..227579ff 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -134,8 +134,12 @@ static const uint16_t clamp_pos[][2] = { BOT_CLAMP_BAY_SIDE_ROTATION_STEP }, { BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP, BOT_CLAMP_BAY_FRONT_ROTATION_STEP }, + { BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP, + BOT_CLAMP_BAY_SIDE_ROTATION_STEP }, { BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP, BOT_CLAMP_BAY_BACK_ROTATION_STEP }, + { BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP, + BOT_CLAMP_BAY_SIDE_ROTATION_STEP }, { BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP, BOT_CLAMP_BAY_SIDE_ROTATION_STEP }, }; @@ -151,6 +155,9 @@ static const uint8_t clamp_slot_door[] = { 0xff }; +static void +clamp_route (void); + void clamp_move (uint8_t pos) { @@ -202,6 +209,18 @@ clamp_handle_event (void) return 1; } } + /* Handle special hardware offset. */ + uint16_t rotation_position = mimot_get_motor1_position (); + if ((ctx.pos_current == CLAMP_BAY_FRONT_LEAVING + && rotation_position > (BOT_CLAMP_BAY_SIDE_ROTATION_STEP - + BOT_CLAMP_BAY_FRONT_ROTATION_STEP) / 2) + || (ctx.pos_current == CLAMP_BAY_BACK_LEAVING + && rotation_position < (BOT_CLAMP_BAY_BACK_ROTATION_STEP - + BOT_CLAMP_BAY_SIDE_ROTATION_STEP) / 2)) + { + /* Go directly to next point. */ + clamp_route (); + } return 0; } @@ -232,13 +251,21 @@ clamp_route (void) pos_new = CLAMP_BAY_SIDE_ENTER_LEAVE; } else if (pos_current == CLAMP_BAY_FRONT_LEAVE) + { + pos_new = CLAMP_BAY_FRONT_LEAVING; + } + else if (pos_current == CLAMP_BAY_BACK_LEAVE) + { + pos_new = CLAMP_BAY_BACK_LEAVING; + } + else if (pos_current == CLAMP_BAY_FRONT_LEAVING) { if (pos_request == CLAMP_SLOT_SIDE) pos_new = CLAMP_BAY_SIDE_ENTER_LEAVE; else pos_new = CLAMP_SLOT_BACK_MIDDLE; } - else if (pos_current == CLAMP_BAY_BACK_LEAVE) + else if (pos_current == CLAMP_BAY_BACK_LEAVING) { if (pos_request == CLAMP_SLOT_SIDE) pos_new = CLAMP_BAY_SIDE_ENTER_LEAVE; -- cgit v1.2.3