From fd8ce8373abf81f5d6781b577a0f50906f2f4ff8 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 27 May 2011 08:15:52 +0200 Subject: digital/io-hub: add offset to back bay too --- digital/io-hub/src/robospierre/bot.h | 12 ++++++++++-- digital/io-hub/src/robospierre/clamp.c | 10 ++++++---- 2 files changed, 16 insertions(+), 6 deletions(-) (limited to 'digital/io-hub/src') 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; -- cgit v1.2.3