From a3fdbecac564a600c65568cac0e02fc72604567e Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 12 May 2011 23:42:26 +0200 Subject: digital/{ai,io-hub}, host/simu/robots/robospierre: add doors and element move --- digital/ai/tools/test_simu_control_robospierre.py | 29 +++- digital/io-hub/src/robospierre/bot.h | 16 ++ digital/io-hub/src/robospierre/clamp.c | 169 ++++++++++++++++++++-- digital/io-hub/src/robospierre/clamp.h | 8 + digital/io-hub/src/robospierre/main.c | 17 ++- digital/io-hub/tools/io_hub/io_hub.py | 3 + host/simu/robots/robospierre/model/bag.py | 6 +- host/simu/robots/robospierre/model/clamp.py | 28 ++-- host/simu/robots/robospierre/view/clamp.py | 10 ++ 9 files changed, 258 insertions(+), 28 deletions(-) diff --git a/digital/ai/tools/test_simu_control_robospierre.py b/digital/ai/tools/test_simu_control_robospierre.py index b1cccf4b..88fbc5f3 100644 --- a/digital/ai/tools/test_simu_control_robospierre.py +++ b/digital/ai/tools/test_simu_control_robospierre.py @@ -46,6 +46,12 @@ class TestSimuControl (TestSimu): indicatoron = False, variable = self.clamp_var, command = self.clamp_command) self.clamp_button.pack () + self.doors_var = IntVar () + self.doors_var.set (1) + self.doors_button = Checkbutton (self.control_frame, text = 'Doors', + indicatoron = False, + variable = self.doors_var, command = self.doors_command) + self.doors_button.pack () self.elevation_up_button = Button (self.control_frame, text = 'Elevation up', padx = 0, pady = 0, command = self.elevation_up_command) @@ -69,6 +75,13 @@ class TestSimuControl (TestSimu): text = 'Move clamp', padx = 0, pady = 0, command = self.clamp_move_command) self.clamp_move_button.pack () + self.clamp_to_scale = Scale (self.control_frame, orient = HORIZONTAL, + from_ = 0, to = 6) + self.clamp_to_scale.pack () + self.clamp_element_move_button = Button (self.control_frame, + text = 'Move element', padx = 0, pady = 0, + command = self.clamp_move_element_command) + self.clamp_element_move_button.pack () self.table_view.bind ('<1>', self.move) self.table_view.bind ('<3>', self.orient) @@ -85,9 +98,9 @@ class TestSimuControl (TestSimu): def clamp_command (self): if self.clamp_var.get (): - self.io.pwm_set_timed (0, -0x3ff, 255, 0) + self.io.pwm_set_timed (2, -0x3ff, 255, 0) else: - self.io.pwm_set_timed (0, 0x3ff, 255, 0) + self.io.pwm_set_timed (2, 0x3ff, 255, 0) def elevation_up_command (self): self.mimot.speed_pos ('a0', self.ELEVATION_STROKE / 2) @@ -104,6 +117,18 @@ class TestSimuControl (TestSimu): def clamp_move_command (self): self.io.clamp_move (self.clamp_pos_scale.get ()) + def clamp_move_element_command (self): + self.io.clamp_move_element (self.clamp_pos_scale.get (), + self.clamp_to_scale.get ()) + + def doors_command (self): + if self.doors_var.get (): + pwm = -0x3ff + else: + pwm = 0x3ff + for i in (0, 1, 3, 4): + self.io.pwm_set_timed (i, pwm, 255, 0) + def change_color (self, *dummy): pass diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 9043a62c..5e795b0f 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -73,4 +73,20 @@ #define BOT_CLAMP_ELEVATION_SPEED 0x60 #define BOT_CLAMP_ROTATION_SPEED 0x30 +#define BOT_PWM_CLAMP 2 +#define BOT_PWM_DOOR_FRONT_BOTTOM 0 +#define BOT_PWM_DOOR_FRONT_TOP 1 +#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 + #endif /* bot_h */ diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 2f1b2471..5d9533fa 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -29,15 +29,28 @@ #include "fsm.h" #include "mimot.h" +#include "pwm.h" #include "bot.h" FSM_INIT FSM_STATES ( - /* Wait order. */ - CLAMP_IDLE, + /* Waiting movement order. */ + CLAMP_MOVE_IDLE, /* Moving to a final or intermediary position. */ - CLAMP_ROUTING) + CLAMP_MOVE_ROUTING, + /* Moving to source slot. */ + CLAMP_MOVE_SRC_ROUTING, + /* Closing the clamp once arrived at source. */ + CLAMP_MOVE_SRC_CLAMP_CLOSING, + /* Opening door once clamp closed. */ + CLAMP_MOVE_SRC_DOOR_OPENDING, + /* Moving to destination slot. */ + CLAMP_MOVE_DST_ROUTING, + /* Closing door once arrived at destination. */ + CLAMP_MOVE_DST_DOOR_CLOSING, + /* Opening the clamp once door closed. */ + CLAMP_MOVE_DST_CLAMP_OPENING) FSM_EVENTS ( /* Order to move the clamp. */ @@ -49,15 +62,17 @@ FSM_EVENTS ( /* Rotation motor failure. */ clamp_rotation_failure) -FSM_START_WITH (CLAMP_IDLE) +FSM_START_WITH (CLAMP_MOVE_IDLE) /** Clamp context. */ struct clamp_t { - /* Current position. */ + /** Current position. */ uint8_t pos_current; - /* Requested position. */ + /** Requested position. */ uint8_t pos_request; + /** Element moving destination. */ + uint8_t moving_to; }; /** Global context. */ @@ -88,16 +103,36 @@ static const uint16_t clamp_pos[][2] = { BOT_CLAMP_BAY_SIDE_ROTATION_STEP }, }; +/** Slot doors. */ +static const uint8_t clamp_slot_door[] = { + BOT_PWM_DOOR_FRONT_BOTTOM, + 0xff, + BOT_PWM_DOOR_FRONT_TOP, + BOT_PWM_DOOR_BACK_BOTTOM, + 0xff, + BOT_PWM_DOOR_BACK_TOP, + 0xff +}; + void clamp_move (uint8_t pos) { if (pos != ctx.pos_current) { ctx.pos_request = pos; + ctx.moving_to = CLAMP_POS_NB; FSM_HANDLE (AI, clamp_move); } } +void +clamp_move_element (uint8_t from, uint8_t to) +{ + ctx.pos_request = from; + ctx.moving_to = to; + FSM_HANDLE (AI, clamp_move); +} + /** Find next position and start motors. */ static void clamp_route (void) @@ -157,26 +192,132 @@ clamp_route (void) ctx.pos_current = pos_new; } -FSM_TRANS (CLAMP_IDLE, clamp_move, CLAMP_ROUTING) +FSM_TRANS (CLAMP_MOVE_IDLE, clamp_move, + move, CLAMP_MOVE_ROUTING, + move_element, CLAMP_MOVE_SRC_ROUTING, + move_element_here, CLAMP_MOVE_SRC_CLAMP_CLOSING) { - clamp_route (); - return FSM_NEXT (CLAMP_IDLE, clamp_move); + if (ctx.moving_to == CLAMP_POS_NB) + { + clamp_route (); + return FSM_NEXT (CLAMP_MOVE_IDLE, clamp_move, move); + } + else + { + if (ctx.pos_current != ctx.pos_request) + { + clamp_route (); + return FSM_NEXT (CLAMP_MOVE_IDLE, clamp_move, move_element); + } + else + { + pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_CLOSE); + return FSM_NEXT (CLAMP_MOVE_IDLE, clamp_move, move_element_here); + } + } } -FSM_TRANS (CLAMP_ROUTING, clamp_elevation_rotation_success, - done, CLAMP_IDLE, - next, CLAMP_ROUTING) +FSM_TRANS (CLAMP_MOVE_ROUTING, clamp_elevation_rotation_success, + done, CLAMP_MOVE_IDLE, + next, CLAMP_MOVE_ROUTING) { if (ctx.pos_current == ctx.pos_request) { - return FSM_NEXT (CLAMP_ROUTING, clamp_elevation_rotation_success, + return FSM_NEXT (CLAMP_MOVE_ROUTING, clamp_elevation_rotation_success, done); } else { clamp_route (); - return FSM_NEXT (CLAMP_ROUTING, clamp_elevation_rotation_success, + return FSM_NEXT (CLAMP_MOVE_ROUTING, clamp_elevation_rotation_success, next); } } +FSM_TRANS (CLAMP_MOVE_SRC_ROUTING, clamp_elevation_rotation_success, + done, CLAMP_MOVE_SRC_CLAMP_CLOSING, + next, CLAMP_MOVE_SRC_ROUTING) +{ + if (ctx.pos_current == ctx.pos_request) + { + pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_CLOSE); + return FSM_NEXT (CLAMP_MOVE_SRC_ROUTING, + clamp_elevation_rotation_success, done); + } + else + { + clamp_route (); + return FSM_NEXT (CLAMP_MOVE_SRC_ROUTING, + clamp_elevation_rotation_success, next); + } +} + +FSM_TRANS_TIMEOUT (CLAMP_MOVE_SRC_CLAMP_CLOSING, BOT_PWM_CLAMP_CLOSE_TIME, + open_door, CLAMP_MOVE_SRC_DOOR_OPENDING, + move, CLAMP_MOVE_DST_ROUTING) +{ + if (clamp_slot_door[ctx.pos_current] != 0xff) + { + pwm_set_timed (clamp_slot_door[ctx.pos_current], BOT_PWM_DOOR_OPEN); + return FSM_NEXT_TIMEOUT (CLAMP_MOVE_SRC_CLAMP_CLOSING, open_door); + } + else + { + ctx.pos_request = ctx.moving_to; + clamp_route (); + return FSM_NEXT_TIMEOUT (CLAMP_MOVE_SRC_CLAMP_CLOSING, move); + } +} + +FSM_TRANS_TIMEOUT (CLAMP_MOVE_SRC_DOOR_OPENDING, BOT_PWM_DOOR_OPEN_TIME, + CLAMP_MOVE_DST_ROUTING) +{ + ctx.pos_request = ctx.moving_to; + clamp_route (); + return FSM_NEXT_TIMEOUT (CLAMP_MOVE_SRC_DOOR_OPENDING); +} + +FSM_TRANS (CLAMP_MOVE_DST_ROUTING, clamp_elevation_rotation_success, + done_close_door, CLAMP_MOVE_DST_DOOR_CLOSING, + done_open_clamp, CLAMP_MOVE_DST_CLAMP_OPENING, + next, CLAMP_MOVE_DST_ROUTING) +{ + if (ctx.pos_current == ctx.pos_request) + { + if (clamp_slot_door[ctx.pos_current] != 0xff) + { + pwm_set_timed (clamp_slot_door[ctx.pos_current], + BOT_PWM_DOOR_CLOSE); + return FSM_NEXT (CLAMP_MOVE_DST_ROUTING, + clamp_elevation_rotation_success, + done_close_door); + } + else + { + pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_OPEN); + return FSM_NEXT (CLAMP_MOVE_DST_ROUTING, + clamp_elevation_rotation_success, + done_open_clamp); + } + } + else + { + clamp_route (); + return FSM_NEXT (CLAMP_MOVE_DST_ROUTING, + clamp_elevation_rotation_success, next); + } +} + +FSM_TRANS_TIMEOUT (CLAMP_MOVE_DST_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME, + CLAMP_MOVE_DST_CLAMP_OPENING) +{ + pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_OPEN); + return FSM_NEXT_TIMEOUT (CLAMP_MOVE_DST_DOOR_CLOSING); +} + +FSM_TRANS_TIMEOUT (CLAMP_MOVE_DST_CLAMP_OPENING, BOT_PWM_CLAMP_OPEN_TIME, + CLAMP_MOVE_IDLE) +{ + return FSM_NEXT_TIMEOUT (CLAMP_MOVE_DST_CLAMP_OPENING); +} + diff --git a/digital/io-hub/src/robospierre/clamp.h b/digital/io-hub/src/robospierre/clamp.h index 104e3cb8..7b488f92 100644 --- a/digital/io-hub/src/robospierre/clamp.h +++ b/digital/io-hub/src/robospierre/clamp.h @@ -40,6 +40,10 @@ enum { CLAMP_BAY_BACK_LEAVE, /* 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, + /** Number of slots. */ + CLAMP_SLOT_NB = CLAMP_SLOT_SIDE + 1, }; /** Is slot in front bay? */ @@ -54,4 +58,8 @@ enum { void clamp_move (uint8_t pos); +/** Move element using clamp. */ +void +clamp_move_element (uint8_t from, uint8_t to); + #endif /* clamp_h */ diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 3a32a1ae..13bad26c 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -47,6 +47,8 @@ #include "clamp.h" +#include "bot.h" + #include "io.h" /** Our color. */ @@ -93,7 +95,7 @@ main_event_to_fsm (void) #define FSM_HANDLE_TIMEOUT_E(fsm) \ do { if (FSM_HANDLE_TIMEOUT (fsm)) return; } while (0) /* Update FSM timeouts. */ - //FSM_HANDLE_TIMEOUT_E (AI); + FSM_HANDLE_TIMEOUT_E (AI); /* Motor status. */ asserv_status_e mimot_motor0_status, mimot_motor1_status; mimot_motor0_status = mimot_motor0_cmd_status (); @@ -184,6 +186,19 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - 1b: position. */ clamp_move (args[0]); break; + case c ('c', 2): + /* Move element using clamp. + * - 1b: source. + * - 1b: destination. */ + clamp_move_element (args[0], args[1]); + break; + case c ('d', 0): + /* Open all doors. */ + pwm_set_timed (BOT_PWM_DOOR_FRONT_BOTTOM, BOT_PWM_DOOR_OPEN); + pwm_set_timed (BOT_PWM_DOOR_FRONT_TOP, BOT_PWM_DOOR_OPEN); + pwm_set_timed (BOT_PWM_DOOR_BACK_BOTTOM, BOT_PWM_DOOR_OPEN); + pwm_set_timed (BOT_PWM_DOOR_BACK_TOP, BOT_PWM_DOOR_OPEN); + break; /* Stats commands. * - b: interval between stats. */ case c ('A', 1): diff --git a/digital/io-hub/tools/io_hub/io_hub.py b/digital/io-hub/tools/io_hub/io_hub.py index a363e5f6..1b25102c 100644 --- a/digital/io-hub/tools/io_hub/io_hub.py +++ b/digital/io-hub/tools/io_hub/io_hub.py @@ -47,6 +47,9 @@ class Proto: def clamp_move (self, pos): self.proto.send ('c', 'B', pos) + def clamp_move_element (self, from_, to): + self.proto.send ('c', 'BB', from_, to) + def close (self): self.reset () self.proto.wait (lambda: True) diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py index 4d87192c..0a89d00c 100644 --- a/host/simu/robots/robospierre/model/bag.py +++ b/host/simu/robots/robospierre/model/bag.py @@ -37,10 +37,12 @@ class Bag: self.contact = [ Switch (contact) 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[0], scheduler, + self.clamping_motor = MotorBasic (link_bag.io_hub.pwm[2], scheduler, 2 * 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) ] self.clamp = Clamp (table, self.position, link_bag.mimot.aux[0], - link_bag.mimot.aux[1], self.clamping_motor) + link_bag.mimot.aux[1], self.clamping_motor, self.door_motors) self.distance_sensor = [ DistanceSensorSensopart (link_bag.io_hub.adc[0], scheduler, table, (20, 20), pi * 10 / 180, (self.position, ), 2), diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index ff27a7ea..9bb8bacc 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -29,12 +29,13 @@ from math import pi, cos, sin class Slot: """Slot which can contain a pawn.""" - def __init__ (self, x, y, z, side): + def __init__ (self, x, y, z, side, door_motor): self.x = x self.y = y self.z = z self.side = side self.pawn = None + self.door_motor = door_motor class Clamp (Observable): @@ -59,21 +60,30 @@ class Clamp (Observable): SLOT_SIDE = 6 def __init__ (self, table, robot_position, elevation_motor, - rotation_motor, clamping_motor): + rotation_motor, clamping_motor, door_motors): Observable.__init__ (self) self.table = table self.robot_position = robot_position self.elevation_motor = elevation_motor self.rotation_motor = rotation_motor self.clamping_motor = clamping_motor + self.door_motors = (door_motors[0], None, door_motors[1], + door_motors[2], None, door_motors[3], None) self.slots = ( - Slot (self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 0), - Slot (self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 0), - Slot (self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 0), - Slot (-self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 1), - Slot (-self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 1), - Slot (-self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 1), - Slot (0, self.BAY_OFFSET, 2 * self.BAY_ZOFFSET, None)) + Slot (self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 0, + door_motors[0]), + Slot (self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 0, + None), + Slot (self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 0, + door_motors[1]), + Slot (-self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 1, + door_motors[2]), + Slot (-self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 1, + None), + Slot (-self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 1, + door_motors[3]), + Slot (0, self.BAY_OFFSET, 2 * self.BAY_ZOFFSET, None, + None)) self.load = None self.robot_position.register (self.__robot_position_notified) self.elevation_motor.register (self.__elevation_notified) diff --git a/host/simu/robots/robospierre/view/clamp.py b/host/simu/robots/robospierre/view/clamp.py index bd5f6dab..130c3eea 100644 --- a/host/simu/robots/robospierre/view/clamp.py +++ b/host/simu/robots/robospierre/view/clamp.py @@ -133,6 +133,7 @@ class ClampSide (Drawable): self.draw_pawn ((-slot.x, slot.z), slot.pawn) # Draw clamp. if self.model.rotation is not None: + self.trans_push () self.trans_translate ((0, self.model.elevation)) m = TransMatrix () m.rotate (pi + self.model.rotation) @@ -169,4 +170,13 @@ class ClampSide (Drawable): self.draw_pawn ((lcenter, 0), self.model.load) self.draw_rectangle ((lbase, 10), (lcenter - 103, 40), **attr) self.draw_rectangle ((rbase, 10), (rtip, 40), **attr) + self.trans_pop () + # Draw doors. + for slot in self.model.slots: + if slot.door_motor is not None: + self.trans_push () + self.trans_translate ((-slot.x, slot.z + 50)) + self.trans_rotate (-0.5 * pi + slot.door_motor.angle) + self.draw_line ((0, 0), (40, 0)) + self.trans_pop () -- cgit v1.2.3