From 40f4332510d5f582e2b5dd19af5809e775f41ed0 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 8 May 2011 18:53:58 +0200 Subject: host/simu/robots/robospierre: move US sensors to right position --- host/simu/robots/robospierre/model/bag.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'host/simu/robots/robospierre') diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py index ce55f997..4d87192c 100644 --- a/host/simu/robots/robospierre/model/bag.py +++ b/host/simu/robots/robospierre/model/bag.py @@ -43,13 +43,13 @@ class Bag: link_bag.mimot.aux[1], self.clamping_motor) self.distance_sensor = [ DistanceSensorSensopart (link_bag.io_hub.adc[0], scheduler, table, - (20, -20), -pi * 10 / 180, (self.position, ), 2), - DistanceSensorSensopart (link_bag.io_hub.adc[1], scheduler, table, (20, 20), pi * 10 / 180, (self.position, ), 2), + DistanceSensorSensopart (link_bag.io_hub.adc[1], scheduler, table, + (20, -20), -pi * 10 / 180, (self.position, ), 2), DistanceSensorSensopart (link_bag.io_hub.adc[2], scheduler, table, - (-20, 20), pi - pi * 10 / 180, (self.position, ), 2), - DistanceSensorSensopart (link_bag.io_hub.adc[3], scheduler, table, (-20, -20), pi + pi * 10 / 180, (self.position, ), 2), + DistanceSensorSensopart (link_bag.io_hub.adc[3], scheduler, table, + (-20, 20), pi - pi * 10 / 180, (self.position, ), 2), ] for adc in link_bag.io_hub.adc[4:]: adc.value = 0 -- cgit v1.2.3 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(-) (limited to 'host/simu/robots/robospierre') 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 From c8d4bd0e0f79dc8e49ad0074a1e67317e903c41f Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 14 May 2011 01:28:54 +0200 Subject: digital/io-hub, host/simu: add contacts --- digital/io-hub/src/robospierre/contact_defs.h | 18 +++++++++++--- digital/io-hub/src/robospierre/simu.host.c | 2 +- digital/io-hub/src/robospierre/simu.host.h | 2 +- digital/io-hub/tools/io_hub/mex.py | 2 +- host/simu/model/switch.py | 8 ++++-- host/simu/robots/robospierre/model/bag.py | 7 +++--- host/simu/robots/robospierre/model/clamp.py | 36 ++++++++++++++++----------- 7 files changed, 49 insertions(+), 26 deletions(-) (limited to 'host/simu/robots/robospierre') diff --git a/digital/io-hub/src/robospierre/contact_defs.h b/digital/io-hub/src/robospierre/contact_defs.h index 55b34354..1e04f6bf 100644 --- a/digital/io-hub/src/robospierre/contact_defs.h +++ b/digital/io-hub/src/robospierre/contact_defs.h @@ -27,11 +27,21 @@ #define CONTACT_COLOR A, 7 #define CONTACT_JACK F, 7 -#define CONTACT_EX1 E, 0 -#define CONTACT_EX2 E, 1 +#define CONTACT_FRONT_BOTTOM A, 4 +#define CONTACT_FRONT_MIDDLE F, 4 +#define CONTACT_BACK_BOTTOM A, 5 +#define CONTACT_BACK_MIDDLE F, 5 +#define CONTACT_FRONT_TOP A, 6 +#define CONTACT_BACK_TOP F, 6 +#define CONTACT_SIDE E, 7 #define CONTACT_LIST \ - CONTACT (CONTACT_EX1) \ - CONTACT (CONTACT_EX2) + CONTACT (CONTACT_FRONT_BOTTOM) \ + CONTACT (CONTACT_FRONT_MIDDLE) \ + CONTACT (CONTACT_FRONT_TOP) \ + CONTACT (CONTACT_BACK_BOTTOM) \ + CONTACT (CONTACT_BACK_MIDDLE) \ + CONTACT (CONTACT_BACK_TOP) \ + CONTACT (CONTACT_SIDE) #endif /* contact_defs_h */ diff --git a/digital/io-hub/src/robospierre/simu.host.c b/digital/io-hub/src/robospierre/simu.host.c index 8f4019aa..281a60d6 100644 --- a/digital/io-hub/src/robospierre/simu.host.c +++ b/digital/io-hub/src/robospierre/simu.host.c @@ -31,7 +31,7 @@ #include "io.h" /** AVR registers. */ -uint8_t PINE; +uint8_t PINA, PINE, PINF; /** Initialise simulation. */ void diff --git a/digital/io-hub/src/robospierre/simu.host.h b/digital/io-hub/src/robospierre/simu.host.h index 4b461e86..ed6a002d 100644 --- a/digital/io-hub/src/robospierre/simu.host.h +++ b/digital/io-hub/src/robospierre/simu.host.h @@ -27,7 +27,7 @@ #ifdef HOST -extern uint8_t PINE; +extern uint8_t PINA, PINE, PINF; #else /* !defined (HOST) */ diff --git a/digital/io-hub/tools/io_hub/mex.py b/digital/io-hub/tools/io_hub/mex.py index 7c8c0012..d9568197 100644 --- a/digital/io-hub/tools/io_hub/mex.py +++ b/digital/io-hub/tools/io_hub/mex.py @@ -31,7 +31,7 @@ ADC_NB = 8 PWM_NB = 6 PWM_VALUE_MAX = 1024 -CONTACT_NB = 4 +CONTACT_NB = 9 CONTACT_INIT = 0xffffffff class Mex: diff --git a/host/simu/model/switch.py b/host/simu/model/switch.py index 30cc1466..6a06a954 100644 --- a/host/simu/model/switch.py +++ b/host/simu/model/switch.py @@ -26,13 +26,17 @@ from utils.observable import Observable class Switch (Observable): - def __init__ (self, link): + def __init__ (self, link, invert = False): Observable.__init__ (self) self.link = link self.state = None + self.invert = invert self.register (self.__update) def __update (self): - self.link.state = self.state + if not self.invert: + self.link.state = self.state + else: + self.link.state = not self.state self.link.notify () diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py index 0a89d00c..5a6fa023 100644 --- a/host/simu/robots/robospierre/model/bag.py +++ b/host/simu/robots/robospierre/model/bag.py @@ -32,8 +32,8 @@ from math import pi class Bag: def __init__ (self, scheduler, table, link_bag): - self.color_switch = Switch (link_bag.io_hub.contact[0]) - self.jack = Switch (link_bag.io_hub.contact[1]) + self.color_switch = Switch (link_bag.io_hub.contact[0], invert = True) + self.jack = Switch (link_bag.io_hub.contact[1], invert = True) self.contact = [ Switch (contact) for contact in link_bag.io_hub.contact[2:] ] self.position = Position (link_bag.asserv.position) @@ -42,7 +42,8 @@ class Bag: 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, self.door_motors) + link_bag.mimot.aux[1], self.clamping_motor, self.door_motors, + self.contact[0:7]) 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 9bb8bacc..e2592aa8 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -29,13 +29,19 @@ from math import pi, cos, sin class Slot: """Slot which can contain a pawn.""" - def __init__ (self, x, y, z, side, door_motor): + def __init__ (self, x, y, z, side, door_motor, contact): self.x = x self.y = y self.z = z self.side = side - self.pawn = None self.door_motor = door_motor + self.contact = contact + self.set_pawn (None) + + def set_pawn (self, pawn): + self.pawn = pawn + self.contact.state = pawn is None + self.contact.notify () class Clamp (Observable): @@ -60,7 +66,7 @@ class Clamp (Observable): SLOT_SIDE = 6 def __init__ (self, table, robot_position, elevation_motor, - rotation_motor, clamping_motor, door_motors): + rotation_motor, clamping_motor, door_motors, slot_contacts): Observable.__init__ (self) self.table = table self.robot_position = robot_position @@ -71,19 +77,19 @@ class Clamp (Observable): door_motors[2], None, door_motors[3], None) self.slots = ( Slot (self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 0, - door_motors[0]), + door_motors[0], slot_contacts[0]), Slot (self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 0, - None), + None, slot_contacts[1]), Slot (self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 0, - door_motors[1]), + door_motors[1], slot_contacts[2]), Slot (-self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 1, - door_motors[2]), + door_motors[2], slot_contacts[3]), Slot (-self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 1, - None), + None, slot_contacts[4]), Slot (-self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 1, - door_motors[3]), + door_motors[3], slot_contacts[5]), Slot (0, self.BAY_OFFSET, 2 * self.BAY_ZOFFSET, None, - None)) + None, slot_contacts[6])) self.load = None self.robot_position.register (self.__robot_position_notified) self.elevation_motor.register (self.__elevation_notified) @@ -98,7 +104,7 @@ class Clamp (Observable): if slot.pawn is None: p = self.__get_floor_elements (slot.side) if p is not None: - slot.pawn = p + slot.set_pawn (p) p.pos = None p.notify () changed = True @@ -133,14 +139,16 @@ class Clamp (Observable): if self.clamping == 0 and self.load is None: # Load an element. slot = self.__get_clamp_slot () - if slot: - self.load, slot.pawn = slot.pawn, self.load + if slot and slot.pawn is not None: + self.load = slot.pawn + slot.set_pawn (None) elif self.clamping == self.CLAMPING_STROKE \ and self.load is not None: # Unload an element. slot = self.__get_clamp_slot () if slot and slot.pawn is None: - self.load, slot.pawn = slot.pawn, self.load + slot.set_pawn (self.load) + self.load = None self.notify () def __get_floor_elements (self, side): -- cgit v1.2.3 From 73f25d3310706c5d7a559f120805c54860b16795 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 15 May 2011 20:14:12 +0200 Subject: host/simu/robots/robospierre: add tower management --- host/simu/robots/robospierre/model/clamp.py | 60 ++++++++++++++++++++++++----- host/simu/robots/robospierre/view/clamp.py | 31 +++++++++------ host/simu/robots/robospierre/view/robot.py | 4 +- host/simu/view/table_eurobot2011.py | 32 +++++++++------ 4 files changed, 92 insertions(+), 35 deletions(-) (limited to 'host/simu/robots/robospierre') diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index e2592aa8..976e559f 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -24,6 +24,7 @@ """Robospierre clamp.""" from utils.observable import Observable from simu.utils.trans_matrix import TransMatrix +from simu.model.round_obstacle import RoundObstacle from math import pi, cos, sin class Slot: @@ -36,12 +37,7 @@ class Slot: self.side = side self.door_motor = door_motor self.contact = contact - self.set_pawn (None) - - def set_pawn (self, pawn): - self.pawn = pawn - self.contact.state = pawn is None - self.contact.notify () + self.pawn = None class Clamp (Observable): @@ -90,6 +86,14 @@ class Clamp (Observable): door_motors[3], slot_contacts[5]), Slot (0, self.BAY_OFFSET, 2 * self.BAY_ZOFFSET, None, None, slot_contacts[6])) + self.front_slots = ( + self.slots[self.SLOT_FRONT_BOTTOM], + self.slots[self.SLOT_FRONT_MIDDLE], + self.slots[self.SLOT_FRONT_TOP]) + self.back_slots = ( + self.slots[self.SLOT_BACK_BOTTOM], + self.slots[self.SLOT_BACK_MIDDLE], + self.slots[self.SLOT_BACK_TOP]) self.load = None self.robot_position.register (self.__robot_position_notified) self.elevation_motor.register (self.__elevation_notified) @@ -104,11 +108,12 @@ class Clamp (Observable): if slot.pawn is None: p = self.__get_floor_elements (slot.side) if p is not None: - slot.set_pawn (p) + slot.pawn = p p.pos = None p.notify () changed = True if changed: + self.update_contacts () self.notify () def __elevation_notified (self): @@ -141,16 +146,53 @@ class Clamp (Observable): slot = self.__get_clamp_slot () if slot and slot.pawn is not None: self.load = slot.pawn - slot.set_pawn (None) + slot.pawn = None elif self.clamping == self.CLAMPING_STROKE \ and self.load is not None: # Unload an element. slot = self.__get_clamp_slot () if slot and slot.pawn is None: - slot.set_pawn (self.load) + slot.pawn = self.load self.load = None + # Little resources saving hack: done here, all motors are notified + # at the same time. + self.check_tower () + self.update_contacts () self.notify () + def check_tower (self): + """Check whether several elements can make a tower.""" + for slots in (self.front_slots, self.back_slots): + if slots[0].pawn is not None and slots[1].pawn is not None: + assert slots[0].pawn.kind != 'tower' + tower = RoundObstacle (100, 1) + tower.kind = 'tower' + tower.tower = [ slots[0].pawn, slots[1].pawn ] + slots[0].pawn, slots[1].pawn = tower, None + if slots[0].pawn is not None and slots[0].pawn.kind == 'tower' \ + and slots[2].pawn and slots[2].door_motor.angle: + slots[0].pawn.tower.append (slots[2].pawn) + slots[2].pawn = None + + def update_contacts (self): + """Update pawn contacts.""" + for slots in (self.front_slots, self.back_slots): + slots[0].contact.state = not (slots[0].pawn is not None) + # A tower at level 0 is seen at level 1. + slots[1].contact.state = not ( + slots[1].pawn is not None + or (slots[0].pawn is not None + and slots[0].pawn.kind == 'tower')) + # This one is really high. + slots[2].contact.state = not (slots[2].pawn is not None) + slot_side = self.slots[self.SLOT_SIDE] + slot_side.contact.state = slot_side.pawn is None + clamp_slot = self.__get_clamp_slot () + if clamp_slot is not None: + clamp_slot.contact.state = False + for slot in self.slots: + slot.contact.notify () + def __get_floor_elements (self, side): """Return an elements in front (side = 0) or in back (side = 1) of the robot, on the floor.""" diff --git a/host/simu/robots/robospierre/view/clamp.py b/host/simu/robots/robospierre/view/clamp.py index 130c3eea..07a10929 100644 --- a/host/simu/robots/robospierre/view/clamp.py +++ b/host/simu/robots/robospierre/view/clamp.py @@ -68,7 +68,7 @@ class ClampTop (Drawable): self.trans_push () self.trans_scale (1 - slot.z / 1000.0) self.trans_translate ((slot.x, slot.y)) - draw_pawn (self, slot.pawn.radius, slot.pawn.kind) + draw_pawn (self, slot.pawn) self.trans_pop () # Draw clamp. if self.model.rotation is not None: @@ -82,7 +82,7 @@ class ClampTop (Drawable): if load is not None: self.trans_push () self.trans_translate ((150, 0)) - draw_pawn (self, load.radius, load.kind) + draw_pawn (self, load) self.trans_pop () # Mobile side. self.trans_rotate (-self.model.clamping / 43) @@ -107,16 +107,23 @@ class ClampSide (Drawable): if pawn is not None: self.trans_push () self.trans_translate (pos) - self.draw_rectangle ((-100, 0), (100, 50), fill = YELLOW) - if pawn.kind == 'king': - self.draw_polygon ((-50, 50), (-10, 170), (-50, 170), (-50, 190), - (-10, 190), (-10, 230), (10, 230), (10, 190), (50, 190), - (50, 170), (5, 170), (50, 50), fill = YELLOW, - outline = BLACK) - elif pawn.kind == 'queen': - self.draw_polygon ((-50, 50), (-10, 180), (10, 180), (50, 50), - fill = YELLOW, outline = BLACK) - self.draw_circle ((0, 180), 50, fill = YELLOW) + if pawn.kind == 'tower': + pawns = pawn.tower + else: + pawns = (pawn, ) + for p in pawns: + self.draw_rectangle ((-100, 0), (100, 50), fill = YELLOW) + if p.kind == 'king': + self.draw_polygon ((-50, 50), (-10, 170), (-50, 170), + (-50, 190), (-10, 190), (-10, 230), (10, 230), + (10, 190), (50, 190), (50, 170), (5, 170), + (50, 50), fill = YELLOW, + outline = BLACK) + elif p.kind == 'queen': + self.draw_polygon ((-50, 50), (-10, 180), (10, 180), + (50, 50), fill = YELLOW, outline = BLACK) + self.draw_circle ((0, 180), 50, fill = YELLOW) + self.trans_translate ((0, 50)) self.trans_pop () def draw (self): diff --git a/host/simu/robots/robospierre/view/robot.py b/host/simu/robots/robospierre/view/robot.py index c0f6624c..90624000 100644 --- a/host/simu/robots/robospierre/view/robot.py +++ b/host/simu/robots/robospierre/view/robot.py @@ -63,7 +63,7 @@ class Robot (simu.inter.drawable.Drawable): self.trans_push () self.trans_scale (1 - slot.z / 1000.0) self.trans_translate ((slot.x, slot.y)) - draw_pawn (self, slot.pawn.radius, slot.pawn.kind) + draw_pawn (self, slot.pawn) self.trans_pop () # Draw clamp. if self.clamp_model.rotation is not None: @@ -77,7 +77,7 @@ class Robot (simu.inter.drawable.Drawable): if load: self.trans_push () self.trans_translate ((150, 0)) - draw_pawn (self, load.radius, load.kind) + draw_pawn (self, load) self.trans_pop () # Mobile side. self.trans_rotate (- self.clamp_model.clamping / 47) diff --git a/host/simu/view/table_eurobot2011.py b/host/simu/view/table_eurobot2011.py index f0a65ce1..5725e5b8 100644 --- a/host/simu/view/table_eurobot2011.py +++ b/host/simu/view/table_eurobot2011.py @@ -31,17 +31,25 @@ GREEN = '#268126' BLACK = '#181818' YELLOW = '#cccc00' -def draw_pawn (d, radius, kind): - d.draw_circle ((0, 0), radius, fill = YELLOW) - if kind == 'king': - a = 0.1 * radius - b = 0.5 * radius - d.draw_line ((a, b), (a, a), (b, a), (b, -a), (a, -a), (a, -b), - (-a, -b), (-a, -a), (-b, -a), (-b, a), (-a, a), (-a, b), - (a, b)) - elif kind == 'queen': - d.draw_circle ((0, 0), 0.5 * radius) - d.draw_circle ((0, 0), 0.4 * radius) +def draw_pawn (d, pawn): + d.trans_push () + if pawn.kind == 'tower': + pawns = pawn.tower + else: + pawns = (pawn, ) + for p in pawns: + d.draw_circle ((0, 0), p.radius, fill = YELLOW) + if p.kind == 'king': + a = 0.1 * p.radius + b = 0.5 * p.radius + d.draw_line ((a, b), (a, a), (b, a), (b, -a), (a, -a), (a, -b), + (-a, -b), (-a, -a), (-b, -a), (-b, a), (-a, a), (-a, b), + (a, b)) + elif p.kind == 'queen': + d.draw_circle ((0, 0), 0.5 * p.radius) + d.draw_circle ((0, 0), 0.4 * p.radius) + d.trans_scale (0.95) + d.trans_pop () class Pawn (Drawable): @@ -59,7 +67,7 @@ class Pawn (Drawable): self.reset () if self.pos: self.trans_translate (self.pos) - draw_pawn (self, self.model.radius, self.model.kind) + draw_pawn (self, self.model) Drawable.draw (self) class Table (Drawable): -- cgit v1.2.3 From 201aa07cbd28c6be8c4003f126b1ae258f859c68 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 15 May 2011 22:44:20 +0200 Subject: host/simu: handle dropping tower on the table --- host/simu/model/table.py | 4 +- host/simu/model/table_eurobot2011.py | 6 +++ host/simu/robots/robospierre/model/clamp.py | 64 ++++++++++++++++++++++++----- host/simu/view/table_eurobot2011.py | 9 +++- 4 files changed, 70 insertions(+), 13 deletions(-) (limited to 'host/simu/robots/robospierre') diff --git a/host/simu/model/table.py b/host/simu/model/table.py index 41767ac4..d79f7758 100644 --- a/host/simu/model/table.py +++ b/host/simu/model/table.py @@ -22,6 +22,7 @@ # # }}} """Table model.""" +from utils.observable import Observable class Intersect: @@ -29,9 +30,10 @@ class Intersect: self.obstacle = obstacle self.distance = distance -class Table: +class Table (Observable): def __init__ (self): + Observable.__init__ (self) self.obstacles = [ ] def intersect (self, a, b, level = None, comp = None): diff --git a/host/simu/model/table_eurobot2011.py b/host/simu/model/table_eurobot2011.py index c9c1d193..f4bb63b7 100644 --- a/host/simu/model/table_eurobot2011.py +++ b/host/simu/model/table_eurobot2011.py @@ -81,3 +81,9 @@ class Table (simu.model.table.Table): self.pawns.append (pawn) # Add everything to obstacles. self.obstacles += self.pawns + + def add_pawn (self, pawn): + self.pawns.append (pawn) + self.obstacles.append (pawn) + self.notify () + diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index 976e559f..fd3dde7c 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -101,16 +101,31 @@ class Clamp (Observable): self.clamping_motor.register (self.__clamping_notified) def __robot_position_notified (self): + # Compute robot direction. + direction = self.__get_robot_direction () # Update bottom slots. changed = False for sloti in (self.SLOT_FRONT_BOTTOM, self.SLOT_BACK_BOTTOM): slot = self.slots[sloti] - if slot.pawn is None: - p = self.__get_floor_elements (slot.side) - if p is not None: - slot.pawn = p - p.pos = None - p.notify () + if direction == slot.side or direction is None: + # If pushing, can take new elements. + if slot.pawn is None: + p = self.__get_floor_elements (slot.side) + if p is not None: + slot.pawn = p + p.pos = None + p.notify () + changed = True + else: + # Else, can drop elements. + if slot.pawn is not None and slot.door_motor.angle: + m = TransMatrix () + m.translate (self.robot_position.pos) + m.rotate (self.robot_position.angle) + xoffset = (self.BAY_OFFSET, -self.BAY_OFFSET)[slot.side] + slot.pawn.pos = m.apply ((xoffset, 0)) + slot.pawn.notify () + slot.pawn = None changed = True if changed: self.update_contacts () @@ -169,6 +184,7 @@ class Clamp (Observable): tower.kind = 'tower' tower.tower = [ slots[0].pawn, slots[1].pawn ] slots[0].pawn, slots[1].pawn = tower, None + self.table.add_pawn (tower) if slots[0].pawn is not None and slots[0].pawn.kind == 'tower' \ and slots[2].pawn and slots[2].door_motor.angle: slots[0].pawn.tower.append (slots[2].pawn) @@ -199,10 +215,7 @@ class Clamp (Observable): if self.robot_position.pos is None: return None # Matrix to transform an obstacle position into robot coordinates. - m = TransMatrix () - m.rotate (-self.robot_position.angle) - m.translate ((-self.robot_position.pos[0], - -self.robot_position.pos[1])) + m = self.__get_robot_matrix () # Look up elements. xoffset = (self.BAY_OFFSET, -self.BAY_OFFSET)[side] xmargin = 20 @@ -230,3 +243,34 @@ class Clamp (Observable): return slot return None + def __get_robot_direction (self): + """Is robot going forward (0), backward (1), or something else + (None)?""" + if self.robot_position.pos is None: + return None + filt = 5 + if hasattr (self, 'old_robot_position'): + m = self.__get_robot_matrix () + oldrel = m.apply (self.old_robot_position) + if oldrel[0] < 0 and self.direction_counter < filt: + self.direction_counter += 1 + elif oldrel[0] > 0 and self.direction_counter > -filt: + self.direction_counter -= 1 + else: + self.direction_counter = 0 + self.old_robot_position = self.robot_position.pos + # Filter oscillations. + if self.direction_counter > 0: + return 0 + elif self.direction_counter < 0: + return 1 + else: + return None + + def __get_robot_matrix (self): + """Return robot transformation matrix.""" + m = TransMatrix () + m.rotate (-self.robot_position.angle) + m.translate ((-self.robot_position.pos[0], + -self.robot_position.pos[1])) + return m diff --git a/host/simu/view/table_eurobot2011.py b/host/simu/view/table_eurobot2011.py index 5725e5b8..b39f2834 100644 --- a/host/simu/view/table_eurobot2011.py +++ b/host/simu/view/table_eurobot2011.py @@ -76,8 +76,13 @@ class Table (Drawable): def __init__ (self, onto, model): Drawable.__init__ (self, onto) self.model = model - for e in model.pawns: - Pawn (self, e) + self.model.register (self.__notified) + self.__notified () + + def __notified (self): + for e in self.model.pawns: + if e not in self.children: + Pawn (self, e) def draw_both (self, primitive, *args, **kargs): """Draw a primitive on both sides.""" -- cgit v1.2.3 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(-) (limited to 'host/simu/robots/robospierre') 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 From c5b1353dea8a426d6eea043fb5aa78722f7b0bc5 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 23 May 2011 00:13:14 +0200 Subject: digital/io-hub: add US distance sensors --- digital/io-hub/src/robospierre/Makefile | 1 + digital/io-hub/src/robospierre/avrconfig.h | 12 ++++++++++++ digital/io-hub/src/robospierre/main.c | 20 ++++++++++++++++++++ digital/io-hub/src/robospierre/simu.host.c | 14 +++++++++++++- digital/io-hub/src/robospierre/simu.host.h | 2 +- digital/io-hub/tools/io_hub/mex.py | 20 ++++++++++++++++---- host/simu/model/distance_sensor_sensopart.py | 2 +- host/simu/robots/robospierre/model/bag.py | 2 -- 8 files changed, 64 insertions(+), 9 deletions(-) (limited to 'host/simu/robots/robospierre') diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index d69918f5..bd8e920f 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -12,6 +12,7 @@ io_hub_SOURCES = main.c \ chrono.c timer.avr.c simu.host.c # Modules needed for IO. MODULES = proto uart twi utils \ + adc devices/usdist \ math/fixed math/geometry AI_MODULES = twi_master common utils fsm # Configuration file. diff --git a/digital/io-hub/src/robospierre/avrconfig.h b/digital/io-hub/src/robospierre/avrconfig.h index 0fc2f11d..53e36ccf 100644 --- a/digital/io-hub/src/robospierre/avrconfig.h +++ b/digital/io-hub/src/robospierre/avrconfig.h @@ -100,6 +100,18 @@ /** Use internal pull up. */ #define AC_TWI_PULL_UP 0 +/* usdist - Analog US distance sensor. */ +/** Number of sensors. */ +#define AC_USDIST_NB 4 +/** Measuring period, in number of update call. */ +#define AC_USDIST_PERIOD 1 +/** List of space separated sensor definition, see usdist.h. */ +#define AC_USDIST_SENSORS \ + USDIST_SENSOR (0, A, 0) \ + USDIST_SENSOR (1, A, 1) \ + USDIST_SENSOR (2, A, 2) \ + USDIST_SENSOR (3, A, 3) + /* io-hub - io/ai board. */ /** TWI address of the io board. */ #define AC_IO_TWI_ADDRESS 10 diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 4e75a92e..8d51c469 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -28,6 +28,8 @@ #include "modules/uart/uart.h" #include "modules/proto/proto.h" +#include "modules/devices/usdist/usdist.h" + #include "timer.h" #include "chrono.h" #include "simu.host.h" @@ -66,6 +68,9 @@ static uint8_t main_stats_asserv_, main_stats_asserv_cpt_; /** Contact stats counters. */ static uint8_t main_stats_contact_, main_stats_contact_cpt_; +/** US sensors stats counters. */ +static uint8_t main_stats_usdist_, main_stats_usdist_cpt_; + /** Main initialisation. */ static void main_init (void) @@ -89,6 +94,7 @@ main_init (void) /* IO modules. */ pwm_init (); contact_init (); + usdist_init (); /* AI modules. */ logistic_init (); /* Initialization done. */ @@ -170,6 +176,10 @@ main_loop (void) /* Update IO modules. */ pwm_update (); contact_update (); + if (usdist_update ()) + { + /* TODO: update radar. */ + } /* Update AI modules. */ logistic_update (); /* Only manage events if slaves are synchronised. */ @@ -191,6 +201,12 @@ main_loop (void) proto_send1d ('P', contact_all ()); main_stats_contact_cpt_ = main_stats_contact_; } + if (main_stats_usdist_ && !--main_stats_usdist_cpt_) + { + proto_send4w ('U', usdist_mm[0], usdist_mm[1], usdist_mm[2], + usdist_mm[3]); + main_stats_usdist_cpt_ = main_stats_usdist_; + } } } @@ -269,6 +285,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Contact stats. */ main_stats_contact_ = main_stats_contact_cpt_ = args[0]; break; + case c ('U', 1): + /* US sensors stats. */ + main_stats_usdist_ = main_stats_usdist_cpt_ = args[0]; + break; default: /* Unknown commands */ proto_send0 ('?'); diff --git a/digital/io-hub/src/robospierre/simu.host.c b/digital/io-hub/src/robospierre/simu.host.c index 281a60d6..f7a43eea 100644 --- a/digital/io-hub/src/robospierre/simu.host.c +++ b/digital/io-hub/src/robospierre/simu.host.c @@ -28,10 +28,20 @@ #include "modules/utils/utils.h" #include "modules/host/host.h" #include "modules/host/mex.h" +#include "modules/adc/adc.h" #include "io.h" /** AVR registers. */ -uint8_t PINA, PINE, PINF; +uint8_t PORTA, DDRA, PINA, PINE, PINF; + +static void +simu_adc_handle (void *user, mex_msg_t *msg) +{ + uint8_t index; + uint16_t value; + mex_msg_pop (msg, "BH", &index, &value); + adc_values[index] = value; +} /** Initialise simulation. */ void @@ -40,6 +50,8 @@ simu_init (void) const char *mex_instance; mex_node_connect (); mex_instance = host_get_instance ("io-hub0", 0); + uint8_t mtype = mex_node_reservef ("%s:adc", mex_instance); + mex_node_register (mtype, simu_adc_handle, 0); } /** Make a simulation step. */ diff --git a/digital/io-hub/src/robospierre/simu.host.h b/digital/io-hub/src/robospierre/simu.host.h index ed6a002d..6a636851 100644 --- a/digital/io-hub/src/robospierre/simu.host.h +++ b/digital/io-hub/src/robospierre/simu.host.h @@ -27,7 +27,7 @@ #ifdef HOST -extern uint8_t PINA, PINE, PINF; +extern uint8_t PORTA, DDRA, PINA, PINE, PINF; #else /* !defined (HOST) */ diff --git a/digital/io-hub/tools/io_hub/mex.py b/digital/io-hub/tools/io_hub/mex.py index d9568197..22e9e900 100644 --- a/digital/io-hub/tools/io_hub/mex.py +++ b/digital/io-hub/tools/io_hub/mex.py @@ -44,9 +44,21 @@ class Mex: """ - def __init__ (self): + def __init__ (self, node, instance, index): Observable.__init__ (self) - self.value = None + self.value = 0 + self.__node = node + self.__mtype = node.reserve (instance + ':adc') + self.__index = index + self.register (self.__notified) + + def __notified (self): + m = simu.mex.msg.Msg (self.__mtype) + v = int (1024 * self.value / 5) + v = min (v, 1023) + v = max (0, v) + m.push ('BH', self.__index, v) + self.__node.send (m) class PWM (Observable): """PWM output. @@ -110,9 +122,9 @@ class Mex: self.node.send (m) def __init__ (self, node, instance = 'io-hub0'): - self.adc = tuple (self.ADC () for i in range (0, ADC_NB)) + self.adc = tuple (self.ADC (node, instance, i) for i in range (0, ADC_NB)) self.pwm = tuple (self.PWM () for i in range (0, PWM_NB)) - self.__adc_pwm = self.PWM.Pack (node, instance, self.pwm) + self.__pwm_pack = self.PWM.Pack (node, instance, self.pwm) self.__contact_pack = self.Contact.Pack (node, instance) self.contact = tuple (self.Contact (self.__contact_pack, i) for i in range (CONTACT_NB)) diff --git a/host/simu/model/distance_sensor_sensopart.py b/host/simu/model/distance_sensor_sensopart.py index 16676e5d..26ed604f 100644 --- a/host/simu/model/distance_sensor_sensopart.py +++ b/host/simu/model/distance_sensor_sensopart.py @@ -59,8 +59,8 @@ class DistanceSensorSensopart (Observable): self.link = link self.scheduler = scheduler self.value = None - self.register (self.__update) self.evaluate () + self.register (self.__update) def evaluate (self): # Compute real distance. diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py index b1c49d35..de8eaa22 100644 --- a/host/simu/robots/robospierre/model/bag.py +++ b/host/simu/robots/robospierre/model/bag.py @@ -54,6 +54,4 @@ class Bag: DistanceSensorSensopart (link_bag.io_hub.adc[3], scheduler, table, (-20, 20), pi - pi * 10 / 180, (self.position, ), 2), ] - for adc in link_bag.io_hub.adc[4:]: - adc.value = 0 -- cgit v1.2.3 From 6548d83aa655176ef939817e36cd05d0e6cfaccd Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 23 May 2011 01:23:43 +0200 Subject: digital/{io,io-hub,ai}: add generic radar, add radar to robospierre --- digital/ai/src/move/radar.c | 168 +++++++++++++++++++++++++ digital/ai/src/move/radar.h | 96 ++++++++++++++ digital/io-hub/src/robospierre/Makefile | 3 +- digital/io-hub/src/robospierre/bot.h | 3 + digital/io-hub/src/robospierre/main.c | 13 +- digital/io-hub/src/robospierre/radar_defs.c | 46 +++++++ digital/io-hub/src/robospierre/radar_defs.h | 41 ++++++ digital/io-hub/src/robospierre/simu.host.c | 15 +++ digital/io-hub/src/robospierre/simu.host.h | 10 ++ digital/io-hub/tools/io_hub/mex.py | 22 ++++ digital/io/src/Makefile | 4 +- digital/io/src/radar.c | 189 ---------------------------- digital/io/src/radar.h | 78 ------------ digital/io/src/radar_defs.c | 54 ++++++++ digital/io/src/radar_defs.h | 41 ++++++ host/simu/robots/robospierre/model/bag.py | 1 + host/simu/robots/robospierre/view/bag.py | 1 + 17 files changed, 514 insertions(+), 271 deletions(-) create mode 100644 digital/ai/src/move/radar.c create mode 100644 digital/ai/src/move/radar.h create mode 100644 digital/io-hub/src/robospierre/radar_defs.c create mode 100644 digital/io-hub/src/robospierre/radar_defs.h delete mode 100644 digital/io/src/radar.c delete mode 100644 digital/io/src/radar.h create mode 100644 digital/io/src/radar_defs.c create mode 100644 digital/io/src/radar_defs.h (limited to 'host/simu/robots/robospierre') diff --git a/digital/ai/src/move/radar.c b/digital/ai/src/move/radar.c new file mode 100644 index 00000000..a572afba --- /dev/null +++ b/digital/ai/src/move/radar.c @@ -0,0 +1,168 @@ +/* radar.c */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2010 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "radar.h" + +#include "bot.h" + +#include "modules/math/geometry/geometry.h" +#include "modules/math/geometry/distance.h" +#include "modules/utils/utils.h" + +/** Maximum distance for a sensor reading to be ignored if another sensor is + * nearer. */ +#define RADAR_FAR_MM 250 + +/** Define radar configuration. */ +extern struct radar_sensor_t radar_sensors[RADAR_SENSOR_NB]; + +uint8_t +radar_valid (vect_t p); + +/** Compute the center position from several radars sensors, return 1 if + * any. */ +static uint8_t +radar_hit_center (uint8_t valid[], vect_t hit[], uint8_t sensor_nb, + vect_t *obs_pos) +{ + uint8_t i, hit_nb = 0; + vect_t hit_center = { 0, 0 }; + for (i = 0; i < sensor_nb; i++) + { + if (valid[i]) + { + vect_add (&hit_center, &hit[i]); + hit_nb++; + } + } + if (hit_nb > 1) + vect_scale_f824 (&hit_center, 0x1000000l / hit_nb); + if (hit_nb) + { + *obs_pos = hit_center; + return 1; + } + else + return 0; +} + +uint8_t +radar_update (const position_t *robot_pos, vect_t *obs_pos) +{ + uint8_t i, j; + vect_t ray; + uint8_t obs_nb = 0; + /* Compute hit points for each sensor and eliminate invalid ones. */ + vect_t hit[UTILS_COUNT (radar_sensors)]; + uint8_t valid[UTILS_COUNT (radar_sensors)]; + uint16_t dist_mm[UTILS_COUNT (radar_sensors)]; + for (i = 0; i < UTILS_COUNT (radar_sensors); i++) + { + dist_mm[i] = *radar_sensors[i].dist_mm; + if (dist_mm[i] != 0xffff) + { + hit[i] = radar_sensors[i].pos; + vect_rotate_uf016 (&hit[i], robot_pos->a); + vect_translate (&hit[i], &robot_pos->v); + vect_from_polar_uf016 (&ray, dist_mm[i], + robot_pos->a + radar_sensors[i].a); + vect_translate (&hit[i], &ray); + valid[i] = radar_valid (hit[i]); + vect_from_polar_uf016 (&ray, RADAR_OBSTACLE_EDGE_RADIUS_MM, + robot_pos->a + radar_sensors[i].a); + vect_translate (&hit[i], &ray); + } + else + valid[i] = 0; + } + /* Ignore sensor results too far from other sensors. */ + for (i = 0; i < UTILS_COUNT (radar_sensors) - 1; i++) + { + for (j = i + 1; valid[i] && j < UTILS_COUNT (radar_sensors); j++) + { + if (valid[j]) + { + if (dist_mm[i] + RADAR_FAR_MM < dist_mm[j]) + valid[j] = 0; + else if (dist_mm[j] + RADAR_FAR_MM < dist_mm[i]) + valid[i] = 0; + } + } + } + /* Specific treatment about sensor topology. */ + obs_nb += radar_hit_center (valid + RADAR_SENSOR_FRONT_FIRST, + hit + RADAR_SENSOR_FRONT_FIRST, + RADAR_SENSOR_FRONT_NB, &obs_pos[obs_nb]); + obs_nb += radar_hit_center (valid + RADAR_SENSOR_BACK_FIRST, + hit + RADAR_SENSOR_BACK_FIRST, + RADAR_SENSOR_BACK_NB, &obs_pos[obs_nb]); + /* Done. */ + return obs_nb; +} + +uint8_t +radar_blocking (const vect_t *robot_pos, const vect_t *dest_pos, + const vect_t *obs_pos, uint8_t obs_nb) +{ + uint8_t i; + /* Stop here if no obstacle. */ + if (!obs_nb) + return 0; + vect_t vd = *dest_pos; vect_sub (&vd, robot_pos); + uint16_t d = vect_norm (&vd); + /* If destination is realy near, stop here. */ + if (d < RADAR_EPSILON_MM) + return 0; + /* If destination is near, use clearance to destination point instead of + * stop length. */ + vect_t t; + if (d < RADAR_STOP_MM) + t = *dest_pos; + else + { + vect_scale_f824 (&vd, (1ll << 24) / d * RADAR_STOP_MM); + t = *robot_pos; + vect_translate (&t, &vd); + } + /* Now, look at obstacles. */ + for (i = 0; i < obs_nb; i++) + { + /* Vector from robot to obstacle. */ + vect_t vo = obs_pos[i]; vect_sub (&vo, robot_pos); + /* Ignore if in our back. */ + int32_t dp = vect_dot_product (&vd, &vo); + if (dp < 0) + continue; + /* Check distance. */ + int16_t od = distance_segment_point (robot_pos, &t, &obs_pos[i]); + if (od > BOT_SIZE_SIDE + RADAR_CLEARANCE_MM / 2 + + RADAR_OBSTACLE_RADIUS_MM) + continue; + /* Else, obstacle is blocking. */ + return 1; + } + return 0; +} + diff --git a/digital/ai/src/move/radar.h b/digital/ai/src/move/radar.h new file mode 100644 index 00000000..2e37d11d --- /dev/null +++ b/digital/ai/src/move/radar.h @@ -0,0 +1,96 @@ +#ifndef radar_h +#define radar_h +/* radar.h */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2010 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "defs.h" + +/** + * Handle any distance sensors information to extract useful data. This + * includes: + * - combining several sensors information for a more precise obstacle + * position, + * - ignoring obstacles not in the playground, + * - determining if an obstacle should make the robot stop. + */ + +/** Estimated obstacle edge radius. As the sensors detect obstacle edge, this is + * added to position obstacle center. */ +#undef RADAR_OBSTACLE_EDGE_RADIUS_MM + +/** Estimated obstacle radius. The obstacle may be larger than at the + * detected edge. */ +#undef RADAR_OBSTACLE_RADIUS_MM + +/** Stop distance. Distance under which an obstacle is considered harmful when + * moving. */ +#undef RADAR_STOP_MM + +/** Clearance distance. Distance over which an obstacle should be to the side + * when moving. + * + * OK, more explanations: when moving, a rectangle is placed in front of the + * robot, of length RADAR_STOP_MM and width 2 * (RADAR_CLEARANCE_MM + + * BOT_SIZE_SIDE). If an obstacle is inside this rectangle, it is considered + * in the way. + * + * If the destination point is near (< RADAR_STOP_MM - RADAR_CLEARANCE_MM), + * this reduce the rectangle length. + * + * If the destination is really near (< RADAR_EPSILON_MM), ignore all this. */ +#undef RADAR_CLEARANCE_MM + +/** Destination distance near enough so that obstacles could be ignored. */ +#undef RADAR_EPSILON_MM + +/* All this in another file. */ +#include "radar_defs.h" + +/** Margin to be considered inside the playground. An obstacle can not be + * exactly at the playground edge. */ +#define RADAR_MARGIN_MM 150 + +/** Describe a radar sensor. */ +struct radar_sensor_t +{ + /** Distance updated by another module. */ + uint16_t *dist_mm; + /** Position relative to the robot center. */ + vect_t pos; + /** Angle relative to the robot X axis. */ + uint16_t a; +}; + +/** Update radar view. Return the number of obstacles found. Obstacles + * positions are returned in obs_pos. */ +uint8_t +radar_update (const position_t *robot_pos, vect_t *obs_pos); + +/** Return non zero if there is a blocking obstacle near the robot while going + * to a destination point. */ +uint8_t +radar_blocking (const vect_t *robot_pos, const vect_t *dest_pos, + const vect_t *obs_pos, uint8_t obs_nb); + +#endif /* radar_h */ diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index bd8e920f..fe2b318e 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -5,6 +5,7 @@ PROGS = io_hub # Sources to compile. io_hub_SOURCES = main.c \ clamp.c logistic.c \ + radar_defs.c radar.c \ init.c fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \ pwm.avr.c pwm.host.c \ contact.avr.c contact.host.c \ @@ -14,7 +15,7 @@ io_hub_SOURCES = main.c \ MODULES = proto uart twi utils \ adc devices/usdist \ math/fixed math/geometry -AI_MODULES = twi_master common utils fsm +AI_MODULES = twi_master common utils fsm move # Configuration file. CONFIGFILE = avrconfig.h AVR_MCU = at90usb1287 diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 5f9d177a..4df9b89d 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -34,6 +34,9 @@ # define BOT_SCALE 0.0415178942124 #endif +/** Distance from the robot axis to the side. */ +#define BOT_SIZE_SIDE 190 + /** Distance between the front contact point and the robot center. */ #define BOT_FRONT_CONTACT_DIST_MM 150 /** Angle error at the front contact point. */ diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 8d51c469..6d32c28f 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -40,6 +40,7 @@ #include "pwm.h" #include "contact.h" +#include "radar.h" #define FSM_NAME AI #include "fsm.h" @@ -62,6 +63,12 @@ /** Our color. */ enum team_color_e team_color; +/** Obstacles positions, updated using radar module. */ +vect_t main_obstacles_pos[2]; + +/** Number of obstacles in main_obstacles_pos. */ +uint8_t main_obstacles_nb; + /** Asserv stats counters. */ static uint8_t main_stats_asserv_, main_stats_asserv_cpt_; @@ -178,7 +185,11 @@ main_loop (void) contact_update (); if (usdist_update ()) { - /* TODO: update radar. */ + position_t robot_pos; + asserv_get_position (&robot_pos); + main_obstacles_nb = radar_update (&robot_pos, main_obstacles_pos); + //move_obstacles_update (); + simu_send_pos_report (main_obstacles_pos, main_obstacles_nb, 0); } /* Update AI modules. */ logistic_update (); diff --git a/digital/io-hub/src/robospierre/radar_defs.c b/digital/io-hub/src/robospierre/radar_defs.c new file mode 100644 index 00000000..85f015e1 --- /dev/null +++ b/digital/io-hub/src/robospierre/radar_defs.c @@ -0,0 +1,46 @@ +/* radar_defs.c */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2010 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "radar.h" + +#include "modules/devices/usdist/usdist.h" +#include "playground.h" + +/** Define radar configuration. */ +struct radar_sensor_t radar_sensors[RADAR_SENSOR_NB] = { + { &usdist_mm[0], { 20, 20 }, G_ANGLE_UF016_DEG (10) }, + { &usdist_mm[1], { 20, -20 }, G_ANGLE_UF016_DEG (-10) }, + { &usdist_mm[2], { -20, -20 }, G_ANGLE_UF016_DEG (180 + 10) }, + { &usdist_mm[3], { -20, 20 }, G_ANGLE_UF016_DEG (180 - 10) }, +}; + +/** Define exclusion area (considered as invalid point). */ +uint8_t +radar_valid (vect_t p) +{ + return p.x >= RADAR_MARGIN_MM && p.x < PG_WIDTH - RADAR_MARGIN_MM + && p.y >= RADAR_MARGIN_MM && p.y < PG_LENGTH - RADAR_MARGIN_MM; +} + diff --git a/digital/io-hub/src/robospierre/radar_defs.h b/digital/io-hub/src/robospierre/radar_defs.h new file mode 100644 index 00000000..5f6a2a73 --- /dev/null +++ b/digital/io-hub/src/robospierre/radar_defs.h @@ -0,0 +1,41 @@ +#ifndef radar_defs_h +#define radar_defs_h +/* radar_defs.h */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2010 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +#define RADAR_OBSTACLE_EDGE_RADIUS_MM 40 +#define RADAR_OBSTACLE_RADIUS_MM 150 +#define RADAR_STOP_MM 350 +#define RADAR_CLEARANCE_MM 100 +#define RADAR_EPSILON_MM 70 + +#define RADAR_SENSOR_NB 4 + +#define RADAR_SENSOR_FRONT_FIRST 0 +#define RADAR_SENSOR_FRONT_NB 2 +#define RADAR_SENSOR_BACK_FIRST 2 +#define RADAR_SENSOR_BACK_NB 2 + +#endif /* radar_defs_h */ diff --git a/digital/io-hub/src/robospierre/simu.host.c b/digital/io-hub/src/robospierre/simu.host.c index f7a43eea..7ca7364b 100644 --- a/digital/io-hub/src/robospierre/simu.host.c +++ b/digital/io-hub/src/robospierre/simu.host.c @@ -34,6 +34,9 @@ /** AVR registers. */ uint8_t PORTA, DDRA, PINA, PINE, PINF; +/** Message types. */ +uint8_t simu_mex_pos_report; + static void simu_adc_handle (void *user, mex_msg_t *msg) { @@ -52,6 +55,7 @@ simu_init (void) mex_instance = host_get_instance ("io-hub0", 0); uint8_t mtype = mex_node_reservef ("%s:adc", mex_instance); mex_node_register (mtype, simu_adc_handle, 0); + simu_mex_pos_report = mex_node_reservef ("%s:pos-report", mex_instance); } /** Make a simulation step. */ @@ -74,3 +78,14 @@ timer_wait (void) return 0; } +void +simu_send_pos_report (vect_t *pos, uint8_t pos_nb, uint8_t id) +{ + mex_msg_t *m; + m = mex_msg_new (simu_mex_pos_report); + mex_msg_push (m, "b", id); + for (; pos_nb; pos++, pos_nb--) + mex_msg_push (m, "hh", pos->x, pos->y); + mex_node_send (m); +} + diff --git a/digital/io-hub/src/robospierre/simu.host.h b/digital/io-hub/src/robospierre/simu.host.h index 6a636851..6224e345 100644 --- a/digital/io-hub/src/robospierre/simu.host.h +++ b/digital/io-hub/src/robospierre/simu.host.h @@ -24,13 +24,23 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ +#include "defs.h" #ifdef HOST extern uint8_t PORTA, DDRA, PINA, PINE, PINF; +/** Send general purpose positions to indicate computation results. + * - pos: array of positions to report. + * - pos_nb: number of elements in the array. + * - id: identifier so that several unrelated positions could be reported. */ +void +simu_send_pos_report (vect_t *pos, uint8_t pos_nb, uint8_t id); + #else /* !defined (HOST) */ +#define simu_send_pos_report(pos, pos_nb, id) ((void) 0) + #endif /* !defined (HOST) */ #endif /* simu_host_h */ diff --git a/digital/io-hub/tools/io_hub/mex.py b/digital/io-hub/tools/io_hub/mex.py index 22e9e900..44c41010 100644 --- a/digital/io-hub/tools/io_hub/mex.py +++ b/digital/io-hub/tools/io_hub/mex.py @@ -121,6 +121,27 @@ class Mex: m.push ('L', self.contacts) self.node.send (m) + class PosReport (Observable): + """General purpose position report. + + - pos: dict of sequence of (x, y) coordinates (millimeters). The dict + is indexed by position identifier. + + """ + + def __init__ (self, node, instance): + Observable.__init__ (self) + self.pos = { } + node.register (instance + ':pos-report', self.__handle) + + def __handle (self, msg): + p = [ ] + id, = msg.pop ('b') + while len (msg) >= 4: + p.append (msg.pop ('hh')) + self.pos[id] = p + self.notify () + def __init__ (self, node, instance = 'io-hub0'): self.adc = tuple (self.ADC (node, instance, i) for i in range (0, ADC_NB)) self.pwm = tuple (self.PWM () for i in range (0, PWM_NB)) @@ -128,4 +149,5 @@ class Mex: self.__contact_pack = self.Contact.Pack (node, instance) self.contact = tuple (self.Contact (self.__contact_pack, i) for i in range (CONTACT_NB)) + self.pos_report = self.PosReport (node, instance) diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile index effe9c45..463e148e 100644 --- a/digital/io/src/Makefile +++ b/digital/io/src/Makefile @@ -6,14 +6,14 @@ PROGS = io io_SOURCES = main.c fsm_queue.c servo.avr.c eeprom.avr.c pwm.c \ switch.avr.c chrono.c timer.avr.c servo_pos.c \ twi_master.c asserv.c mimot.c \ - simu.host.c contact.c radar.c \ + simu.host.c contact.c radar.c radar_defs.c \ path.c food.c events.host.c \ fsm.host.c init.c move.c top.c hola.c loader.c fsm_AI_gen.avr.c # Modules needed for IO. MODULES = proto uart twi utils adc math/fixed math/geometry path/astar \ devices/usdist \ trace flash spi -AI_MODULES = twi_master common utils fsm +AI_MODULES = twi_master common utils fsm move # Configuration file. CONFIGFILE = avrconfig.h # IO board use an ATMega128. diff --git a/digital/io/src/radar.c b/digital/io/src/radar.c deleted file mode 100644 index 0f61a1bf..00000000 --- a/digital/io/src/radar.c +++ /dev/null @@ -1,189 +0,0 @@ -/* radar.c */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2010 Nicolas Schodet - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * }}} */ -#include "common.h" -#include "radar.h" - -#include "playground_2010.h" -#include "bot.h" - -#include "modules/devices/usdist/usdist.h" -#include "modules/math/geometry/geometry.h" -#include "modules/math/geometry/distance.h" -#include "modules/utils/utils.h" - -/** Margin to be considered inside the playground. An obstacle can not be - * exactly at the playground edge. */ -#define RADAR_MARGIN_MM 150 - -/** Maximum distance for a sensor reading to be ignored if another sensor is - * nearer. */ -#define RADAR_FAR_MM 250 - -/** Describe a radar sensor. */ -struct radar_sensor_t -{ - /** Distance updated by another module. */ - uint16_t *dist_mm; - /** Position relative to the robot center. */ - vect_t pos; - /** Angle relative to the robot X axis. */ - uint16_t a; -}; - -/** Define radar configuration. */ -struct radar_sensor_t radar_sensors[] = { -#define RADAR_SENSOR_FRONT 0 - { &usdist_mm[0], { 30 - 20, 0 }, G_ANGLE_UF016_DEG (0) }, -#define RADAR_SENSOR_LEFT 1 - { &usdist_mm[1], { 20 - 20, 20 }, G_ANGLE_UF016_DEG (30) }, -#define RADAR_SENSOR_RIGHT 2 - { &usdist_mm[2], { 20 - 20, -20 }, G_ANGLE_UF016_DEG (-30) }, -#define RADAR_SENSOR_BACK 3 - { &usdist_mm[3], { -30 - 20, 0 }, G_ANGLE_UF016_DEG (180) }, -}; - -/** Define exclusion area (considered as invalid point). */ -static uint8_t -radar_valid (vect_t p) -{ - return p.x >= RADAR_MARGIN_MM && p.x < PG_WIDTH - RADAR_MARGIN_MM - && p.y >= RADAR_MARGIN_MM && p.y < PG_LENGTH - RADAR_MARGIN_MM - /* Ignore points on slope, no margin for the slope start. */ - && (p.x < PG_WIDTH / 2 - PG_SLOPE_WIDTH / 2 - || p.x >= PG_WIDTH / 2 + PG_SLOPE_WIDTH / 2 - || p.y < PG_LENGTH - PG_SLOPE_LENGTH - RADAR_MARGIN_MM / 2); -} - -uint8_t -radar_update (const position_t *robot_pos, vect_t *obs_pos) -{ - uint8_t i, j; - vect_t ray; - uint8_t obs_nb = 0; - uint8_t front_nb; - vect_t front_center; - /* Compute hit points for each sensor and eliminate invalid ones. */ - vect_t hit[UTILS_COUNT (radar_sensors)]; - uint8_t valid[UTILS_COUNT (radar_sensors)]; - uint16_t dist_mm[UTILS_COUNT (radar_sensors)]; - for (i = 0; i < UTILS_COUNT (radar_sensors); i++) - { - dist_mm[i] = *radar_sensors[i].dist_mm; - if (dist_mm[i] != 0xffff) - { - hit[i] = radar_sensors[i].pos; - vect_rotate_uf016 (&hit[i], robot_pos->a); - vect_translate (&hit[i], &robot_pos->v); - vect_from_polar_uf016 (&ray, dist_mm[i], - robot_pos->a + radar_sensors[i].a); - vect_translate (&hit[i], &ray); - valid[i] = radar_valid (hit[i]); - vect_from_polar_uf016 (&ray, RADAR_OBSTACLE_EDGE_RADIUS_MM, - robot_pos->a + radar_sensors[i].a); - vect_translate (&hit[i], &ray); - } - else - valid[i] = 0; - } - /* Ignore sensor results too far from other sensors. */ - for (i = 0; i < UTILS_COUNT (radar_sensors) - 1; i++) - { - for (j = i + 1; valid[i] && j < UTILS_COUNT (radar_sensors); j++) - { - if (valid[j]) - { - if (dist_mm[i] + RADAR_FAR_MM < dist_mm[j]) - valid[j] = 0; - else if (dist_mm[j] + RADAR_FAR_MM < dist_mm[i]) - valid[i] = 0; - } - } - } - /* Specific treatment about sensor topology. */ - if (valid[RADAR_SENSOR_BACK]) - obs_pos[obs_nb++] = hit[RADAR_SENSOR_BACK]; - front_nb = 0; - front_center.x = 0; front_center.y = 0; - for (i = RADAR_SENSOR_FRONT; i < RADAR_SENSOR_BACK; i++) - { - if (valid[i]) - { - vect_add (&front_center, &hit[i]); - front_nb++; - } - } - if (front_nb) - { - vect_scale_f824 (&front_center, 0x1000000l / front_nb); - obs_pos[obs_nb++] = front_center; - } - /* Done. */ - return obs_nb; -} - -uint8_t -radar_blocking (const vect_t *robot_pos, const vect_t *dest_pos, - const vect_t *obs_pos, uint8_t obs_nb) -{ - uint8_t i; - /* Stop here if no obstacle. */ - if (!obs_nb) - return 0; - vect_t vd = *dest_pos; vect_sub (&vd, robot_pos); - uint16_t d = vect_norm (&vd); - /* If destination is realy near, stop here. */ - if (d < RADAR_EPSILON_MM) - return 0; - /* If destination is near, use clearance to destination point instead of - * stop length. */ - vect_t t; - if (d < RADAR_STOP_MM) - t = *dest_pos; - else - { - vect_scale_f824 (&vd, (1ll << 24) / d * RADAR_STOP_MM); - t = *robot_pos; - vect_translate (&t, &vd); - } - /* Now, look at obstacles. */ - for (i = 0; i < obs_nb; i++) - { - /* Vector from robot to obstacle. */ - vect_t vo = obs_pos[i]; vect_sub (&vo, robot_pos); - /* Ignore if in our back. */ - int32_t dp = vect_dot_product (&vd, &vo); - if (dp < 0) - continue; - /* Check distance. */ - int16_t od = distance_segment_point (robot_pos, &t, &obs_pos[i]); - if (od > BOT_SIZE_SIDE + RADAR_CLEARANCE_MM / 2 - + RADAR_OBSTACLE_RADIUS_MM) - continue; - /* Else, obstacle is blocking. */ - return 1; - } - return 0; -} - diff --git a/digital/io/src/radar.h b/digital/io/src/radar.h deleted file mode 100644 index b4611ec7..00000000 --- a/digital/io/src/radar.h +++ /dev/null @@ -1,78 +0,0 @@ -#ifndef radar_h -#define radar_h -/* radar.h */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2010 Nicolas Schodet - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * }}} */ -#include "defs.h" - -/** - * Handle any distance sensors information to extract useful data. This - * includes: - * - combining several sensors information for a more precise obstacle - * position, - * - ignoring obstacles not in the playground, - * - determining if an obstacle should make the robot stop. - */ - -/** Estimated obstacle edge radius. As the sensors detect obstacle edge, this is - * added to position obstacle center. */ -#define RADAR_OBSTACLE_EDGE_RADIUS_MM 40 - -/** Estimated obstacle radius. The obstacle may be larger than at the - * detected edge. */ -#define RADAR_OBSTACLE_RADIUS_MM 150 - -/** Stop distance. Distance under which an obstacle is considered harmful when - * moving. */ -#define RADAR_STOP_MM 350 - -/** Clearance distance. Distance over which an obstacle should be to the side - * when moving. - * - * OK, more explanations: when moving, a rectangle is placed in front of the - * robot, of length RADAR_STOP_MM and width 2 * (RADAR_CLEARANCE_MM + - * BOT_SIZE_SIDE). If an obstacle is inside this rectangle, it is considered - * in the way. - * - * If the destination point is near (< RADAR_STOP_MM - RADAR_CLEARANCE_MM), - * this reduce the rectangle length. - * - * If the destination is really near (< RADAR_EPSILON_MM), ignore all this. */ -#define RADAR_CLEARANCE_MM 100 - -/** Destination distance near enough so that obstacles could be ignored. */ -#define RADAR_EPSILON_MM 70 - -/** Update radar view. Return the number of obstacles found. Obstacles - * positions are returned in obs_pos. */ -uint8_t -radar_update (const position_t *robot_pos, vect_t *obs_pos); - -/** Return non zero if there is a blocking obstacle near the robot while going - * to a destination point. */ -uint8_t -radar_blocking (const vect_t *robot_pos, const vect_t *dest_pos, - const vect_t *obs_pos, uint8_t obs_nb); - -#endif /* radar_h */ diff --git a/digital/io/src/radar_defs.c b/digital/io/src/radar_defs.c new file mode 100644 index 00000000..2787ed97 --- /dev/null +++ b/digital/io/src/radar_defs.c @@ -0,0 +1,54 @@ +/* radar_defs.c */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2010 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "radar.h" + +#include "modules/devices/usdist/usdist.h" +#include "playground_2010.h" + +/** Define radar configuration. */ +struct radar_sensor_t radar_sensors[RADAR_SENSOR_NB] = { +#define RADAR_SENSOR_FRONT 0 + { &usdist_mm[0], { 30 - 20, 0 }, G_ANGLE_UF016_DEG (0) }, +#define RADAR_SENSOR_LEFT 1 + { &usdist_mm[1], { 20 - 20, 20 }, G_ANGLE_UF016_DEG (30) }, +#define RADAR_SENSOR_RIGHT 2 + { &usdist_mm[2], { 20 - 20, -20 }, G_ANGLE_UF016_DEG (-30) }, +#define RADAR_SENSOR_BACK 3 + { &usdist_mm[3], { -30 - 20, 0 }, G_ANGLE_UF016_DEG (180) }, +}; + +/** Define exclusion area (considered as invalid point). */ +uint8_t +radar_valid (vect_t p) +{ + return p.x >= RADAR_MARGIN_MM && p.x < PG_WIDTH - RADAR_MARGIN_MM + && p.y >= RADAR_MARGIN_MM && p.y < PG_LENGTH - RADAR_MARGIN_MM + /* Ignore points on slope, no margin for the slope start. */ + && (p.x < PG_WIDTH / 2 - PG_SLOPE_WIDTH / 2 + || p.x >= PG_WIDTH / 2 + PG_SLOPE_WIDTH / 2 + || p.y < PG_LENGTH - PG_SLOPE_LENGTH - RADAR_MARGIN_MM / 2); +} + diff --git a/digital/io/src/radar_defs.h b/digital/io/src/radar_defs.h new file mode 100644 index 00000000..51bc5943 --- /dev/null +++ b/digital/io/src/radar_defs.h @@ -0,0 +1,41 @@ +#ifndef radar_defs_h +#define radar_defs_h +/* radar_defs.h */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2010 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +#define RADAR_OBSTACLE_EDGE_RADIUS_MM 40 +#define RADAR_OBSTACLE_RADIUS_MM 150 +#define RADAR_STOP_MM 350 +#define RADAR_CLEARANCE_MM 100 +#define RADAR_EPSILON_MM 70 + +#define RADAR_SENSOR_NB 4 + +#define RADAR_SENSOR_FRONT_FIRST 0 +#define RADAR_SENSOR_FRONT_NB 3 +#define RADAR_SENSOR_BACK_FIRST 3 +#define RADAR_SENSOR_BACK_NB 1 + +#endif /* radar_defs_h */ diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py index de8eaa22..260960f8 100644 --- a/host/simu/robots/robospierre/model/bag.py +++ b/host/simu/robots/robospierre/model/bag.py @@ -54,4 +54,5 @@ class Bag: DistanceSensorSensopart (link_bag.io_hub.adc[3], scheduler, table, (-20, 20), pi - pi * 10 / 180, (self.position, ), 2), ] + self.pos_report = link_bag.io_hub.pos_report diff --git a/host/simu/robots/robospierre/view/bag.py b/host/simu/robots/robospierre/view/bag.py index 0b5a85ad..73aa9fe3 100644 --- a/host/simu/robots/robospierre/view/bag.py +++ b/host/simu/robots/robospierre/view/bag.py @@ -43,4 +43,5 @@ class Bag: ClampSide.height), model_bag.clamp)) self.distance_sensor = [DistanceSensorUS (self.robot, ds) for ds in model_bag.distance_sensor] + self.pos_report = PosReport (table, model_bag.pos_report) -- cgit v1.2.3 From 9b1ce3a050d9169f3bf77764063de0cff800190e Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 27 May 2011 07:58:36 +0200 Subject: digital/{ai,io-hub,mimot}, host/simu: change robospierre rotation motor --- digital/ai/tools/robospierre.py | 2 +- digital/ai/tools/test_simu_control_robospierre.py | 2 +- digital/io-hub/src/robospierre/bot.h | 18 ++++++------- digital/mimot/src/dirty/models.host.c | 32 +++++++++++++++++++++++ digital/mimot/tools/mimot/init.py | 2 +- host/simu/robots/robospierre/model/clamp.py | 2 +- 6 files changed, 45 insertions(+), 13 deletions(-) (limited to 'host/simu/robots/robospierre') diff --git a/digital/ai/tools/robospierre.py b/digital/ai/tools/robospierre.py index e8384f6c..6a44883e 100644 --- a/digital/ai/tools/robospierre.py +++ b/digital/ai/tools/robospierre.py @@ -25,7 +25,7 @@ class Robot: self.robot_view = simu.robots.robospierre.view.bag asserv_cmd = ('../../asserv/src/asserv/asserv.host', '-m9', 'robospierre') - mimot_cmd = ('../../mimot/src/dirty/dirty.host', '-m9', 'marcel') + mimot_cmd = ('../../mimot/src/dirty/dirty.host', '-m9', 'robospierre') io_hub_cmd = ('../../io-hub/src/robospierre/io_hub.host') self.asserv = asserv.Proto (PopenIO (asserv_cmd), proto_time, **asserv.init.host['robospierre']) diff --git a/digital/ai/tools/test_simu_control_robospierre.py b/digital/ai/tools/test_simu_control_robospierre.py index 0d52219e..1795e17f 100644 --- a/digital/ai/tools/test_simu_control_robospierre.py +++ b/digital/ai/tools/test_simu_control_robospierre.py @@ -30,7 +30,7 @@ class TestSimuControl (TestSimu): ELEVATION_STROKE = 0x3b0b - ROTATION_STROKE = 0x11c6 + ROTATION_STROKE = 0x233e def __init__ (self, robot_class): TestSimu.__init__ (self, robot_class) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 1071bf7e..65f6adf6 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -67,9 +67,9 @@ # 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_SLOT_BACK_BOTTOM_ROTATION_STEP 0x233e +# define BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP 0x233e +# define BOT_CLAMP_SLOT_BACK_TOP_ROTATION_STEP 0x233e # define BOT_CLAMP_BAY_FRONT_ROTATION_STEP \ BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP @@ -96,23 +96,23 @@ # 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_SLOT_BACK_BOTTOM_ROTATION_STEP 0x233e +# define BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP 0x233e +# define BOT_CLAMP_SLOT_BACK_TOP_ROTATION_STEP 0x233e # 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 +# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP 0x10de #define BOT_CLAMP_CLOSED_ROTATION_OFFSET -61 #endif /* !HOST */ #define BOT_CLAMP_ELEVATION_SPEED 0x60 -#define BOT_CLAMP_ROTATION_SPEED 0x30 -#define BOT_CLAMP_ROTATION_OFFSET_SPEED 2 +#define BOT_CLAMP_ROTATION_SPEED 0x60 +#define BOT_CLAMP_ROTATION_OFFSET_SPEED 1 #define BOT_PWM_CLAMP 2 #define BOT_PWM_DOOR_FRONT_BOTTOM 0 diff --git a/digital/mimot/src/dirty/models.host.c b/digital/mimot/src/dirty/models.host.c index a835298f..b1141748 100644 --- a/digital/mimot/src/dirty/models.host.c +++ b/digital/mimot/src/dirty/models.host.c @@ -52,6 +52,26 @@ static const struct motor_def_t marcel_clamp_f2342_model = -INFINITY, +INFINITY, }; +/* Robospierre rotation motor, AMAX32GHP with 1:16 gearbox model. */ +static const struct motor_def_t robospierre_rotation_amax32ghp_model = +{ + /* Motor characteristics. */ + 269 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */ + 25.44 / 1000, /* Torque constant (N.m/A). */ + 0, /* Bearing friction (N.m/(rad/s)). */ + 3.99, /* Terminal resistance (Ohm). */ + 0.24 / 1000, /* Terminal inductance (H). */ + 24.0, /* Maximum voltage (V). */ + /* Gearbox characteristics. */ + 16, /* Gearbox ratio. */ + 0.75, /* Gearbox efficiency. */ + /* Load characteristics. */ + 0.200 * 0.010 * 0.010, /* Load (kg.m^2). */ + /* This is a pifometric estimation. */ + /* Hardware limits. */ + -INFINITY, +INFINITY, +}; + /* Marcel, APBTeam 2010. */ static const struct robot_t marcel_robot = { @@ -63,6 +83,17 @@ static const struct robot_t marcel_robot = simu_sensor_update_marcel, }; +/* Robospierre, APBTeam 2011. */ +static const struct robot_t robospierre_robot = +{ + /** Auxiliary motors, NULL if not present. */ + { &marcel_clamp_f2342_model, &robospierre_rotation_amax32ghp_model }, + /** Number of steps for each auxiliary motor encoder. */ + { 256, 250 }, + /** Sensor update function. */ + NULL, +}; + /* Table of models. */ static const struct { @@ -70,6 +101,7 @@ static const struct const struct robot_t *robot; } models[] = { { "marcel", &marcel_robot }, + { "robospierre", &robospierre_robot }, { 0, 0 } }; diff --git a/digital/mimot/tools/mimot/init.py b/digital/mimot/tools/mimot/init.py index a05f8c44..d302e060 100644 --- a/digital/mimot/tools/mimot/init.py +++ b/digital/mimot/tools/mimot/init.py @@ -17,7 +17,7 @@ target_robospierre = dict ( a1a = 0.5, a1sm = 0x30, a1ss = 0x08, a1be = 256, a1bs = 0x18, a1bc = 5, E = 0x3ff, D = 0x1ff, - w = 0x01, + w = 0x03, ) target = { 'marcel': target_marcel, diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index fd3dde7c..c36d0d36 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -45,7 +45,7 @@ class Clamp (Observable): ELEVATION_MOTOR_STROKE = 120.0 * 5.0 / 6.0 ROTATION_STROKE = pi - ROTATION_MOTOR_STROKE = pi * 115.0 / 12.0 + ROTATION_MOTOR_STROKE = 2 * pi * 36.088 / 16 CLAMPING_STROKE = 10 CLAMPING_MOTOR_STROKE = pi -- cgit v1.2.3 From 4882fbefaeca9cc5292cec9a2f93d34f04e67b5c Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 28 May 2011 00:31:13 +0200 Subject: digital/io-hub: add clamp initialisation --- digital/ai/src/twi_master/mimot.c | 42 +++++++++++++ digital/ai/src/twi_master/mimot.h | 16 +++++ digital/io-hub/src/robospierre/bot.h | 14 +++++ digital/io-hub/src/robospierre/clamp.c | 92 ++++++++++++++++++++++++++++- digital/io-hub/src/robospierre/main.c | 4 ++ digital/mimot/src/dirty/twi_proto.c | 21 +++++++ host/simu/robots/robospierre/model/clamp.py | 2 + 7 files changed, 188 insertions(+), 3 deletions(-) (limited to 'host/simu/robots/robospierre') diff --git a/digital/ai/src/twi_master/mimot.c b/digital/ai/src/twi_master/mimot.c index af6ff5f9..c3b82823 100644 --- a/digital/ai/src/twi_master/mimot.c +++ b/digital/ai/src/twi_master/mimot.c @@ -117,6 +117,30 @@ mimot_reset (void) twi_master_send_buffer (1); } +void +mimot_set_motor0_position (uint16_t position) +{ + uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); + buffer[0] = 'p'; + buffer[1] = 'Y'; + buffer[2] = 0; + buffer[3] = v16_to_v8 (position, 1); + buffer[4] = v16_to_v8 (position, 0); + twi_master_send_buffer (5); +} + +void +mimot_set_motor1_position (uint16_t position) +{ + uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); + buffer[0] = 'p'; + buffer[1] = 'Y'; + buffer[2] = 1; + buffer[3] = v16_to_v8 (position, 1); + buffer[4] = v16_to_v8 (position, 0); + twi_master_send_buffer (5); +} + void mimot_move_motor0_absolute (uint16_t position, uint8_t speed) { @@ -181,3 +205,21 @@ mimot_motor1_clamp (int8_t speed, int16_t pwm) twi_master_send_buffer (5); } +void +mimot_motor0_free (void) +{ + uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); + buffer[0] = 'w'; + buffer[1] = 0; + twi_master_send_buffer (2); +} + +void +mimot_motor1_free (void) +{ + uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); + buffer[0] = 'w'; + buffer[1] = 1; + twi_master_send_buffer (2); +} + diff --git a/digital/ai/src/twi_master/mimot.h b/digital/ai/src/twi_master/mimot.h index 60811c10..97575b0d 100644 --- a/digital/ai/src/twi_master/mimot.h +++ b/digital/ai/src/twi_master/mimot.h @@ -68,6 +68,14 @@ mimot_get_motor1_position (void); void mimot_reset (void); +/** Set motor0 position in steps. */ +void +mimot_set_motor0_position (uint16_t position); + +/** Set motor1 position in steps. */ +void +mimot_set_motor1_position (uint16_t position); + /** Move motor0 to absolute position in steps. */ void mimot_move_motor0_absolute (uint16_t position, uint8_t speed); @@ -92,4 +100,12 @@ mimot_motor0_clamp (int8_t speed, int16_t pwm); void mimot_motor1_clamp (int8_t speed, int16_t pwm); +/** Free motor0. */ +void +mimot_motor0_free (void); + +/** Free motor1. */ +void +mimot_motor1_free (void); + #endif /* mimot_h */ diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 02b74da9..7a8065dd 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -63,6 +63,8 @@ # 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_INIT_FRONT_SEEN_ELEVATION_STEP 0x18ca +# define BOT_CLAMP_INIT_BACK_SEEN_ELEVATION_STEP 0x18ca # define BOT_CLAMP_SLOT_FRONT_BOTTOM_ROTATION_STEP 0 # define BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP 0 @@ -93,6 +95,9 @@ # 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) +// TODO: to be measured. +# define BOT_CLAMP_INIT_FRONT_SEEN_ELEVATION_STEP (0x1da7 / 2) +# define BOT_CLAMP_INIT_BACK_SEEN_ELEVATION_STEP (0x1f03 / 2) # define BOT_CLAMP_SLOT_FRONT_BOTTOM_ROTATION_STEP 0 # define BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP 0 @@ -112,12 +117,17 @@ #endif /* !HOST */ +#define BOT_CLAMP_INIT_ELEVATION_STEP \ + BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP + #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_INIT_ELEVATION_SPEED 0x08 +#define BOT_CLAMP_INIT_ROTATION_SPEED -0x04 #define BOT_CLAMP_ELEVATION_SPEED 0x60 #define BOT_CLAMP_ROTATION_SPEED 0x60 #define BOT_CLAMP_ROTATION_OFFSET_SPEED 1 @@ -140,4 +150,8 @@ -0x3ff, 50, ((slot == CLAMP_SLOT_FRONT_BOTTOM \ || slot == CLAMP_SLOT_BACK_BOTTOM) ? -0x100 : -0x180) +#define BOT_PWM_CLAMP_INIT 0x1ff, 125, 0 +#define BOT_PWM_DOOR_INIT 0x1ff, 74, 0x55 +#define BOT_PWM_CLAMP_DOOR_INIT 125 + #endif /* bot_h */ diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 71cb2dfb..65a90a7a 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -35,6 +35,8 @@ #include "fsm.h" #include "fsm_queue.h" +#include "modules/proto/proto.h" + #include "logistic.h" /* @@ -50,8 +52,18 @@ FSM_INIT FSM_STATES ( - /* Initial state, to be complete with full initialisation. */ + /* Initial state. */ CLAMP_START, + /* Initialisation sequence: opening everything. */ + CLAMP_INIT_OPENING, + /* Initialisation sequence: going up until a middle pawn sensor + * see the clamp. */ + CLAMP_INIT_UPING_UNTIL_SEEN, + /* Initialisation sequence: going to the middle level. */ + CLAMP_INIT_GOING_MIDDLE, + /* Initialisation sequence: finding front right edge. */ + CLAMP_INIT_FINDING_EDGE, + /* Returning to idle position. */ CLAMP_GOING_IDLE, /* Waiting external events, clamp at middle level. */ @@ -85,6 +97,8 @@ FSM_STATES ( CLAMP_MOVE_DST_CLAMP_OPENING) FSM_EVENTS ( + /* During initialisation sequence, clamp seen by a sensor. */ + clamp_init_seen, /* New element inside bottom slot. */ clamp_new_element, /* Order to drop elements. */ @@ -100,8 +114,12 @@ FSM_EVENTS ( clamp_move_success, /* Elevation and elevation motor success. */ clamp_elevation_rotation_success, + /* Elevation motor success. */ + clamp_elevation_success, /* Elevation motor failure. */ clamp_elevation_failure, + /* Rotation motor success. */ + clamp_rotation_success, /* Rotation motor failure. */ clamp_rotation_failure) @@ -127,6 +145,10 @@ struct clamp_t uint8_t open; /** True if clamp position is controled. */ uint8_t controled; + /** Position of clamp when seen by sensor. */ + uint16_t init_seen_step; + /** Position to initialise at seen position. */ + uint16_t init_seen_init_step; }; /** Global context. */ @@ -280,6 +302,24 @@ clamp_handle_event (void) /* Go directly to next point. */ clamp_route (); } + /* Handle initialisation. */ + if (FSM_CAN_HANDLE (AI, clamp_init_seen)) + { + if (!IO_GET (CONTACT_FRONT_MIDDLE)) + { + ctx.init_seen_step = mimot_get_motor0_position (); + ctx.init_seen_init_step = BOT_CLAMP_INIT_FRONT_SEEN_ELEVATION_STEP; + FSM_HANDLE (AI, clamp_init_seen); + return 1; + } + if (!IO_GET (CONTACT_BACK_MIDDLE)) + { + ctx.init_seen_step = mimot_get_motor0_position (); + ctx.init_seen_init_step = BOT_CLAMP_INIT_BACK_SEEN_ELEVATION_STEP; + FSM_HANDLE (AI, clamp_init_seen); + return 1; + } + } return 0; } @@ -374,12 +414,58 @@ clamp_route (void) /* CLAMP FSM */ -FSM_TRANS (CLAMP_START, init_actuators, CLAMP_GOING_IDLE) +FSM_TRANS (CLAMP_START, init_actuators, CLAMP_INIT_OPENING) { - clamp_move (CLAMP_SLOT_FRONT_MIDDLE); + pwm_set_timed (BOT_PWM_DOOR_FRONT_BOTTOM, BOT_PWM_DOOR_INIT); + pwm_set_timed (BOT_PWM_DOOR_FRONT_TOP, BOT_PWM_DOOR_INIT); + pwm_set_timed (BOT_PWM_DOOR_BACK_BOTTOM, BOT_PWM_DOOR_INIT); + pwm_set_timed (BOT_PWM_DOOR_BACK_TOP, BOT_PWM_DOOR_INIT); + pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_INIT); return FSM_NEXT (CLAMP_START, init_actuators); } +FSM_TRANS_TIMEOUT (CLAMP_INIT_OPENING, BOT_PWM_CLAMP_DOOR_INIT, + CLAMP_INIT_UPING_UNTIL_SEEN) +{ + mimot_move_motor0_absolute (mimot_get_motor0_position () + + BOT_CLAMP_INIT_ELEVATION_STEP, + BOT_CLAMP_INIT_ELEVATION_SPEED); + return FSM_NEXT_TIMEOUT (CLAMP_INIT_OPENING); +} + +FSM_TRANS (CLAMP_INIT_UPING_UNTIL_SEEN, clamp_elevation_success, CLAMP_START) +{ + /* Dead end. */ + mimot_motor0_free (); + return FSM_NEXT (CLAMP_INIT_UPING_UNTIL_SEEN, clamp_elevation_success); +} + +FSM_TRANS (CLAMP_INIT_UPING_UNTIL_SEEN, clamp_init_seen, + CLAMP_INIT_GOING_MIDDLE) +{ + mimot_motor0_free (); + mimot_set_motor0_position (ctx.init_seen_init_step + + mimot_get_motor0_position () + - ctx.init_seen_step); + proto_send1w ('C', ctx.init_seen_step); + mimot_move_motor0_absolute (BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP, + BOT_CLAMP_ELEVATION_SPEED); + return FSM_NEXT (CLAMP_INIT_UPING_UNTIL_SEEN, clamp_init_seen); +} + +FSM_TRANS (CLAMP_INIT_GOING_MIDDLE, clamp_elevation_success, + CLAMP_INIT_FINDING_EDGE) +{ + mimot_motor1_zero_position (BOT_CLAMP_INIT_ROTATION_SPEED); + return FSM_NEXT (CLAMP_INIT_GOING_MIDDLE, clamp_elevation_success); +} + +FSM_TRANS (CLAMP_INIT_FINDING_EDGE, clamp_rotation_success, CLAMP_GOING_IDLE) +{ + clamp_move (CLAMP_SLOT_FRONT_MIDDLE); + return FSM_NEXT (CLAMP_INIT_FINDING_EDGE, clamp_rotation_success); +} + FSM_TRANS (CLAMP_GOING_IDLE, clamp_move_success, CLAMP_IDLE) { return FSM_NEXT (CLAMP_GOING_IDLE, clamp_move_success); diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index b1e85b72..7c773761 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -139,8 +139,12 @@ main_event_to_fsm (void) if (mimot_motor0_status == success && mimot_motor1_status == success) FSM_HANDLE_E (AI, clamp_elevation_rotation_success); + if (mimot_motor0_status == success) + FSM_HANDLE_E (AI, clamp_elevation_success); else if (mimot_motor0_status == failure) FSM_HANDLE_E (AI, clamp_elevation_failure); + if (mimot_motor1_status == success) + FSM_HANDLE_E (AI, clamp_rotation_success); else if (mimot_motor1_status == failure) FSM_HANDLE_E (AI, clamp_rotation_failure); /* Clamp specific events. */ diff --git a/digital/mimot/src/dirty/twi_proto.c b/digital/mimot/src/dirty/twi_proto.c index 3048c594..bda255ef 100644 --- a/digital/mimot/src/dirty/twi_proto.c +++ b/digital/mimot/src/dirty/twi_proto.c @@ -150,6 +150,18 @@ twi_proto_callback (u8 *buf, u8 size) else buf[0] = 0; break; + case c ('w', 1): + /* Free motor. + * - b: aux index. */ + if (buf[2] < AC_ASSERV_AUX_NB) + { + pos_reset (&pos_aux[buf[2]]); + state_aux[buf[2]].mode = MODE_PWM; + pwm_set (&pwm_aux[buf[2]], 0); + } + else + buf[0] = 0; + break; case c ('p', x): /* Set parameters. */ if (twi_proto_params (&buf[2], size - 2) != 0) @@ -173,6 +185,15 @@ twi_proto_params (u8 *buf, u8 size) size--; switch (*buf++) { + case 'Y': + /* Set current aux position. + * - b: aux index. + * - w: position. */ + if (buf[0] >= AC_ASSERV_AUX_NB || size < 3) + return 1; + aux[buf[0]].pos = v8_to_v16 (buf[1], buf[2]); + eat = 3; + break; default: return 1; } diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index c36d0d36..a3476a46 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -69,6 +69,8 @@ class Clamp (Observable): self.elevation_motor = elevation_motor self.rotation_motor = rotation_motor self.clamping_motor = clamping_motor + self.rotation_motor.limits.min = 0 + self.rotation_motor.limits.notify () self.door_motors = (door_motors[0], None, door_motors[1], door_motors[2], None, door_motors[3], None) self.slots = ( -- cgit v1.2.3 From 5c8d716b96d32609d18af5219c541201209e586d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 29 May 2011 19:19:28 +0200 Subject: digital/io-hub: handle element detection on table --- digital/io-hub/src/robospierre/Makefile | 2 +- digital/io-hub/src/robospierre/bot.h | 7 ++ digital/io-hub/src/robospierre/clamp.c | 12 +-- digital/io-hub/src/robospierre/pawn_sensor.c | 113 +++++++++++++++++++++++++++ digital/io-hub/src/robospierre/pawn_sensor.h | 33 ++++++++ host/simu/robots/robospierre/model/clamp.py | 2 +- 6 files changed, 161 insertions(+), 8 deletions(-) create mode 100644 digital/io-hub/src/robospierre/pawn_sensor.c create mode 100644 digital/io-hub/src/robospierre/pawn_sensor.h (limited to 'host/simu/robots/robospierre') diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index e4b87124..a9bb2283 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -5,7 +5,7 @@ PROGS = io_hub HOST_PROGS = test_element # Sources to compile. io_hub_SOURCES = main.c \ - clamp.c logistic.c element.c \ + clamp.c logistic.c element.c pawn_sensor.c \ radar_defs.c radar.c path.c move.c \ init.c fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \ pwm.avr.c pwm.host.c \ diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 890780b3..7e773032 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -46,6 +46,13 @@ /** Angle error at the front contact point. */ #define BOT_FRONT_CONTACT_ANGLE_ERROR_DEG 0 +/** Distance from robot center to front pawn detection threshold. */ +#define BOT_PAWN_FRONT_DETECTION_THRESHOLD_MM 190 +/** Distance from robot center to back pawn detection threshold. */ +#define BOT_PAWN_BACK_DETECTION_THRESHOLD_MM -190 +/** Distance from robot center to an element near enough to be taken. */ +#define BOT_PAWN_TAKING_DISTANCE_MM 150 + /** Speed used for initialisation. */ #define BOT_SPEED_INIT 0x10, 0x10, 0x10, 0x10 /** Normal cruise speed. */ diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 4c0fc9a7..bae3591d 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -38,6 +38,7 @@ #include "modules/proto/proto.h" #include "logistic.h" +#include "pawn_sensor.h" /* * There is two FSM in this file. @@ -275,16 +276,15 @@ clamp_handle_event (void) { if (FSM_CAN_HANDLE (AI, clamp_new_element)) { - /* XXX: temporary hack. */ - uint8_t element_type = contact_get_color () ? ELEMENT_PAWN : ELEMENT_KING; - if (!IO_GET (CONTACT_FRONT_BOTTOM) - && !logistic_global.slots[CLAMP_SLOT_FRONT_BOTTOM]) + uint8_t element_type; + element_type = pawn_sensor_get (DIRECTION_FORWARD); + if (element_type) { clamp_new_element (CLAMP_SLOT_FRONT_BOTTOM, element_type); return 1; } - if (!IO_GET (CONTACT_BACK_BOTTOM) - && !logistic_global.slots[CLAMP_SLOT_BACK_BOTTOM]) + element_type = pawn_sensor_get (DIRECTION_BACKWARD); + if (element_type) { clamp_new_element (CLAMP_SLOT_BACK_BOTTOM, element_type); return 1; diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c new file mode 100644 index 00000000..65f0afe4 --- /dev/null +++ b/digital/io-hub/src/robospierre/pawn_sensor.c @@ -0,0 +1,113 @@ +/* pawn_sensor.c */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "pawn_sensor.h" + +#include "asserv.h" +#include "contact.h" +#include "logistic.h" +#include "element.h" +#include "clamp.h" +#include "bot.h" + +#include "modules/math/geometry/distance.h" + +/* Handle pawn sensors. When a pawn is detected, it can not be taken + * directly, but only once it is inside the robot. */ + +/** Pawn sensor context. */ +struct pawn_sensor_t +{ + /** Is there something in front of the sensor? */ + uint8_t active; + /** If active, supposed position of element. */ + vect_t active_position; +}; + +/** Global contexts. */ +struct pawn_sensor_t pawn_sensor_front, pawn_sensor_back; + +static uint8_t +pawn_sensor_get_type (uint8_t direction) +{ + uint8_t element_type = contact_get_color () ? ELEMENT_PAWN : ELEMENT_KING; + return element_type; +} + +uint8_t +pawn_sensor_get (uint8_t direction) +{ + struct pawn_sensor_t *ctx; + uint8_t contact_value, slot; + int16_t dist; + /* Check direction. */ + if (direction == DIRECTION_FORWARD) + { + ctx = &pawn_sensor_front; + contact_value = !IO_GET (CONTACT_FRONT_BOTTOM); + slot = CLAMP_SLOT_FRONT_BOTTOM; + dist = BOT_PAWN_FRONT_DETECTION_THRESHOLD_MM; + } + else + { + ctx = &pawn_sensor_back; + contact_value = !IO_GET (CONTACT_BACK_BOTTOM); + slot = CLAMP_SLOT_BACK_BOTTOM; + dist = BOT_PAWN_BACK_DETECTION_THRESHOLD_MM; + } + /* Handle contact. */ + if (contact_value) + { + if (!logistic_global.slots[slot] + && logistic_global.moving_to != slot) + { + position_t robot_position; + asserv_get_position (&robot_position); + if (ctx->active) + { + int32_t d = distance_point_point (&ctx->active_position, + &robot_position.v); + if (d < BOT_PAWN_TAKING_DISTANCE_MM) + { + ctx->active = 0; + return pawn_sensor_get_type (direction); + } + } + else + { + ctx->active = 1; + vect_from_polar_uf016 (&ctx->active_position, dist, + robot_position.a); + vect_translate (&ctx->active_position, &robot_position.v); + } + } + } + else + { + ctx->active = 0; + } + return 0; +} + diff --git a/digital/io-hub/src/robospierre/pawn_sensor.h b/digital/io-hub/src/robospierre/pawn_sensor.h new file mode 100644 index 00000000..14ce2d62 --- /dev/null +++ b/digital/io-hub/src/robospierre/pawn_sensor.h @@ -0,0 +1,33 @@ +#ifndef pawn_sensor_h +#define pawn_sensor_h +/* pawn_sensor.h */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/** Update sensor state and return the element type if an element is ready to + * be taken. */ +uint8_t +pawn_sensor_get (uint8_t direction); + +#endif /* pawn_sensor_h */ diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index a3476a46..0af4ccf3 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -220,7 +220,7 @@ class Clamp (Observable): m = self.__get_robot_matrix () # Look up elements. xoffset = (self.BAY_OFFSET, -self.BAY_OFFSET)[side] - xmargin = 20 + xmargin = 40 ymargin = 50 for o in self.table.obstacles: if o.level == 1 and o.pos is not None: -- cgit v1.2.3 From 5e4f6921f117d650025fee172af7bf4e68b69129 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 30 May 2011 11:45:09 +0200 Subject: digital/io-hub, host/simu: add Strat contact --- digital/io-hub/src/robospierre/contact_defs.h | 8 +++++--- digital/io-hub/src/robospierre/pawn_sensor.c | 2 +- digital/io-hub/tools/io_hub/mex.py | 2 +- host/simu/robots/robospierre/model/bag.py | 3 ++- host/simu/robots/robospierre/view/bag.py | 2 ++ 5 files changed, 11 insertions(+), 6 deletions(-) (limited to 'host/simu/robots/robospierre') diff --git a/digital/io-hub/src/robospierre/contact_defs.h b/digital/io-hub/src/robospierre/contact_defs.h index 1e04f6bf..8c60342a 100644 --- a/digital/io-hub/src/robospierre/contact_defs.h +++ b/digital/io-hub/src/robospierre/contact_defs.h @@ -25,8 +25,9 @@ * * }}} */ -#define CONTACT_COLOR A, 7 -#define CONTACT_JACK F, 7 +#define CONTACT_COLOR E, 5 +#define CONTACT_JACK E, 6 +#define CONTACT_STRAT E, 5 #define CONTACT_FRONT_BOTTOM A, 4 #define CONTACT_FRONT_MIDDLE F, 4 #define CONTACT_BACK_BOTTOM A, 5 @@ -42,6 +43,7 @@ CONTACT (CONTACT_BACK_BOTTOM) \ CONTACT (CONTACT_BACK_MIDDLE) \ CONTACT (CONTACT_BACK_TOP) \ - CONTACT (CONTACT_SIDE) + CONTACT (CONTACT_SIDE) \ + CONTACT (CONTACT_STRAT) #endif /* contact_defs_h */ diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c index 65f0afe4..3a59e908 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.c +++ b/digital/io-hub/src/robospierre/pawn_sensor.c @@ -52,7 +52,7 @@ struct pawn_sensor_t pawn_sensor_front, pawn_sensor_back; static uint8_t pawn_sensor_get_type (uint8_t direction) { - uint8_t element_type = contact_get_color () ? ELEMENT_PAWN : ELEMENT_KING; + uint8_t element_type = IO_GET (CONTACT_STRAT) ? ELEMENT_PAWN : ELEMENT_KING; return element_type; } diff --git a/digital/io-hub/tools/io_hub/mex.py b/digital/io-hub/tools/io_hub/mex.py index 44c41010..66060ea1 100644 --- a/digital/io-hub/tools/io_hub/mex.py +++ b/digital/io-hub/tools/io_hub/mex.py @@ -31,7 +31,7 @@ ADC_NB = 8 PWM_NB = 6 PWM_VALUE_MAX = 1024 -CONTACT_NB = 9 +CONTACT_NB = 10 CONTACT_INIT = 0xffffffff class Mex: diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py index 260960f8..b98dafd9 100644 --- a/host/simu/robots/robospierre/model/bag.py +++ b/host/simu/robots/robospierre/model/bag.py @@ -34,8 +34,9 @@ class Bag: def __init__ (self, scheduler, table, link_bag): self.color_switch = Switch (link_bag.io_hub.contact[0], invert = True) self.jack = Switch (link_bag.io_hub.contact[1], invert = True) + self.strat_switch = Switch (link_bag.io_hub.contact[9], invert = True) self.contact = [ Switch (contact) - for contact in link_bag.io_hub.contact[2:] ] + for contact in link_bag.io_hub.contact[2:9] ] self.position = Position (link_bag.asserv.position) self.clamping_motor = MotorBasic (link_bag.io_hub.pwm[2], scheduler, 8 * pi, 0, pi) diff --git a/host/simu/robots/robospierre/view/bag.py b/host/simu/robots/robospierre/view/bag.py index 73aa9fe3..b718f6fe 100644 --- a/host/simu/robots/robospierre/view/bag.py +++ b/host/simu/robots/robospierre/view/bag.py @@ -35,6 +35,8 @@ class Bag: self.jack = Switch (sensor_frame, model_bag.jack, 'Jack') self.color_switch = Switch (sensor_frame, model_bag.color_switch, 'Color') + self.strat_switch = Switch (sensor_frame, model_bag.strat_switch, + 'Strat') self.robot = Robot (table, model_bag.position, model_bag.clamp) self.clamp = ( ClampTop (actuator_view.add_view (ClampTop.width, -- cgit v1.2.3 From 850cc292ac7ea847f6be2afcdc5bae89418cabda Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 30 May 2011 23:42:52 +0200 Subject: digital/io-hub: add path finding --- digital/io-hub/src/robospierre/Makefile | 2 +- digital/io-hub/src/robospierre/avrconfig.h | 6 + digital/io-hub/src/robospierre/path.c | 398 ++++++++++++++++++++++++++++- digital/io-hub/src/robospierre/simu.host.c | 16 ++ digital/io-hub/tools/io_hub/mex.py | 19 ++ host/simu/robots/robospierre/model/bag.py | 1 + host/simu/robots/robospierre/view/bag.py | 1 + 7 files changed, 440 insertions(+), 3 deletions(-) (limited to 'host/simu/robots/robospierre') diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index 43970369..714541dc 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -16,7 +16,7 @@ test_element_SOURCES = test_element.c logistic.c element.c # Modules needed for IO. MODULES = proto uart twi utils \ adc devices/usdist \ - math/fixed math/geometry + math/fixed math/geometry path/astar AI_MODULES = twi_master common utils fsm move test_element_MODULES = host math/fixed math/geometry # Configuration file. diff --git a/digital/io-hub/src/robospierre/avrconfig.h b/digital/io-hub/src/robospierre/avrconfig.h index c3f107c5..581fa136 100644 --- a/digital/io-hub/src/robospierre/avrconfig.h +++ b/digital/io-hub/src/robospierre/avrconfig.h @@ -120,6 +120,12 @@ /** Number of possible obstacles. */ #define AC_PATH_OBSTACLES_NB 2 +/* astar - A* path finding module. */ +/** Neighbor callback. */ +#define AC_ASTAR_NEIGHBOR_CALLBACK path_astar_neighbor_callback +/** Heuristic callback. */ +#define AC_ASTAR_HEURISTIC_CALLBACK path_astar_heuristic_callback + /* io-hub - io/ai board. */ /** TWI address of the io board. */ #define AC_IO_TWI_ADDRESS 10 diff --git a/digital/io-hub/src/robospierre/path.c b/digital/io-hub/src/robospierre/path.c index b3c91ac8..1122c684 100644 --- a/digital/io-hub/src/robospierre/path.c +++ b/digital/io-hub/src/robospierre/path.c @@ -23,18 +23,235 @@ * * }}} */ #include "common.h" +#include "defs.h" #include "path.h" +#include "bot.h" +#include "playground.h" + +#include "modules/path/astar/astar.h" +#include "modules/utils/utils.h" +#include "modules/math/geometry/distance.h" + +#define PATH_DEBUG 0 + +#if PATH_DEBUG +#include "debug.host.h" +#endif + +/** + * This year, due to the large number of obstacles, a grid like structure is + * used for path finding on the playground. The A* algorithm is used to find + * path along nodes. + * + * The grid is composed of 11 columns of 4 node each. They are numbered by + * column. Even columns are aligned with center of squares, while odd columns + * are at squares intersections. Therefore, odd columns have a offset of + * 352/2 mm, and that is the reason why code should handle odd and even + * columns differently. + * + * All those tricks are used to reduce the number of nodes. + */ + +/** Number of possible obstacles. */ +#define PATH_OBSTACLES_NB AC_PATH_OBSTACLES_NB + +/** Number of nodes in a column. */ +#define PATH_COLUMN_NODES_NB 4 + +/** Number of columns. */ +#define PATH_COLUMNS_NB 11 + +/** Number of nodes in the grid. */ +#define PATH_GRID_NODES_NB (PATH_COLUMNS_NB * PATH_COLUMN_NODES_NB) + +/** Number of nodes in search graph, last two nodes are destination and source + * nodes. */ +#define PATH_NODES_NB (PATH_GRID_NODES_NB + 2) + +/** Index of destination node. */ +#define PATH_DST_NODE_INDEX PATH_GRID_NODES_NB + +/** Index of source node. */ +#define PATH_SRC_NODE_INDEX (PATH_DST_NODE_INDEX + 1) + +/** Information on a node. */ +struct path_node_t +{ + /** Whether this node can be used. */ + uint8_t usable; +}; /** Context. */ struct path_t { + /** List of obstacles. */ + struct path_obstacle_t obstacles[PATH_OBSTACLES_NB]; + /** Escape factor, 0 if none. */ + uint8_t escape_factor; + /** List of nodes used for A*. */ + struct astar_node_t astar_nodes[PATH_NODES_NB]; + /** Cache of whether a node is blocked. */ + uint8_t valid[PATH_GRID_NODES_NB]; /** Position of end points. */ vect_t endpoints[2]; /** Whether the last update was a success. */ uint8_t found; + /** Which node to look at for next step. */ + uint8_t get; }; static struct path_t path; +/** Static information on nodes. */ +static const struct path_node_t path_nodes[PATH_GRID_NODES_NB] = { + /* {{{ */ + { 1 }, /* 0 column 0. */ + { 1 }, /* 1 */ + { 1 }, /* 2 */ + { 1 }, /* 3 */ + { 1 }, /* 4 column 1. */ + { 1 }, /* 5 */ + { 1 }, /* 6 */ + { 1 }, /* 7 */ + { 1 }, /* 8 column 2. */ + { 1 }, /* 9 */ + { 1 }, /* 10 */ + { 1 }, /* 11 */ + { 1 }, /* 12 column 3. */ + { 1 }, /* 13 */ + { 1 }, /* 14 */ + { 1 }, /* 15 */ + { 1 }, /* 16 column 4. */ + { 1 }, /* 17 */ + { 1 }, /* 18 */ + { 1 }, /* 19 */ + { 1 }, /* 20 column 5. */ + { 1 }, /* 21 */ + { 1 }, /* 22 */ + { 1 }, /* 23 */ + { 1 }, /* 24 column 6. */ + { 1 }, /* 25 */ + { 1 }, /* 26 */ + { 1 }, /* 27 */ + { 1 }, /* 28 column 7. */ + { 1 }, /* 29 */ + { 1 }, /* 30 */ + { 1 }, /* 31 */ + { 1 }, /* 32 column 8. */ + { 1 }, /* 33 */ + { 1 }, /* 34 */ + { 1 }, /* 35 */ + { 1 }, /* 36 column 9. */ + { 1 }, /* 37 */ + { 1 }, /* 38 */ + { 1 }, /* 39 */ + { 1 }, /* 40 column 10. */ + { 1 }, /* 41 */ + { 1 }, /* 42 */ + { 1 }, /* 43 */ + /* }}} */ +}; + +/** Compute position of a node. */ +static void +path_pos (uint8_t node, vect_t *pos) +{ + assert (node < PATH_NODES_NB); + if (node < PATH_GRID_NODES_NB) + { + uint8_t col = node / PATH_COLUMN_NODES_NB; + uint8_t line = node - col * PATH_COLUMN_NODES_NB; + pos->x = 400 + 50 + 350 / 2 + col * 350 / 2; + pos->y = 2100 - 350 - 350 / 2 + + (col % 2 ? 350 / 2 : 0) + - line * 350; + } + else + { + *pos = path.endpoints[node - PATH_GRID_NODES_NB]; + } +} + +/** Return 1 if the direct path between a and b nodes is blocked, also compute + * distance. */ +static uint8_t +path_blocking (uint8_t a, uint8_t b, int16_t *dp) +{ + uint8_t i; + vect_t va; + vect_t vb; + uint8_t escape_factor = 0; + if (a == PATH_SRC_NODE_INDEX || b == PATH_SRC_NODE_INDEX) + escape_factor = path.escape_factor; + path_pos (a, &va); + path_pos (b, &vb); + /* Test for a blocking obstacle. */ + for (i = 0; i < PATH_OBSTACLES_NB; i++) + { + if (path.obstacles[i].valid) + { + uint16_t d = distance_segment_point (&va, &vb, + &path.obstacles[i].c); + if (d < path.obstacles[i].r) + { + if (escape_factor) + { + int16_t d = distance_point_point (&va, &vb); + *dp = d * escape_factor; + return 0; + } + else + return 1; + } + } + } + /* Compute distance. */ + int16_t d = distance_point_point (&va, &vb); + if (d == 0) + { + *dp = 0; + return 0; + } + /* No blocking. */ + *dp = d; + return 0; +} + +/** Update the cache of blocked nodes. */ +static void +path_blocked_update (void) +{ + uint8_t i, j; + for (i = 0; i < PATH_GRID_NODES_NB; i++) + { + uint8_t valid = 1; + /* First, gather information from tables. */ + if (!path_nodes[i].usable) + valid = 0; + else + { + vect_t pos; + path_pos (i, &pos); + /* Then, test for obstacles. */ + for (j = 0; j < PATH_OBSTACLES_NB; j++) + { + if (path.obstacles[j].valid) + { + vect_t v = pos; vect_sub (&v, &path.obstacles[j].c); + uint32_t dsq = vect_dot_product (&v, &v); + uint32_t r = path.obstacles[j].r; + if (dsq <= r * r) + { + valid = 0; + break; + } + } + } + } + /* Update cache. */ + path.valid[i] = valid; + } +} + void path_init (void) { @@ -50,23 +267,51 @@ path_endpoints (vect_t s, vect_t d) void path_escape (uint8_t factor) { + path.escape_factor = factor; } void path_obstacle (uint8_t i, vect_t c, uint16_t r, uint8_t factor, uint16_t valid) { + assert (i < AC_PATH_OBSTACLES_NB); + assert (factor == 0); + path.obstacles[i].c = c; + path.obstacles[i].r = r; + path.obstacles[i].valid = valid; } void path_decay (void) { + uint8_t i; + for (i = 0; i < PATH_OBSTACLES_NB; i++) + { + if (path.obstacles[i].valid + && path.obstacles[i].valid != PATH_OBSTACLE_VALID_ALWAYS) + path.obstacles[i].valid--; + } } void path_update (void) { - path.found = 1; + path_blocked_update (); + path.found = astar (path.astar_nodes, PATH_NODES_NB, PATH_DST_NODE_INDEX, + PATH_SRC_NODE_INDEX); + path.get = PATH_SRC_NODE_INDEX; +#if AC_PATH_REPORT + if (path.found) + { + uint8_t n, len = 0; + vect_t points[PATH_NODES_NB]; + for (n = path.get; n != PATH_DST_NODE_INDEX; n = path.astar_nodes[n].prev) + path_pos (n, &points[len++]); + path_pos (n, &points[len++]); + AC_PATH_REPORT_CALLBACK (points, len, path.obstacles, + PATH_OBSTACLES_NB); + } +#endif } uint8_t @@ -74,10 +319,159 @@ path_get_next (vect_t *p) { if (path.found) { - *p = path.endpoints[0]; + assert (path.get != PATH_DST_NODE_INDEX); + uint8_t prev = path.get; + vect_t pp; + path_pos (prev, &pp); + uint8_t next = path.astar_nodes[path.get].prev; + path.get = next; + path_pos (next, p); + while (next != 0xff) + { + /* Try to remove useless points. */ + uint8_t next = path.astar_nodes[path.get].prev; + if (next == 0xff || next == PATH_DST_NODE_INDEX) + break; + vect_t np; + path_pos (next, &np); + vect_t vnp = np; vect_sub (&vnp, &pp); + vect_t vp = *p; vect_sub (&vp, &pp); + if (vect_normal_dot_product (&vp, &vnp) == 0) + { + path.get = next; + *p = np; + } + else + break; + } return 1; } else return 0; } +/** Neighbors callback for nodes in grid. */ +static uint8_t +path_astar_neighbor_callback_grid (uint8_t node, + struct astar_neighbor_t *neighbors) +{ + uint8_t neighbors_nb = 0; + uint8_t i; + /* Add neighbors in all 8 directions. */ + static const struct { + /** Column offset of this neighbor. */ + int8_t column_offset; + /** Line offset of this neighbor, for even columns. */ + int8_t even_line_offset; + /** Line offset for odd columns. */ + int8_t odd_line_offset; + /** Distance to this neighbor. */ + uint16_t weight; + } star_n[] = { + { 0, -1, -1, 350 }, /* N */ + { -1, 0, -1, 248 }, /* NW */ + { -2, 0, 0, 350 }, /* W */ + { -1, 1, 0, 248 }, /* SW */ + { 0, 1, 1, 350 }, /* S */ + { 1, 1, 0, 248 }, /* SE */ + { 2, 0, 0, 350 }, /* E */ + { 1, 0, -1, 248 }, /* NE */ + }; + uint8_t col = node / PATH_COLUMN_NODES_NB; + uint8_t line = node - col * PATH_COLUMN_NODES_NB; + uint8_t odd = col % 2; + for (i = 0; i < UTILS_COUNT (star_n); i++) + { + int8_t new_col = col + star_n[i].column_offset; + int8_t new_line = line + (odd ? star_n[i].odd_line_offset + : star_n[i].even_line_offset); + int8_t new_node = new_col * PATH_COLUMN_NODES_NB + new_line; + if (new_col >= 0 && new_col < PATH_COLUMNS_NB + && new_line >= 0 && new_line < PATH_COLUMN_NODES_NB) + { + uint8_t valid = path.valid[new_node]; + if (valid) + { + neighbors[neighbors_nb].node = new_node; + neighbors[neighbors_nb].weight = star_n[i].weight + 1; + neighbors_nb++; + } + } + } + /* Check if direct path OK. */ + int16_t d; + if (!path_blocking (node, PATH_SRC_NODE_INDEX, &d)) + { + /* Add this neighbor. */ + neighbors[neighbors_nb].node = PATH_SRC_NODE_INDEX; + neighbors[neighbors_nb].weight = d + 1; + neighbors_nb++; + } +#if PATH_DEBUG + for (i = 0; i < neighbors_nb; i++) + DPRINTF (" n %d %d\n", neighbors[i].node, neighbors[i].weight); +#endif + return neighbors_nb; +} + +/** Neighbors callback for endpoints. */ +static uint8_t +path_astar_neighbor_callback_endpoints (uint8_t node, + struct astar_neighbor_t *neighbors) +{ + uint8_t neighbors_nb = 0; + uint8_t i; + assert (node == PATH_DST_NODE_INDEX); + /* Select neighbors in the grid. */ + for (i = 0; i < PATH_GRID_NODES_NB; i++) + { + /* Discard blocking nodes. */ + if (!path.valid[i]) + continue; + /* Check if there is an obstacle along the path. */ + int16_t d; + if (path_blocking (PATH_DST_NODE_INDEX, i, &d)) + continue; + /* Add this neighbor. */ + neighbors[neighbors_nb].node = i; + neighbors[neighbors_nb].weight = d + 1; + neighbors_nb++; + } + /* Check if direct path OK. */ + int16_t d; + if (!path_blocking (PATH_DST_NODE_INDEX, PATH_SRC_NODE_INDEX, &d)) + { + /* Add this neighbor. */ + neighbors[neighbors_nb].node = PATH_SRC_NODE_INDEX; + neighbors[neighbors_nb].weight = d + 1; + neighbors_nb++; + } +#if PATH_DEBUG + for (i = 0; i < neighbors_nb; i++) + DPRINTF (" n %d %d\n", neighbors[i].node, neighbors[i].weight); +#endif + return neighbors_nb; +} + +uint8_t +path_astar_neighbor_callback (uint8_t node, + struct astar_neighbor_t *neighbors) +{ +#if PATH_DEBUG + DPRINTF ("neighbor %d\n", node); +#endif + if (node < PATH_GRID_NODES_NB) + return path_astar_neighbor_callback_grid (node, neighbors); + else + return path_astar_neighbor_callback_endpoints (node, neighbors); +} + +uint16_t +path_astar_heuristic_callback (uint8_t node) +{ + /* TODO: a better and faster heuristic can be found, considering that + * movement is only allowed on the grid. */ + vect_t pos; + path_pos (node, &pos); + return distance_point_point (&pos, &path.endpoints[0]); +} diff --git a/digital/io-hub/src/robospierre/simu.host.c b/digital/io-hub/src/robospierre/simu.host.c index 7ca7364b..70055d7d 100644 --- a/digital/io-hub/src/robospierre/simu.host.c +++ b/digital/io-hub/src/robospierre/simu.host.c @@ -29,6 +29,7 @@ #include "modules/host/host.h" #include "modules/host/mex.h" #include "modules/adc/adc.h" +#include "modules/path/path.h" #include "io.h" /** AVR registers. */ @@ -36,6 +37,7 @@ uint8_t PORTA, DDRA, PINA, PINE, PINF; /** Message types. */ uint8_t simu_mex_pos_report; +uint8_t simu_mex_path; static void simu_adc_handle (void *user, mex_msg_t *msg) @@ -56,6 +58,7 @@ simu_init (void) uint8_t mtype = mex_node_reservef ("%s:adc", mex_instance); mex_node_register (mtype, simu_adc_handle, 0); simu_mex_pos_report = mex_node_reservef ("%s:pos-report", mex_instance); + simu_mex_path = mex_node_reservef ("%s:path", mex_instance); } /** Make a simulation step. */ @@ -78,6 +81,19 @@ timer_wait (void) return 0; } +/** Send computed path. */ +void +simu_send_path (vect_t *points, uint8_t len, + struct path_obstacle_t *obstacles, uint8_t obstacles_nb) +{ + int i; + mex_msg_t *m; + m = mex_msg_new (simu_mex_path); + for (i = 0; i < len; i++) + mex_msg_push (m, "hh", points[i].x, points[i].y); + mex_node_send (m); +} + void simu_send_pos_report (vect_t *pos, uint8_t pos_nb, uint8_t id) { diff --git a/digital/io-hub/tools/io_hub/mex.py b/digital/io-hub/tools/io_hub/mex.py index 66060ea1..8d758382 100644 --- a/digital/io-hub/tools/io_hub/mex.py +++ b/digital/io-hub/tools/io_hub/mex.py @@ -121,6 +121,24 @@ class Mex: m.push ('L', self.contacts) self.node.send (m) + class Path (Observable): + """Path finding algorithm report. + + - path: sequence of (x, y) coordinates (millimeters). + + """ + + def __init__ (self, node, instance): + Observable.__init__ (self) + self.path = [ ] + node.register (instance + ':path', self.__handle) + + def __handle (self, msg): + self.path = [ ] + while len (msg) >= 4: + self.path.append (msg.pop ('hh')) + self.notify () + class PosReport (Observable): """General purpose position report. @@ -149,5 +167,6 @@ class Mex: self.__contact_pack = self.Contact.Pack (node, instance) self.contact = tuple (self.Contact (self.__contact_pack, i) for i in range (CONTACT_NB)) + self.path = self.Path (node, instance) self.pos_report = self.PosReport (node, instance) diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py index b98dafd9..284e5bb8 100644 --- a/host/simu/robots/robospierre/model/bag.py +++ b/host/simu/robots/robospierre/model/bag.py @@ -55,5 +55,6 @@ class Bag: DistanceSensorSensopart (link_bag.io_hub.adc[3], scheduler, table, (-20, 20), pi - pi * 10 / 180, (self.position, ), 2), ] + self.path = link_bag.io_hub.path self.pos_report = link_bag.io_hub.pos_report diff --git a/host/simu/robots/robospierre/view/bag.py b/host/simu/robots/robospierre/view/bag.py index b718f6fe..d87cecb9 100644 --- a/host/simu/robots/robospierre/view/bag.py +++ b/host/simu/robots/robospierre/view/bag.py @@ -45,5 +45,6 @@ class Bag: ClampSide.height), model_bag.clamp)) self.distance_sensor = [DistanceSensorUS (self.robot, ds) for ds in model_bag.distance_sensor] + self.path = Path (table, model_bag.path) self.pos_report = PosReport (table, model_bag.pos_report) -- cgit v1.2.3 From 51b917f7994d6d53fde11263ba4da3470fd2a6c4 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 1 Jun 2011 15:26:22 +0200 Subject: digital/io-hub, host/simu: add codebar --- digital/io-hub/src/robospierre/Makefile | 1 + digital/io-hub/src/robospierre/codebar.avr.c | 64 +++++++++++++++++++++++++++ digital/io-hub/src/robospierre/codebar.h | 36 +++++++++++++++ digital/io-hub/src/robospierre/codebar.host.c | 56 +++++++++++++++++++++++ digital/io-hub/src/robospierre/main.c | 15 +++++++ digital/io-hub/src/robospierre/pawn_sensor.c | 5 ++- digital/io-hub/tools/io_hub/mex.py | 43 ++++++++++++++++++ host/simu/robots/robospierre/model/bag.py | 2 +- host/simu/robots/robospierre/model/clamp.py | 15 +++++-- 9 files changed, 231 insertions(+), 6 deletions(-) create mode 100644 digital/io-hub/src/robospierre/codebar.avr.c create mode 100644 digital/io-hub/src/robospierre/codebar.h create mode 100644 digital/io-hub/src/robospierre/codebar.host.c (limited to 'host/simu/robots/robospierre') diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index 714541dc..88e6c79e 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -6,6 +6,7 @@ HOST_PROGS = test_element # Sources to compile. io_hub_SOURCES = main.c top.c \ clamp.c logistic.c element.c pawn_sensor.c \ + codebar.avr.c codebar.host.c \ radar_defs.c radar.c path.c move.c \ init.c fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \ pwm.avr.c pwm.host.c \ diff --git a/digital/io-hub/src/robospierre/codebar.avr.c b/digital/io-hub/src/robospierre/codebar.avr.c new file mode 100644 index 00000000..d2609012 --- /dev/null +++ b/digital/io-hub/src/robospierre/codebar.avr.c @@ -0,0 +1,64 @@ +/* codebar.avr.c */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "codebar.h" + +#include "defs.h" + +#include "modules/twi/twi.h" +#include "modules/utils/utils.h" +#include "modules/utils/crc.h" +#include "modules/utils/byte.h" + +#define CODEBAR_ADDRESS 0x20 +#define CODEBAR_STATUS_LENGTH 7 + +void +codebar_init (void) +{ +} + +uint8_t +codebar_get (uint8_t direction) +{ + uint8_t buffer[CODEBAR_STATUS_LENGTH]; + /* Read status. */ + twi_master_recv (CODEBAR_ADDRESS, buffer, sizeof (buffer)); + uint8_t ret = twi_master_wait (); + if (ret != CODEBAR_STATUS_LENGTH) + return 0; + uint8_t crc = crc_compute (buffer + 1, CODEBAR_STATUS_LENGTH - 1); + if (crc != buffer[0]) + return 0; + /* Get data. */ + uint8_t offset = direction == DIRECTION_FORWARD ? 1 : 4; + uint16_t age = v8_to_v16 (buffer[offset], buffer[offset + 1]); + uint16_t type = buffer[offset + 2]; + if (age > 225) + return 0; + else + return type; +} + diff --git a/digital/io-hub/src/robospierre/codebar.h b/digital/io-hub/src/robospierre/codebar.h new file mode 100644 index 00000000..d334f131 --- /dev/null +++ b/digital/io-hub/src/robospierre/codebar.h @@ -0,0 +1,36 @@ +#ifndef codebar_h +#define codebar_h +/* codebar.h */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/** Initialise module. */ +void +codebar_init (void); + +/** Get element type on the specified direction. */ +uint8_t +codebar_get (uint8_t direction); + +#endif /* codebar_h */ diff --git a/digital/io-hub/src/robospierre/codebar.host.c b/digital/io-hub/src/robospierre/codebar.host.c new file mode 100644 index 00000000..68ced300 --- /dev/null +++ b/digital/io-hub/src/robospierre/codebar.host.c @@ -0,0 +1,56 @@ +/* codebar.host.c */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "codebar.h" +#include "defs.h" + +#include "modules/host/host.h" +#include "modules/host/mex.h" + +uint8_t codebar_front, codebar_back; + +static void +codebar_handle (void *user, mex_msg_t *msg) +{ + mex_msg_pop (msg, "BB", &codebar_front, &codebar_back); +} + +void +codebar_init (void) +{ + const char *mex_instance = host_get_instance ("io-hub0", 0); + uint8_t mtype = mex_node_reservef ("%s:codebar", mex_instance); + mex_node_register (mtype, codebar_handle, 0); +} + +uint8_t +codebar_get (uint8_t direction) +{ + if (direction == DIRECTION_FORWARD) + return codebar_front; + else + return codebar_back; +} + diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 79c0fe2a..42a2809a 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -40,6 +40,7 @@ #include "pwm.h" #include "contact.h" +#include "codebar.h" #include "radar.h" #define FSM_NAME AI @@ -77,6 +78,9 @@ static uint8_t main_stats_asserv_, main_stats_asserv_cpt_; /** Contact stats counters. */ static uint8_t main_stats_contact_, main_stats_contact_cpt_; +/** Codebar stats counters. */ +static uint8_t main_stats_codebar_, main_stats_codebar_cpt_; + /** US sensors stats counters. */ static uint8_t main_stats_usdist_, main_stats_usdist_cpt_; @@ -103,6 +107,7 @@ main_init (void) /* IO modules. */ pwm_init (); contact_init (); + codebar_init (); usdist_init (); /* AI modules. */ clamp_init (); @@ -223,6 +228,12 @@ main_loop (void) proto_send1d ('P', contact_all ()); main_stats_contact_cpt_ = main_stats_contact_; } + if (main_stats_codebar_ && !--main_stats_codebar_cpt_) + { + proto_send2b ('B', codebar_get (DIRECTION_FORWARD), + codebar_get (DIRECTION_BACKWARD)); + main_stats_codebar_cpt_ = main_stats_codebar_; + } if (main_stats_usdist_ && !--main_stats_usdist_cpt_) { proto_send4w ('U', usdist_mm[0], usdist_mm[1], usdist_mm[2], @@ -331,6 +342,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Contact stats. */ main_stats_contact_ = main_stats_contact_cpt_ = args[0]; break; + case c ('B', 1): + /* Codebar stats. */ + main_stats_codebar_ = main_stats_codebar_cpt_ = args[0]; + break; case c ('U', 1): /* US sensors stats. */ main_stats_usdist_ = main_stats_usdist_cpt_ = args[0]; diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c index be6f3c87..cb4cfe4a 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.c +++ b/digital/io-hub/src/robospierre/pawn_sensor.c @@ -31,6 +31,7 @@ #include "element.h" #include "clamp.h" #include "bot.h" +#include "codebar.h" #include "modules/utils/utils.h" #include "modules/math/geometry/distance.h" @@ -53,7 +54,9 @@ struct pawn_sensor_t pawn_sensor_front, pawn_sensor_back; static uint8_t pawn_sensor_get_type (uint8_t direction) { - uint8_t element_type = IO_GET (CONTACT_STRAT) ? ELEMENT_PAWN : ELEMENT_KING; + uint8_t element_type = codebar_get (direction); + if (!element_type) + element_type = ELEMENT_PAWN; return element_type; } diff --git a/digital/io-hub/tools/io_hub/mex.py b/digital/io-hub/tools/io_hub/mex.py index 8d758382..9b72ccde 100644 --- a/digital/io-hub/tools/io_hub/mex.py +++ b/digital/io-hub/tools/io_hub/mex.py @@ -121,6 +121,46 @@ class Mex: m.push ('L', self.contacts) self.node.send (m) + class Codebar (Observable): + """Codebar stub. + + - element_type: 'queen', 'king', or anything else. + + """ + + def __init__ (self, pack, index): + Observable.__init__ (self) + self.pack = pack + self.index = index + self.element_type = None + self.register (self.__notified) + + def __notified (self): + self.pack.set (self.index, self.element_type) + + class Pack: + """Handle emission of several codebar for one message.""" + + def __init__ (self, node, instance): + self.node = node + self.codebars = [0, 0] + self.mtype = node.reserve (instance + ':codebar') + + def set (self, index, element_type): + if element_type == 'queen': + self.codebars[index] = 4 + elif element_type == 'king': + self.codebars[index] = 8 + else: + self.codebars[index] = 0 + self.__send () + + def __send (self): + m = simu.mex.msg.Msg (self.mtype) + for c in self.codebars: + m.push ('b', c) + self.node.send (m) + class Path (Observable): """Path finding algorithm report. @@ -167,6 +207,9 @@ class Mex: self.__contact_pack = self.Contact.Pack (node, instance) self.contact = tuple (self.Contact (self.__contact_pack, i) for i in range (CONTACT_NB)) + self.__codebar_pack = self.Codebar.Pack (node, instance) + self.codebar = tuple (self.Codebar (self.__codebar_pack, i) + for i in (0, 1)) self.path = self.Path (node, instance) self.pos_report = self.PosReport (node, instance) diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py index 284e5bb8..b9d3bc3f 100644 --- a/host/simu/robots/robospierre/model/bag.py +++ b/host/simu/robots/robospierre/model/bag.py @@ -44,7 +44,7 @@ class Bag: 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]) + self.contact[0:7], link_bag.io_hub.codebar) 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 0af4ccf3..dbcab5f2 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -30,13 +30,14 @@ from math import pi, cos, sin class Slot: """Slot which can contain a pawn.""" - def __init__ (self, x, y, z, side, door_motor, contact): + def __init__ (self, x, y, z, side, door_motor, contact, codebar = None): self.x = x self.y = y self.z = z self.side = side self.door_motor = door_motor self.contact = contact + self.codebar = codebar self.pawn = None class Clamp (Observable): @@ -62,7 +63,8 @@ class Clamp (Observable): SLOT_SIDE = 6 def __init__ (self, table, robot_position, elevation_motor, - rotation_motor, clamping_motor, door_motors, slot_contacts): + rotation_motor, clamping_motor, door_motors, slot_contacts, + codebars): Observable.__init__ (self) self.table = table self.robot_position = robot_position @@ -75,13 +77,13 @@ class Clamp (Observable): door_motors[2], None, door_motors[3], None) self.slots = ( Slot (self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 0, - door_motors[0], slot_contacts[0]), + door_motors[0], slot_contacts[0], codebars[0]), Slot (self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 0, None, slot_contacts[1]), Slot (self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 0, door_motors[1], slot_contacts[2]), Slot (-self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 1, - door_motors[2], slot_contacts[3]), + door_motors[2], slot_contacts[3], codebars[1]), Slot (-self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 1, None, slot_contacts[4]), Slot (-self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 1, @@ -203,6 +205,11 @@ class Clamp (Observable): and slots[0].pawn.kind == 'tower')) # This one is really high. slots[2].contact.state = not (slots[2].pawn is not None) + if slots[0].pawn: + slots[0].codebar.element_type = slots[0].pawn.kind + else: + slots[0].codebar.element_type = None + slots[0].codebar.notify () slot_side = self.slots[self.SLOT_SIDE] slot_side.contact.state = slot_side.pawn is None clamp_slot = self.__get_clamp_slot () -- cgit v1.2.3 From 4eb705e639457a07e6d5e9423ba81f8cede1b979 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 04:37:35 +0200 Subject: digital/io-hub: handle element change to head --- digital/io-hub/src/robospierre/clamp.c | 39 +++++++++++++++++++++++++++-- digital/io-hub/src/robospierre/logistic.c | 8 ++++++ digital/io-hub/src/robospierre/logistic.h | 4 +++ host/simu/robots/robospierre/model/clamp.py | 6 ++--- 4 files changed, 52 insertions(+), 5 deletions(-) (limited to 'host/simu/robots/robospierre') diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index e05f79d7..6b48c56a 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -373,11 +373,17 @@ clamp_route (void) } else if (pos_current == CLAMP_BAY_FRONT_LEAVE) { - pos_new = CLAMP_BAY_FRONT_LEAVING; + if (CLAMP_IS_SLOT_IN_FRONT_BAY (pos_request)) + pos_new = pos_request; + else + pos_new = CLAMP_BAY_FRONT_LEAVING; } else if (pos_current == CLAMP_BAY_BACK_LEAVE) { - pos_new = CLAMP_BAY_BACK_LEAVING; + if (CLAMP_IS_SLOT_IN_BACK_BAY (pos_request)) + pos_new = pos_request; + else + pos_new = CLAMP_BAY_BACK_LEAVING; } else if (pos_current == CLAMP_BAY_FRONT_LEAVING) { @@ -417,6 +423,34 @@ clamp_route (void) ctx.pos_current = pos_new; } +/* When lifting an element, we can discover it is actually a head. In this + * case, change destination. */ +void +clamp_change (void) +{ + uint8_t from = logistic_global.moving_from; + if (logistic_global.slots[from] == ELEMENT_PAWN) + { + /* Look head contact. */ + uint8_t contact_head = 0; + if (from == CLAMP_SLOT_FRONT_BOTTOM) + contact_head = !IO_GET (CONTACT_FRONT_TOP); + else if (from == CLAMP_SLOT_BACK_BOTTOM) + contact_head = !IO_GET (CONTACT_BACK_TOP); + /* Change? */ + if (contact_head) + { + logistic_element_change (from, ELEMENT_KING); + if (logistic_global.moving_from != from) + /* Cancel move. */ + ctx.pos_request = ctx.moving_to = from; + else + /* Change move. */ + ctx.pos_request = ctx.moving_to = logistic_global.moving_to; + } + } +} + static void clamp_blocked (void) { @@ -861,6 +895,7 @@ FSM_TRANS (CLAMP_MOVE_DST_ROUTING, clamp_elevation_rotation_success, done_open_clamp, CLAMP_MOVE_DST_CLAMP_OPENING, next, CLAMP_MOVE_DST_ROUTING) { + clamp_change (); if (ctx.pos_current == ctx.pos_request) { if (clamp_slot_door[ctx.pos_current] != 0xff) diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index dea6e0a5..f59f2da4 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -533,6 +533,14 @@ logistic_element_new (uint8_t pos, uint8_t element_type) logistic_decision (); } +void +logistic_element_change (uint8_t pos, uint8_t element_type) +{ + assert (pos < CLAMP_SLOT_NB); + ctx.slots[pos] = element_type; + logistic_decision (); +} + void logistic_element_move_done (void) { diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h index 6acb681e..4a06dcb7 100644 --- a/digital/io-hub/src/robospierre/logistic.h +++ b/digital/io-hub/src/robospierre/logistic.h @@ -115,6 +115,10 @@ logistic_update (void); void logistic_element_new (uint8_t pos, uint8_t element_type); +/** Oh la la, the pawn was not a pawn, it's a head. */ +void +logistic_element_change (uint8_t pos, uint8_t element_type); + /** To be called when a element movement is done. */ void logistic_element_move_done (void); diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index dbcab5f2..c199259e 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -203,8 +203,7 @@ class Clamp (Observable): slots[1].pawn is not None or (slots[0].pawn is not None and slots[0].pawn.kind == 'tower')) - # This one is really high. - slots[2].contact.state = not (slots[2].pawn is not None) + slots[2].contact.state = True if slots[0].pawn: slots[0].codebar.element_type = slots[0].pawn.kind else: @@ -213,7 +212,8 @@ class Clamp (Observable): slot_side = self.slots[self.SLOT_SIDE] slot_side.contact.state = slot_side.pawn is None clamp_slot = self.__get_clamp_slot () - if clamp_slot is not None: + if clamp_slot is not None and clamp_slot != self.SLOT_FRONT_TOP \ + and clamp_slot != self.SLOT_BACK_TOP: clamp_slot.contact.state = False for slot in self.slots: slot.contact.notify () -- cgit v1.2.3 From f9f6fbde971e8a7d4698e197b09a0ea2dd8a8526 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 05:22:43 +0200 Subject: host/simu/robots/robospierre: fix level 2 drop --- host/simu/robots/robospierre/model/clamp.py | 3 +++ 1 file changed, 3 insertions(+) (limited to 'host/simu/robots/robospierre') diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index c199259e..d32edf25 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -193,6 +193,9 @@ class Clamp (Observable): and slots[2].pawn and slots[2].door_motor.angle: slots[0].pawn.tower.append (slots[2].pawn) slots[2].pawn = None + if slots[0].pawn is None and slots[1].pawn is None \ + and slots[2].pawn and slots[2].door_motor.angle: + slots[0].pawn, slots[2].pawn = slots[2].pawn, None def update_contacts (self): """Update pawn contacts.""" -- cgit v1.2.3 From 53483bc33fd092d70be211fcaba1db5372e1706d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 16:35:00 +0200 Subject: digital/io-hub: fix bug in head detection workaround --- digital/io-hub/src/robospierre/clamp.c | 25 +++++++++++++++++++++---- digital/io-hub/src/robospierre/logistic.c | 2 +- host/simu/robots/robospierre/model/clamp.py | 2 +- 3 files changed, 23 insertions(+), 6 deletions(-) (limited to 'host/simu/robots/robospierre') diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index f91af6a0..a74c643c 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -161,6 +161,8 @@ struct clamp_t uint8_t open; /** True if clamp position is controled. */ uint8_t controled; + /** Contact state at start of move, for head check. */ + uint8_t contact_head_before_move; }; /** Global context. */ @@ -212,6 +214,9 @@ clamp_openclose (uint8_t open); static void clamp_route (void); +static void +clamp_head_check_prepare (uint8_t from); + void clamp_init (void) { @@ -243,6 +248,7 @@ clamp_move_element (uint8_t from, uint8_t to) assert (from != to); ctx.pos_request = from; ctx.moving_to = to; + clamp_head_check_prepare (from); FSM_HANDLE (AI, clamp_move); } @@ -435,11 +441,11 @@ clamp_taken_pawn (uint8_t element_type) /* When lifting an element, we can discover it is actually a head. In this * case, change destination. */ -void -clamp_change (void) +static void +clamp_head_check (void) { uint8_t from = logistic_global.moving_from; - if (logistic_global.slots[from] == ELEMENT_PAWN) + if (!ctx.contact_head_before_move && logistic_global.slots[from] == ELEMENT_PAWN) { /* Look head contact. */ uint8_t contact_head = 0; @@ -462,6 +468,17 @@ clamp_change (void) } } +/** Prepare head check, if contact is set yet, this is not a new head. */ +static void +clamp_head_check_prepare (uint8_t from) +{ + ctx.contact_head_before_move = 0; + if (CLAMP_IS_SLOT_IN_FRONT_BAY (from)) + ctx.contact_head_before_move = !IO_GET (CONTACT_FRONT_TOP); + else if (CLAMP_IS_SLOT_IN_BACK_BAY (from)) + ctx.contact_head_before_move = !IO_GET (CONTACT_BACK_TOP); +} + static void clamp_blocked (void) { @@ -909,7 +926,7 @@ FSM_TRANS (CLAMP_MOVE_DST_ROUTING, clamp_elevation_rotation_success, done_open_clamp, CLAMP_MOVE_DST_CLAMP_OPENING, next, CLAMP_MOVE_DST_ROUTING) { - clamp_change (); + clamp_head_check (); if (ctx.pos_current == ctx.pos_request) { if (clamp_slot_door[ctx.pos_current] != 0xff) diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index f59f2da4..f9b13427 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -544,7 +544,7 @@ logistic_element_change (uint8_t pos, uint8_t element_type) void logistic_element_move_done (void) { - assert (!ctx.slots[ctx.moving_to]); + assert (!ctx.slots[ctx.moving_to] || ctx.moving_to == ctx.moving_from); ctx.slots[ctx.moving_to] = ctx.slots[ctx.moving_from]; ctx.slots[ctx.moving_from] = 0; ctx.moving_from = ctx.moving_to = CLAMP_SLOT_NB; diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index d32edf25..4bbe988f 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -206,7 +206,7 @@ class Clamp (Observable): slots[1].pawn is not None or (slots[0].pawn is not None and slots[0].pawn.kind == 'tower')) - slots[2].contact.state = True + slots[2].contact.state = not (slots[2] is not None) if slots[0].pawn: slots[0].codebar.element_type = slots[0].pawn.kind else: -- cgit v1.2.3 From 1ba50389bff517512e417500f4c52ceeafdb06a5 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 13 Jun 2011 00:40:31 +0200 Subject: host/simu: randomize team color --- host/simu/robots/robospierre/model/bag.py | 3 +++ host/simu/view/switch.py | 2 ++ 2 files changed, 5 insertions(+) (limited to 'host/simu/robots/robospierre') diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py index b9d3bc3f..cedb3805 100644 --- a/host/simu/robots/robospierre/model/bag.py +++ b/host/simu/robots/robospierre/model/bag.py @@ -28,11 +28,14 @@ from simu.model.motor_basic import MotorBasic from simu.model.distance_sensor_sensopart import DistanceSensorSensopart from simu.robots.robospierre.model.clamp import Clamp from math import pi +import random class Bag: def __init__ (self, scheduler, table, link_bag): self.color_switch = Switch (link_bag.io_hub.contact[0], invert = True) + self.color_switch.state = random.choice ((False, True)) + self.color_switch.notify () self.jack = Switch (link_bag.io_hub.contact[1], invert = True) self.strat_switch = Switch (link_bag.io_hub.contact[9], invert = True) self.contact = [ Switch (contact) diff --git a/host/simu/view/switch.py b/host/simu/view/switch.py index c0ec2d58..445cbd02 100644 --- a/host/simu/view/switch.py +++ b/host/simu/view/switch.py @@ -28,6 +28,8 @@ class Switch: def __init__ (self, frame, model, text): self.var = IntVar () + if model.state is not None: + self.var.set ((0, 1)[model.state]) self.button = Checkbutton (frame, variable = self.var, command = self.__update, text = text, indicatoron = False) self.button.pack () -- cgit v1.2.3 From 8ddd21a6f3c65cd94b794cb25e6fa74dc20da3cc Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 9 Feb 2012 21:23:47 +0100 Subject: digital/ai/tools, host/simu/robots: simulate several robots --- digital/ai/tools/marcel.py | 58 ++++++++++++----------- digital/ai/tools/robospierre.py | 62 ++++++++++++++----------- digital/ai/tools/test_simu.py | 80 +++++++++++++------------------- host/simu/robots/aquajim/link/bag.py | 6 +-- host/simu/robots/giboulee/link/bag.py | 6 +-- host/simu/robots/marcel/link/bag.py | 8 ++-- host/simu/robots/robospierre/link/bag.py | 8 ++-- 7 files changed, 113 insertions(+), 115 deletions(-) (limited to 'host/simu/robots/robospierre') diff --git a/digital/ai/tools/marcel.py b/digital/ai/tools/marcel.py index 08314ca3..001eca5e 100644 --- a/digital/ai/tools/marcel.py +++ b/digital/ai/tools/marcel.py @@ -1,10 +1,3 @@ -import simu.model.table_eurobot2010 -import simu.view.table_eurobot2010 - -import simu.robots.marcel.link.bag -import simu.robots.marcel.model.bag -import simu.robots.marcel.view.bag - import asserv import asserv.init import mimot @@ -16,24 +9,37 @@ from proto.popen_io import PopenIO import math class Robot: + """Marcel robot instance.""" + + import simu.model.table_eurobot2010 as table_model + import simu.view.table_eurobot2010 as table_view + + import simu.robots.marcel.link.bag as robot_link + import simu.robots.marcel.model.bag as robot_model + import simu.robots.marcel.view.bag as robot_view + + robot_start_pos = { + False: (300, 2100 - 305, math.radians (-270)), + True: (3000 - 300, 2100 - 305, math.radians (-270)) + } + + client_nb = 3 - def __init__ (self, proto_time): - self.table_model = simu.model.table_eurobot2010 - self.table_view = simu.view.table_eurobot2010 - self.robot_link = simu.robots.marcel.link.bag - self.robot_model = simu.robots.marcel.model.bag - self.robot_view = simu.robots.marcel.view.bag - asserv_cmd = ('../../asserv/src/asserv/asserv.host', '-m9', 'marcel') - mimot_cmd = ('../../mimot/src/dirty/dirty.host', '-m9', 'marcel') - io_cmd = ('../../io/src/io.host') - self.asserv = asserv.Proto (PopenIO (asserv_cmd), proto_time, - **asserv.init.host['marcel']) - self.mimot = mimot.Proto (PopenIO (mimot_cmd), proto_time, - **mimot.init.host['marcel']) - self.io = io.Proto (PopenIO (io_cmd), proto_time, - **io.init.host['marcel']) - self.robot_start_pos = { - False: (300, 2100 - 305, math.radians (-270)), - True: (3000 - 300, 2100 - 305, math.radians (-270)) - } + def __init__ (self, proto_time, instance = 'robot0'): + self.instance = instance + def proto (proto_class, cmd, init): + cmd = [ s.format (instance = instance) for s in cmd ] + return proto_class (PopenIO (cmd), proto_time, **init) + asserv_cmd = ('../../asserv/src/asserv/asserv.host', + '-i{instance}:asserv0', '-m9', 'marcel') + mimot_cmd = ('../../mimot/src/dirty/dirty.host', + '-i{instance}:mimot0', '-m9', 'marcel') + io_cmd = ('../../io/src/io.host', '-i{instance}:io0') + self.asserv = proto (asserv.Proto, asserv_cmd, + asserv.init.host['marcel']) + self.mimot = proto (mimot.Proto, mimot_cmd, + mimot.init.host['marcel']) + self.io = proto (io.Proto, io_cmd, + io.init.host['marcel']) + self.protos = (self.asserv, self.mimot, self.io) diff --git a/digital/ai/tools/robospierre.py b/digital/ai/tools/robospierre.py index 6a44883e..0d258b34 100644 --- a/digital/ai/tools/robospierre.py +++ b/digital/ai/tools/robospierre.py @@ -1,10 +1,3 @@ -import simu.model.table_eurobot2011 -import simu.view.table_eurobot2011 - -import simu.robots.robospierre.link.bag -import simu.robots.robospierre.model.bag -import simu.robots.robospierre.view.bag - import asserv import asserv.init import mimot @@ -16,26 +9,39 @@ from proto.popen_io import PopenIO import math class Robot: + """Robospierre robot instance.""" + + import simu.model.table_eurobot2011 as table_model + import simu.view.table_eurobot2011 as table_view + + import simu.robots.robospierre.link.bag as robot_link + import simu.robots.robospierre.model.bag as robot_model + import simu.robots.robospierre.view.bag as robot_view + + robot_start_pos = { + # In real life, better place the robot in green zone. + False: (300, 2100 - 200, math.radians (180)), + True: (3000 - 300, 2100 - 200, math.radians (0)) + } + + client_nb = 3 - def __init__ (self, proto_time): - self.table_model = simu.model.table_eurobot2011 - self.table_view = simu.view.table_eurobot2011 - self.robot_link = simu.robots.robospierre.link.bag - self.robot_model = simu.robots.robospierre.model.bag - self.robot_view = simu.robots.robospierre.view.bag - asserv_cmd = ('../../asserv/src/asserv/asserv.host', '-m9', - 'robospierre') - mimot_cmd = ('../../mimot/src/dirty/dirty.host', '-m9', 'robospierre') - io_hub_cmd = ('../../io-hub/src/robospierre/io_hub.host') - self.asserv = asserv.Proto (PopenIO (asserv_cmd), proto_time, - **asserv.init.host['robospierre']) - self.mimot = mimot.Proto (PopenIO (mimot_cmd), proto_time, - **mimot.init.host['robospierre']) - self.io = io_hub.Proto (PopenIO (io_hub_cmd), proto_time, - **io_hub.init.host['robospierre']) - self.robot_start_pos = { - # In real life, better place the robot in green zone. - False: (300, 2100 - 200, math.radians (180)), - True: (3000 - 300, 2100 - 200, math.radians (0)) - } + def __init__ (self, proto_time, instance = 'robot0'): + self.instance = instance + def proto (proto_class, cmd, init): + cmd = [ s.format (instance = instance) for s in cmd ] + return proto_class (PopenIO (cmd), proto_time, **init) + asserv_cmd = ('../../asserv/src/asserv/asserv.host', + '-i{instance}:asserv0', '-m9', 'robospierre') + mimot_cmd = ('../../mimot/src/dirty/dirty.host', + '-i{instance}:mimot0', '-m9', 'robospierre') + io_hub_cmd = ('../../io-hub/src/robospierre/io_hub.host', + '-i{instance}:io0') + self.asserv = proto (asserv.Proto, asserv_cmd, + asserv.init.host['robospierre']) + self.mimot = proto (mimot.Proto, mimot_cmd, + mimot.init.host['robospierre']) + self.io = proto (io_hub.Proto, io_hub_cmd, + io_hub.init.host['robospierre']) + self.protos = (self.asserv, self.mimot, self.io) diff --git a/digital/ai/tools/test_simu.py b/digital/ai/tools/test_simu.py index 80008bb9..50eaf265 100644 --- a/digital/ai/tools/test_simu.py +++ b/digital/ai/tools/test_simu.py @@ -49,32 +49,29 @@ class ObstacleWithBeacon (obstacle_view.RoundObstacle): class TestSimu (InterNode): """Interface, with simulated programs.""" - def __init__ (self, robot_class): + def __init__ (self, robot_class, robot_nb = 1): # Hub. - self.hub = mex.hub.Hub (min_clients = 4) + self.hub = mex.hub.Hub (min_clients = 1 + robot_class.client_nb + * robot_nb) self.forked_hub = utils.forked.Forked (self.hub.wait) # InterNode. InterNode.__init__ (self) def proto_time (): return self.node.date / self.node.tick - # Robot parameters. - robot = robot_class (proto_time) - self.robot = robot - # Asserv. - self.asserv = robot.asserv - self.asserv.async = True - self.tk.createfilehandler (self.asserv, READABLE, self.asserv_read) - # Mimot. - self.mimot = robot.mimot - self.mimot.async = True - self.tk.createfilehandler (self.mimot, READABLE, self.mimot_read) - # Io. - self.io = robot.io - self.io.async = True - self.tk.createfilehandler (self.io, READABLE, self.io_read) + # Robot instances. + self.robots = [ robot_class (proto_time, 'robot%d' % i) + for i in xrange (robot_nb) ] + for r in self.robots: + for prog in r.protos: + prog.async = True + def prog_read (f, mask, prog = prog): + prog.proto.read () + prog.proto.sync () + self.tk.createfilehandler (prog, READABLE, prog_read) # Add table. - self.table_model = robot.table_model.Table () - self.table = robot.table_view.Table (self.table_view, self.table_model) + self.table_model = robot_class.table_model.Table () + self.table = robot_class.table_view.Table (self.table_view, + self.table_model) self.obstacle = obstacle_model.RoundObstacle (150) self.table_model.obstacles.append (self.obstacle) self.obstacle_beacon = obstacle_model.RoundObstacle (40, 2) @@ -82,43 +79,30 @@ class TestSimu (InterNode): self.obstacle_view = ObstacleWithBeacon (self.table, self.obstacle, self.obstacle_beacon) self.table_view.bind ('<2>', self.place_obstacle) - # Add robot. - self.robot_link = robot.robot_link.Bag (self.node) - self.robot_model = robot.robot_model.Bag (self.node, self.table_model, - self.robot_link) - self.robot_view = robot.robot_view.Bag (self.table, - self.actuator_view, self.sensor_frame, self.robot_model) - # Color switch. - self.robot_model.color_switch.register (self.change_color) + # Add robots. + for r in self.robots: + r.link = r.robot_link.Bag (self.node, r.instance) + r.model = r.robot_model.Bag (self.node, self.table_model, r.link) + r.view = r.robot_view.Bag (self.table, self.actuator_view, + self.sensor_frame, r.model) + # Color switch. + def change_color (r = r): + i = r.model.color_switch.state + r.asserv.set_simu_pos (*r.robot_start_pos[i]); + r.model.color_switch.register (change_color) def close (self): self.forked_hub.kill () import time time.sleep (1) - def asserv_read (self, file, mask): - self.asserv.proto.read () - self.asserv.proto.sync () - - def mimot_read (self, file, mask): - self.mimot.proto.read () - self.mimot.proto.sync () - - def io_read (self, file, mask): - self.io.proto.read () - self.io.proto.sync () - def step (self): """Overide step to handle retransmissions, could be made cleaner using simulated time.""" InterNode.step (self) - self.asserv.proto.sync () - self.mimot.proto.sync () - self.io.proto.sync () - - def change_color (self, *dummy): - i = self.robot_model.color_switch.state - self.asserv.set_simu_pos (*self.robot.robot_start_pos[i]); + for r in self.robots: + for prog in r.protos: + prog.proto.sync () def place_obstacle (self, ev): pos = self.table_view.screen_coord ((ev.x, ev.y)) @@ -132,6 +116,8 @@ def run (default_robot, test_class = TestSimu): parser = optparse.OptionParser () parser.add_option ('-r', '--robot', help = "use specified robot", metavar = 'NAME', default = default_robot) + parser.add_option ('-n', '--robot-nb', help = "number of robots", + type = 'int', metavar = 'NB', default = 1) (options, args) = parser.parse_args () if args: parser.error ("too many arguments") @@ -143,7 +129,7 @@ def run (default_robot, test_class = TestSimu): robot = robospierre.Robot else: parser.error ("unknown robot") - app = test_class (robot) + app = test_class (robot, options.robot_nb) app.mainloop () app.close () diff --git a/host/simu/robots/aquajim/link/bag.py b/host/simu/robots/aquajim/link/bag.py index 3cb9e1b2..f0f96b9d 100644 --- a/host/simu/robots/aquajim/link/bag.py +++ b/host/simu/robots/aquajim/link/bag.py @@ -27,7 +27,7 @@ import asserv.mex class Bag: - def __init__ (self, node): - self.asserv = asserv.mex.Mex (node) - self.io = io.mex.Mex (node) + def __init__ (self, node, instance = 'robot0'): + self.asserv = asserv.mex.Mex (node, '%s:asserv0' % instance) + self.io = io.mex.Mex (node, '%s:io0' % instance) diff --git a/host/simu/robots/giboulee/link/bag.py b/host/simu/robots/giboulee/link/bag.py index c8c1b285..4dbf0554 100644 --- a/host/simu/robots/giboulee/link/bag.py +++ b/host/simu/robots/giboulee/link/bag.py @@ -27,7 +27,7 @@ import asserv.mex class Bag: - def __init__ (self, node): - self.asserv = asserv.mex.Mex (node) - self.io = io.mex.Mex (node) + def __init__ (self, node, instance = 'robot0'): + self.asserv = asserv.mex.Mex (node, '%s:asserv0' % instance) + self.io = io.mex.Mex (node, '%s:io0' % instance) diff --git a/host/simu/robots/marcel/link/bag.py b/host/simu/robots/marcel/link/bag.py index 7cb34c28..c0e36dea 100644 --- a/host/simu/robots/marcel/link/bag.py +++ b/host/simu/robots/marcel/link/bag.py @@ -28,8 +28,8 @@ import mimot.mex class Bag: - def __init__ (self, node): - self.asserv = asserv.mex.Mex (node) - self.io = io.mex.Mex (node) - self.mimot = mimot.mex.Mex (node) + def __init__ (self, node, instance = 'robot0'): + self.asserv = asserv.mex.Mex (node, '%s:asserv0' % instance) + self.io = io.mex.Mex (node, '%s:io0' % instance) + self.mimot = mimot.mex.Mex (node, '%s:mimot0' % instance) diff --git a/host/simu/robots/robospierre/link/bag.py b/host/simu/robots/robospierre/link/bag.py index ac68889a..e3368d55 100644 --- a/host/simu/robots/robospierre/link/bag.py +++ b/host/simu/robots/robospierre/link/bag.py @@ -28,8 +28,8 @@ import mimot.mex class Bag: - def __init__ (self, node): - self.asserv = asserv.mex.Mex (node) - self.io_hub = io_hub.mex.Mex (node) - self.mimot = mimot.mex.Mex (node) + def __init__ (self, node, instance = 'robot0'): + self.asserv = asserv.mex.Mex (node, '%s:asserv0' % instance) + self.io_hub = io_hub.mex.Mex (node, '%s:io0' % instance) + self.mimot = mimot.mex.Mex (node, '%s:mimot0' % instance) -- cgit v1.2.3