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/bot.h | 4 ++-- digital/io-hub/src/robospierre/clamp.c | 29 ++++++++++++++++++++++++++++- digital/io-hub/src/robospierre/clamp.h | 8 +++++++- 3 files changed, 37 insertions(+), 4 deletions(-) (limited to 'digital') diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 5e795b0f..e014321e 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -43,8 +43,8 @@ # define BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP (0x3b0b / 2) # define BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP 0x3b0b # define BOT_CLAMP_SLOT_SIDE_ELEVATION_STEP 0x3b0b -# define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP (0x3b0b / 2) -# define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP (0x3b0b / 2) +# define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP (0x3b0b / 3) +# define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP (0x3b0b / 3) # define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP (0x3b0b / 2) # define BOT_CLAMP_BAY_FRONT_ROTATION_STEP 0 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; diff --git a/digital/io-hub/src/robospierre/clamp.h b/digital/io-hub/src/robospierre/clamp.h index 8c5f5c4c..bb15b62e 100644 --- a/digital/io-hub/src/robospierre/clamp.h +++ b/digital/io-hub/src/robospierre/clamp.h @@ -36,9 +36,15 @@ enum { CLAMP_SLOT_SIDE, /** Leave the front bay, ready to enter side tunnel. */ CLAMP_BAY_FRONT_LEAVE, + /** Leaving the front bay, entered the side tunnel. When at midway point, + * go directly to the next position. */ + CLAMP_BAY_FRONT_LEAVING, /** Leave the back bay, ready to enter side tunnel. */ CLAMP_BAY_BACK_LEAVE, - /* Enter the side bay. Position on the side, above wheels. */ + /** Leaving the back bay, entered the side tunnel. When at midway point, + * go directly to the next position. */ + CLAMP_BAY_BACK_LEAVING, + /** Enter the side bay. Position on the side, above wheels. */ CLAMP_BAY_SIDE_ENTER_LEAVE, /** Total number of position, including intermediary positions. */ CLAMP_POS_NB, -- cgit v1.2.3