From 447138614e45686cff0bf55a351610c1c108168a Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 22 May 2011 18:42:06 +0200 Subject: digital/io-hub: update clamp positions and pwm values --- digital/io-hub/src/robospierre/bot.h | 75 ++++++++++++++++++++----------- digital/io-hub/src/robospierre/clamp.c | 17 +++---- host/simu/robots/robospierre/model/bag.py | 4 +- 3 files changed, 59 insertions(+), 37 deletions(-) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index a986c015..5f9d177a 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -53,30 +53,49 @@ # 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 / 3) -# define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP (0x3b0b / 3) +# define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP (0x3b0b / 2 + 1000) +# define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP (0x3b0b / 2 + 1000) # define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP (0x3b0b / 2) -# define BOT_CLAMP_BAY_FRONT_ROTATION_STEP 0 -# define BOT_CLAMP_BAY_BACK_ROTATION_STEP 0x11c6 -# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP (0x11c6 / 2) +# define BOT_CLAMP_SLOT_FRONT_BOTTOM_ROTATION_STEP 0 +# define BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP 0 +# define BOT_CLAMP_SLOT_FRONT_TOP_ROTATION_STEP 0 +# define BOT_CLAMP_SLOT_BACK_BOTTOM_ROTATION_STEP 0x11c6 +# define BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP 0x11c6 +# define BOT_CLAMP_SLOT_BACK_TOP_ROTATION_STEP 0x11c6 + +# define BOT_CLAMP_BAY_FRONT_ROTATION_STEP \ + BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP +# define BOT_CLAMP_BAY_BACK_ROTATION_STEP \ + BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP +# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP \ + (BOT_CLAMP_BAY_BACK_ROTATION_STEP / 2) #else /* !HOST */ # define BOT_CLAMP_SLOT_FRONT_BOTTOM_ELEVATION_STEP 0 -# define BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP 0x1d83 -# define BOT_CLAMP_SLOT_FRONT_TOP_ELEVATION_STEP 0x3288 -# define BOT_CLAMP_SLOT_BACK_BOTTOM_ELEVATION_STEP 0 -# define BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP 0x1d83 -# define BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP 0x3288 -# define BOT_CLAMP_SLOT_SIDE_ELEVATION_STEP 0x3288 -# define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP 0x1d83 -# define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP 0x1d83 -# define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP 0x1d83 - -# define BOT_CLAMP_BAY_FRONT_ROTATION_STEP 0 -# define BOT_CLAMP_BAY_BACK_ROTATION_STEP 0x10e2 -# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP 0x820 +# define BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP (0x1da7 - 250) +# define BOT_CLAMP_SLOT_FRONT_TOP_ELEVATION_STEP 0x34f7 +# define BOT_CLAMP_SLOT_BACK_BOTTOM_ELEVATION_STEP 0x0169 +# define BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP (0x1f03 - 250) +# define BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP 0x3610 +# define BOT_CLAMP_SLOT_SIDE_ELEVATION_STEP 0x3729 +# define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP 0x1da7 +# define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP 0x1f03 +# define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP ((0x1da7 + 0x1f03) / 2) + +# define BOT_CLAMP_SLOT_FRONT_BOTTOM_ROTATION_STEP 0 +# define BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP 0 +# define BOT_CLAMP_SLOT_FRONT_TOP_ROTATION_STEP 0 +# define BOT_CLAMP_SLOT_BACK_BOTTOM_ROTATION_STEP 0x10ce +# define BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP ((0x10ce + 0x10e2) / 2) +# define BOT_CLAMP_SLOT_BACK_TOP_ROTATION_STEP 0x10e2 + +# define BOT_CLAMP_BAY_FRONT_ROTATION_STEP \ + BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP +# define BOT_CLAMP_BAY_BACK_ROTATION_STEP \ + BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP +# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP 0x816 #endif /* !HOST */ @@ -89,14 +108,16 @@ #define BOT_PWM_DOOR_BACK_BOTTOM 3 #define BOT_PWM_DOOR_BACK_TOP 4 -#define BOT_PWM_CLAMP_OPEN_TIME 225 -#define BOT_PWM_CLAMP_OPEN 0x3ff, 225, 0 -#define BOT_PWM_CLAMP_CLOSE_TIME 225 -#define BOT_PWM_CLAMP_CLOSE -0x3ff, 225, 0 - -#define BOT_PWM_DOOR_OPEN_TIME 225 -#define BOT_PWM_DOOR_OPEN 0x3ff, 225, 0 -#define BOT_PWM_DOOR_CLOSE_TIME 225 -#define BOT_PWM_DOOR_CLOSE -0x3ff, 225, 0 +#define BOT_PWM_CLAMP_OPEN_TIME 75 +#define BOT_PWM_CLAMP_OPEN 0x3ff, 75, 0 +#define BOT_PWM_CLAMP_CLOSE_TIME 75 +#define BOT_PWM_CLAMP_CLOSE -0x3ff, 75, 0 + +#define BOT_PWM_DOOR_OPEN_TIME 12 +#define BOT_PWM_DOOR_OPEN 0x3ff, 37, 0x55 +#define BOT_PWM_DOOR_CLOSE_TIME 50 +#define BOT_PWM_DOOR_CLOSE(slot) \ + -0x3ff, 50, ((slot == CLAMP_SLOT_FRONT_BOTTOM \ + || slot == CLAMP_SLOT_BACK_BOTTOM) ? -0x100 : -0x180) #endif /* bot_h */ diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 583cadb5..b7c85250 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -130,17 +130,17 @@ struct clamp_t clamp_global; /** Clamp positions. */ static const uint16_t clamp_pos[][2] = { { BOT_CLAMP_SLOT_FRONT_BOTTOM_ELEVATION_STEP, - BOT_CLAMP_BAY_FRONT_ROTATION_STEP }, + BOT_CLAMP_SLOT_FRONT_BOTTOM_ROTATION_STEP }, { BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP, - BOT_CLAMP_BAY_FRONT_ROTATION_STEP }, + BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP }, { BOT_CLAMP_SLOT_FRONT_TOP_ELEVATION_STEP, - BOT_CLAMP_BAY_FRONT_ROTATION_STEP }, + BOT_CLAMP_SLOT_FRONT_TOP_ROTATION_STEP }, { BOT_CLAMP_SLOT_BACK_BOTTOM_ELEVATION_STEP, - BOT_CLAMP_BAY_BACK_ROTATION_STEP }, + BOT_CLAMP_SLOT_BACK_BOTTOM_ROTATION_STEP }, { BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP, - BOT_CLAMP_BAY_BACK_ROTATION_STEP }, + BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP }, { BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP, - BOT_CLAMP_BAY_BACK_ROTATION_STEP }, + BOT_CLAMP_SLOT_BACK_TOP_ROTATION_STEP }, { BOT_CLAMP_SLOT_SIDE_ELEVATION_STEP, BOT_CLAMP_BAY_SIDE_ROTATION_STEP }, { BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP, @@ -336,7 +336,8 @@ FSM_TRANS (CLAMP_GOING_IDLE, clamp_move_success, CLAMP_IDLE) FSM_TRANS (CLAMP_IDLE, clamp_new_element, CLAMP_TAKING_DOOR_CLOSING) { - pwm_set_timed (clamp_slot_door[ctx.pos_new], BOT_PWM_DOOR_CLOSE); + pwm_set_timed (clamp_slot_door[ctx.pos_new], + BOT_PWM_DOOR_CLOSE (ctx.pos_new)); return FSM_NEXT (CLAMP_IDLE, clamp_new_element); } @@ -524,7 +525,7 @@ FSM_TRANS (CLAMP_MOVE_DST_ROUTING, clamp_elevation_rotation_success, if (clamp_slot_door[ctx.pos_current] != 0xff) { pwm_set_timed (clamp_slot_door[ctx.pos_current], - BOT_PWM_DOOR_CLOSE); + BOT_PWM_DOOR_CLOSE (ctx.pos_current)); return FSM_NEXT (CLAMP_MOVE_DST_ROUTING, clamp_elevation_rotation_success, done_close_door); diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py index 5a6fa023..b1c49d35 100644 --- a/host/simu/robots/robospierre/model/bag.py +++ b/host/simu/robots/robospierre/model/bag.py @@ -38,9 +38,9 @@ class Bag: for contact in link_bag.io_hub.contact[2:] ] self.position = Position (link_bag.asserv.position) self.clamping_motor = MotorBasic (link_bag.io_hub.pwm[2], scheduler, - 2 * pi, 0, pi) + 8 * pi, 0, pi) self.door_motors = [ MotorBasic (link_bag.io_hub.pwm[i], scheduler, - 2 * pi, 0, 0.5 * pi) for i in (0, 1, 3, 4) ] + 8 * pi, 0, 0.5 * pi) for i in (0, 1, 3, 4) ] self.clamp = Clamp (table, self.position, link_bag.mimot.aux[0], link_bag.mimot.aux[1], self.clamping_motor, self.door_motors, self.contact[0:7]) -- cgit v1.2.3