summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorNicolas Schodet2011-05-14 12:48:49 +0200
committerNicolas Schodet2011-05-14 12:48:49 +0200
commit3fa20141eb2d87df11ded1f05ff7ddf17882c86b (patch)
tree16e5c937412724d1c2a067e6d3f22965c4d4caef
parent366eeaccdca0ce784415a7b1bbc7e75f7c889af7 (diff)
digital/io-hub: add hardware offset when turning clamp
-rw-r--r--digital/io-hub/src/robospierre/bot.h4
-rw-r--r--digital/io-hub/src/robospierre/clamp.c29
-rw-r--r--digital/io-hub/src/robospierre/clamp.h8
3 files changed, 37 insertions, 4 deletions
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;
}
@@ -233,12 +252,20 @@ clamp_route (void)
}
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,