summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/robospierre
diff options
context:
space:
mode:
authorNicolas Schodet2011-05-27 08:15:52 +0200
committerNicolas Schodet2011-05-29 09:57:32 +0200
commitfd8ce8373abf81f5d6781b577a0f50906f2f4ff8 (patch)
treeb85a04436dcf737a7c20138187b951c9d0309530 /digital/io-hub/src/robospierre
parent9d61a51ee604fb1f9863d00ff4348c939ce2eeb5 (diff)
digital/io-hub: add offset to back bay too
Diffstat (limited to 'digital/io-hub/src/robospierre')
-rw-r--r--digital/io-hub/src/robospierre/bot.h12
-rw-r--r--digital/io-hub/src/robospierre/clamp.c10
2 files changed, 16 insertions, 6 deletions
diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h
index 6079b9d8..02b74da9 100644
--- a/digital/io-hub/src/robospierre/bot.h
+++ b/digital/io-hub/src/robospierre/bot.h
@@ -78,7 +78,8 @@
# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP \
(BOT_CLAMP_BAY_BACK_ROTATION_STEP / 2)
-#define BOT_CLAMP_CLOSED_ROTATION_OFFSET 0
+#define BOT_CLAMP_CLOSED_FRONT_ROTATION_OFFSET 0
+#define BOT_CLAMP_CLOSED_BACK_ROTATION_OFFSET 0
#else /* !HOST */
@@ -106,10 +107,17 @@
BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP
# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP 0x10de
-#define BOT_CLAMP_CLOSED_ROTATION_OFFSET -61
+#define BOT_CLAMP_CLOSED_FRONT_ROTATION_OFFSET -129
+#define BOT_CLAMP_CLOSED_BACK_ROTATION_OFFSET -60
#endif /* !HOST */
+#define BOT_CLAMP_CLOSED_ROTATION_OFFSET(pos) \
+ (CLAMP_IS_SLOT_IN_FRONT_BAY (pos) \
+ ? BOT_CLAMP_CLOSED_FRONT_ROTATION_OFFSET \
+ : (CLAMP_IS_SLOT_IN_BACK_BAY (pos) \
+ ? BOT_CLAMP_CLOSED_BACK_ROTATION_OFFSET : 0))
+
#define BOT_CLAMP_ELEVATION_SPEED 0x60
#define BOT_CLAMP_ROTATION_SPEED 0x60
#define BOT_CLAMP_ROTATION_OFFSET_SPEED 1
diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c
index 0c01e468..71cb2dfb 100644
--- a/digital/io-hub/src/robospierre/clamp.c
+++ b/digital/io-hub/src/robospierre/clamp.c
@@ -291,9 +291,11 @@ clamp_openclose (uint8_t open)
pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_OPEN);
else
pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_CLOSE);
- if (ctx.controled && CLAMP_IS_SLOT_IN_FRONT_BAY (ctx.pos_current))
+ if (ctx.controled && (CLAMP_IS_SLOT_IN_FRONT_BAY (ctx.pos_current)
+ || CLAMP_IS_SLOT_IN_BACK_BAY (ctx.pos_current)))
{
- int16_t offset = open ? 0 : BOT_CLAMP_CLOSED_ROTATION_OFFSET;
+ int16_t offset = open ? 0
+ : BOT_CLAMP_CLOSED_ROTATION_OFFSET (ctx.pos_current);
mimot_move_motor1_absolute (clamp_pos[ctx.pos_current][1] + offset,
BOT_CLAMP_ROTATION_OFFSET_SPEED);
}
@@ -361,8 +363,8 @@ clamp_route (void)
/* Run motors. */
mimot_move_motor0_absolute (clamp_pos[pos_new][0],
BOT_CLAMP_ELEVATION_SPEED);
- int16_t offset = !ctx.open && CLAMP_IS_SLOT_IN_FRONT_BAY (pos_new)
- ? BOT_CLAMP_CLOSED_ROTATION_OFFSET : 0;
+ int16_t offset = ctx.open ? 0
+ : BOT_CLAMP_CLOSED_ROTATION_OFFSET (pos_new);
mimot_move_motor1_absolute (clamp_pos[pos_new][1] + offset,
BOT_CLAMP_ROTATION_SPEED);
ctx.controled = 1;