From 60d3060306a09f301b09201cfaefc9a1f88096c4 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 10 May 2011 19:37:43 +0200 Subject: digital/{ai,io}: add define for backward/forward --- digital/ai/src/common/defs.h | 7 +++++++ digital/ai/src/twi_master/asserv.c | 6 +++--- digital/ai/src/twi_master/asserv.h | 8 ++++---- 3 files changed, 14 insertions(+), 7 deletions(-) (limited to 'digital/ai') diff --git a/digital/ai/src/common/defs.h b/digital/ai/src/common/defs.h index f16e9e52..beec5608 100644 --- a/digital/ai/src/common/defs.h +++ b/digital/ai/src/common/defs.h @@ -43,6 +43,13 @@ typedef struct position_t position_t; /** Convert degrees to an angle usable in position_t. */ #define POSITION_A_DEG(a) G_ANGLE_UF016_DEG (a) +/** No particular direction. */ +#define DIRECTION_NONE 0 +/** Forward direction, along the robot X axis. */ +#define DIRECTION_FORWARD 1 +/** Backward, opposite the robot X axis. */ +#define DIRECTION_BACKWARD 2 + /** Team color, determine the start zone side. */ enum team_color_e { diff --git a/digital/ai/src/twi_master/asserv.c b/digital/ai/src/twi_master/asserv.c index d066737b..d9ae9330 100644 --- a/digital/ai/src/twi_master/asserv.c +++ b/digital/ai/src/twi_master/asserv.c @@ -202,12 +202,12 @@ asserv_get_moving_direction (void) { /* Foward move? */ if (asserv_status.status & _BV (asserv_status_flag_move_forward)) - return 1; + return DIRECTION_FORWARD; /* Backward move? */ if (asserv_status.status & _BV (asserv_status_flag_move_backward)) - return 2; + return DIRECTION_BACKWARD; /* Not moving */ - return 0; + return DIRECTION_NONE; } uint8_t diff --git a/digital/ai/src/twi_master/asserv.h b/digital/ai/src/twi_master/asserv.h index 0d29ba96..2aa7a6f8 100644 --- a/digital/ai/src/twi_master/asserv.h +++ b/digital/ai/src/twi_master/asserv.h @@ -121,16 +121,16 @@ asserv_get_motor1_position (void); /** * Are we moving forward/backward? * @return - * - 0 we are not moving; - * - 1 we are moving forward; - * - 2 we are moving backward. + * - DIRECTION_NONE we are not moving; + * - DIRECTION_FORWARD we are moving forward; + * - DIRECTION_BACKWARD we are moving backward. */ uint8_t asserv_get_moving_direction (void); /** * Get the last moving direction of the bot. - * @return 1 is forward, 2 is backward. + * @return DIRECTION_FORWARD or DIRECTION_BACKWARD. */ uint8_t asserv_get_last_moving_direction (void); -- 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 'digital/ai') 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 3d91eb26478dd0801294ce0ead01605687687d5e Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Sat, 14 May 2011 13:29:02 +0200 Subject: digital/ai/src/fsm: use program space read function for transition table --- digital/ai/src/fsm/fsm.host.c | 40 ++++++++++++++++++++++++++++++---------- 1 file changed, 30 insertions(+), 10 deletions(-) (limited to 'digital/ai') diff --git a/digital/ai/src/fsm/fsm.host.c b/digital/ai/src/fsm/fsm.host.c index bf8b761d..b7b6ab4e 100644 --- a/digital/ai/src/fsm/fsm.host.c +++ b/digital/ai/src/fsm/fsm.host.c @@ -1056,11 +1056,18 @@ fsm_build_gen_avr_h (fsm_build_t *fsm, uint embedded_strings) /* Gen function table. */ fprintf (f, "typedef fsm_%s_branch_t (*fsm_%s_func_t)(void);\n", fsm->name, fsm->name); - fprintf (f, "extern const fsm_%s_func_t PROGMEM fsm_%s_trans_table[%u][%u];\n\n", + fprintf (f, "extern const fsm_%s_func_t PROGMEM fsm_%s_trans_table[%u][%u];\n", fsm->name, fsm->name, fsm->event_nb, fsm->state_nb); + /* Gen read function for trans table. */ + fprintf (f, "fsm_%s_func_t fsm_%s_read_trans (fsm_%s_event_t event, " + "fsm_%s_state_t state);\n\n", + fsm->name, + fsm->name, + fsm->name, + fsm->name); /* Gen active states array. */ fprintf (f, "extern fsm_%s_state_t fsm_%s_active_states[%u];\n\n", @@ -1280,6 +1287,19 @@ fsm_build_gen_avr_c (fsm_build_t *fsm, uint embedded_strings) } fprintf (f, "};\n\n"); + /* Gen read function for trans table. */ + fprintf (f, "fsm_%s_func_t fsm_%s_read_trans (fsm_%s_event_t event, " + "fsm_%s_state_t state)\n{\n", + fsm->name, + fsm->name, + fsm->name, + fsm->name); + fprintf (f, "\treturn (fsm_%s_func_t) pgm_read_word " + "(&fsm_%s_trans_table[event][state]);\n", + fsm->name, + fsm->name); + fprintf (f, "}\n\n"); + /* Gen active states array. */ fprintf (f, "fsm_%s_state_t fsm_%s_active_states[%u];\n\n", fsm->name, @@ -1324,15 +1344,15 @@ fsm_build_gen_avr_c (fsm_build_t *fsm, uint embedded_strings) fprintf (f, "\tint handled = 0;\n"); fprintf (f, "\tfor (i = 0; i < fsm_%s_max_active_states; i++)\n\t{\n", fsm->name); - fprintf (f, "\t\tif (fsm_%s_trans_table[e][fsm_%s_active_states[i]])\n", + fprintf (f, "\t\tif (fsm_%s_read_trans (e, fsm_%s_active_states[i]))\n", fsm->name, fsm->name); fprintf (f, "\t\t{\n"); - fprintf (f, "\t\t\tfsm_%s_active_states[i] = \ - fsm_%s_trans_table[e][fsm_%s_active_states[i]]();\n", - fsm->name, - fsm->name, - fsm->name); + fprintf (f, "\t\t\tfsm_%s_active_states[i] = fsm_%s_read_trans (e, " + "fsm_%s_active_states[i])();\n", + fsm->name, + fsm->name, + fsm->name); fprintf (f, "\t\t\thandled = 1;\n"); if (fsm->timeouts != NULL) { @@ -1352,9 +1372,9 @@ fsm_build_gen_avr_c (fsm_build_t *fsm, uint embedded_strings) fprintf (f, "\tuint16_t i;\n"); fprintf (f, "\tfor (i = 0; i < fsm_%s_max_active_states; i++)\n", fsm->name); - fprintf (f, "\t\tif (fsm_%s_trans_table[e][fsm_%s_active_states[i]])\n", - fsm->name, - fsm->name); + fprintf (f, "\t\tif (fsm_%s_read_trans (e, fsm_%s_active_states[i]))\n", + fsm->name, + fsm->name); fprintf (f, "\t\t\treturn 1;\n"); fprintf (f, "\treturn 0;\n"); fprintf (f, "}\n\n"); -- cgit v1.2.3 From 5ba03e9744a8916730d9b8462cbfa6512d380ac6 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 14 May 2011 16:49:48 +0200 Subject: digital/io-hub: add drop --- digital/ai/tools/test_simu_control_robospierre.py | 21 ++++++- digital/io-hub/src/robospierre/clamp.c | 73 +++++++++++++++++++++++ digital/io-hub/src/robospierre/clamp.h | 9 +++ digital/io-hub/src/robospierre/logistic.c | 11 ++++ digital/io-hub/src/robospierre/logistic.h | 5 ++ digital/io-hub/src/robospierre/main.c | 8 +++ digital/io-hub/tools/io_hub/io_hub.py | 10 ++++ 7 files changed, 136 insertions(+), 1 deletion(-) (limited to 'digital/ai') diff --git a/digital/ai/tools/test_simu_control_robospierre.py b/digital/ai/tools/test_simu_control_robospierre.py index 88fbc5f3..acd3ca7c 100644 --- a/digital/ai/tools/test_simu_control_robospierre.py +++ b/digital/ai/tools/test_simu_control_robospierre.py @@ -82,12 +82,21 @@ class TestSimuControl (TestSimu): text = 'Move element', padx = 0, pady = 0, command = self.clamp_move_element_command) self.clamp_element_move_button.pack () + self.drop_var = IntVar () + self.drop_button = Checkbutton (self.control_frame, text = 'Drop', + indicatoron = False, + variable = self.drop_var, command = self.drop_command) + self.drop_button.pack () + self.backward_var = IntVar () + self.backward_button = Checkbutton (self.control_frame, + text = 'Backward', variable = self.backward_var) + self.backward_button.pack () self.table_view.bind ('<1>', self.move) self.table_view.bind ('<3>', self.orient) def move (self, ev): pos = self.table_view.screen_coord ((ev.x, ev.y)) - self.asserv.goto (pos[0], pos[1]) + self.asserv.goto (pos[0], pos[1], self.backward_var.get ()) def orient (self, ev): x, y = self.table_view.screen_coord ((ev.x, ev.y)) @@ -129,6 +138,16 @@ class TestSimuControl (TestSimu): for i in (0, 1, 3, 4): self.io.pwm_set_timed (i, pwm, 255, 0) + def drop_command (self): + if self.drop_var.get (): + if self.backward_var.get (): + order = 'drop_backward' + else: + order = 'drop_forward' + else: + order = 'drop_clear' + self.io.drop (order) + def change_color (self, *dummy): pass diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 227579ff..6c063979 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -60,6 +60,10 @@ FSM_STATES ( CLAMP_TAKING_DOOR_CLOSING, /* Moving elements around. */ CLAMP_MOVING_ELEMENT, + /* Droping a tower. */ + CLAMP_DROPING_DOOR_OPENING, + /* Droping a tower, waiting for robot to advance. */ + CLAMP_DROPING_WAITING_ROBOT, /* Waiting movement order. */ CLAMP_MOVE_IDLE, @@ -83,6 +87,13 @@ FSM_EVENTS ( start, /* New element inside bottom slot. */ clamp_new_element, + /* Order to drop elements. */ + clamp_drop, + /* Sent once drop is done, but robot should advance to completely + * free the dropped tower. */ + clamp_drop_waiting, + /* Received when top FSM made the robot advance after a drop. */ + clamp_drop_clear, /* Order to move the clamp. */ clamp_move, /* Clamp movement success. */ @@ -110,6 +121,8 @@ struct clamp_t uint8_t pos_new; /** New element kind. */ uint8_t new_element; + /** Drop direction, drop on the other side. */ + uint8_t drop_direction; }; /** Global context. */ @@ -189,6 +202,25 @@ clamp_new_element (uint8_t pos, uint8_t element) FSM_HANDLE (AI, clamp_new_element); } +uint8_t +clamp_drop (uint8_t drop_direction) +{ + if (FSM_CAN_HANDLE (AI, clamp_drop)) + { + ctx.drop_direction = drop_direction; + FSM_HANDLE (AI, clamp_drop); + return 1; + } + else + return 0; +} + +void +clamp_drop_clear (void) +{ + FSM_HANDLE (AI, clamp_drop_clear); +} + uint8_t clamp_handle_event (void) { @@ -310,6 +342,16 @@ FSM_TRANS (CLAMP_IDLE, clamp_new_element, CLAMP_TAKING_DOOR_CLOSING) return FSM_NEXT (CLAMP_IDLE, clamp_new_element); } +FSM_TRANS (CLAMP_IDLE, clamp_drop, CLAMP_DROPING_DOOR_OPENING) +{ + /* If going forward, drop at back. */ + uint8_t bay = ctx.drop_direction == DIRECTION_FORWARD + ? CLAMP_SLOT_BACK_BOTTOM : CLAMP_SLOT_FRONT_BOTTOM; + pwm_set_timed (clamp_slot_door[bay + 0], BOT_PWM_DOOR_OPEN); + pwm_set_timed (clamp_slot_door[bay + 2], BOT_PWM_DOOR_OPEN); + return FSM_NEXT (CLAMP_IDLE, clamp_drop); +} + FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME, move_element, CLAMP_MOVING_ELEMENT, move_to_idle, CLAMP_GOING_IDLE, @@ -355,6 +397,37 @@ FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_success, done); } +FSM_TRANS_TIMEOUT (CLAMP_DROPING_DOOR_OPENING, BOT_PWM_CLAMP_OPEN_TIME, + CLAMP_DROPING_WAITING_ROBOT) +{ + fsm_queue_post_event (FSM_EVENT (AI, clamp_drop_waiting)); + return FSM_NEXT_TIMEOUT (CLAMP_DROPING_DOOR_OPENING); +} + +FSM_TRANS (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, + move_element, CLAMP_MOVING_ELEMENT, + move_to_idle, CLAMP_GOING_IDLE, + done, CLAMP_IDLE) +{ + logistic_drop (ctx.drop_direction); + if (logistic_global.moving_from != CLAMP_SLOT_NB) + { + clamp_move_element (logistic_global.moving_from, + logistic_global.moving_to); + return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, + move_element); + } + else if (logistic_global.clamp_pos_idle != ctx.pos_current) + { + clamp_move (logistic_global.clamp_pos_idle); + return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, + move_to_idle); + } + else + return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, + done); +} + /* CLAMP_MOVE FSM */ FSM_TRANS (CLAMP_MOVE_IDLE, clamp_move, diff --git a/digital/io-hub/src/robospierre/clamp.h b/digital/io-hub/src/robospierre/clamp.h index bb15b62e..a59e91ae 100644 --- a/digital/io-hub/src/robospierre/clamp.h +++ b/digital/io-hub/src/robospierre/clamp.h @@ -68,6 +68,15 @@ clamp_move (uint8_t pos); void clamp_move_element (uint8_t from, uint8_t to); +/** Drop an element tower. Return 0 if not currently possible. If + * drop_direction is forward, drop at the back. */ +uint8_t +clamp_drop (uint8_t drop_direction); + +/** Signal robot advanced, and drop is finished. */ +void +clamp_drop_clear (void); + /** Examine sensors to generate new events, return non zero if an event was * generated. */ uint8_t diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index 534f1c46..945bd583 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -210,3 +210,14 @@ logistic_element_move_done (void) logistic_decision (); } +void +logistic_drop (uint8_t direction) +{ + uint8_t bay = direction == DIRECTION_FORWARD + ? CLAMP_SLOT_BACK_BOTTOM : CLAMP_SLOT_FRONT_BOTTOM; + uint8_t i; + for (i = bay; i < bay + 3; i++) + ctx.slots[i] = 0; + logistic_decision (); +} + diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h index 75b49d53..c6aa4159 100644 --- a/digital/io-hub/src/robospierre/logistic.h +++ b/digital/io-hub/src/robospierre/logistic.h @@ -65,4 +65,9 @@ logistic_element_new (uint8_t pos, uint8_t element); void logistic_element_move_done (void); +/** To be called when elements have been dropped on the opposite side of + * direction. */ +void +logistic_drop (uint8_t direction); + #endif /* logistic_h */ diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index ffb05a99..938703b6 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -222,6 +222,14 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) 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; + case c ('d', 1): + /* Drop elements. + * - 1b: 00: drop clear, 01: drop forward, 02: drop backward. */ + if (args[0] == 0x00) + clamp_drop_clear (); + else + clamp_drop (args[0]); + 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 1b25102c..d6ae8618 100644 --- a/digital/io-hub/tools/io_hub/io_hub.py +++ b/digital/io-hub/tools/io_hub/io_hub.py @@ -50,6 +50,16 @@ class Proto: def clamp_move_element (self, from_, to): self.proto.send ('c', 'BB', from_, to) + def drop (self, order): + if order == 'drop_clear': + self.proto.send ('d', 'B', 0x00) + elif order == 'drop_forward': + self.proto.send ('d', 'B', 0x01) + elif order == 'drop_backward': + self.proto.send ('d', 'B', 0x02) + else: + raise ValueError + def close (self): self.reset () self.proto.wait (lambda: True) -- cgit v1.2.3 From 9d26b60d08f937ba49962ebfeaa3664f5527bf1a Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 17 May 2011 01:58:19 +0200 Subject: digital/asserv: add robospierre model --- digital/ai/tools/robospierre.py | 3 +- digital/asserv/src/asserv/models.host.c | 65 ++++++++++++++++++++++-- digital/asserv/src/asserv/models.host.h | 7 +++ digital/asserv/src/asserv/simu.host.c | 76 +++++++++++++++++++++------- digital/asserv/src/asserv/simu.host.h | 3 ++ digital/asserv/src/asserv/test_motor_model.c | 6 +++ 6 files changed, 138 insertions(+), 22 deletions(-) (limited to 'digital/ai') diff --git a/digital/ai/tools/robospierre.py b/digital/ai/tools/robospierre.py index 09be4bea..8d5db276 100644 --- a/digital/ai/tools/robospierre.py +++ b/digital/ai/tools/robospierre.py @@ -23,7 +23,8 @@ class Robot: 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', 'marcel') + asserv_cmd = ('../../asserv/src/asserv/asserv.host', '-m9', + 'robospierre') mimot_cmd = ('../../mimot/src/dirty/dirty.host', '-m9', 'marcel') io_hub_cmd = ('../../io-hub/src/robospierre/io_hub.host') self.asserv = asserv.Proto (PopenIO (asserv_cmd), proto_time, diff --git a/digital/asserv/src/asserv/models.host.c b/digital/asserv/src/asserv/models.host.c index f4d8ca5a..ae123585 100644 --- a/digital/asserv/src/asserv/models.host.c +++ b/digital/asserv/src/asserv/models.host.c @@ -32,6 +32,8 @@ #include #include +#define NO_CORNER { { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 } } + /* RE25CLL with 1:10 gearbox model. */ static const struct motor_def_t re25cll_model = { @@ -89,6 +91,25 @@ static const struct motor_def_t amax32ghp_model = -INFINITY, +INFINITY, }; +/* Faulhaber 2657 and a 1:9.7 ratio gearbox. */ +static const struct motor_def_t faulhaber_2657_model = +{ + /* Motor characteristics. */ + 274 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */ + 34.8 / 1000, /* Torque constant (N.m/A). */ + 0, /* Bearing friction (N.m/(rad/s)). */ + 2.84, /* Terminal resistance (Ohm). */ + 0.380 / 1000, /* Terminal inductance (H). */ + 24.0, /* Maximum voltage (V). */ + /* Gearbox characteristics. */ + 9.7, /* Gearbox ratio. */ + 0.80, /* Gearbox efficiency. */ + /* Load characteristics. */ + 0.0, /* Load (kg.m^2). */ + /* Hardware limits. */ + 0.0, +INFINITY, +}; + /* Gloubi, Efrei 2006. */ static const struct robot_t gloubi_robot = { @@ -106,7 +127,7 @@ static const struct robot_t gloubi_robot = 13.0, // approx /* Whether the encoder is mounted on the main motor (false) or not (true). */ 0, - 0.0, 0.0, { NULL, NULL }, { 0, 0 }, NULL + 0.0, 0.0, { NULL, NULL }, { 0, 0 }, NULL, NULL, NO_CORNER }; /* Taz, APBTeam/Efrei 2005. */ @@ -126,7 +147,7 @@ static const struct robot_t taz_robot = 0.0, /* Whether the encoder is mounted on the main motor (false) or not (true). */ 0, - 0.0, 0.0, { NULL, NULL }, { 0, 0 }, NULL + 0.0, 0.0, { NULL, NULL }, { 0, 0 }, NULL, NULL, NO_CORNER }; /* TazG, Taz with RE25G motors. */ @@ -146,7 +167,7 @@ static const struct robot_t tazg_robot = 0.0, /* Whether the encoder is mounted on the main motor (false) or not (true). */ 0, - 0.0, 0.0, { NULL, NULL }, { 0, 0 }, NULL + 0.0, 0.0, { NULL, NULL }, { 0, 0 }, NULL, NULL, NO_CORNER }; /* Giboulée arm model, with a RE25CLL and a 1:10 ratio gearbox. */ @@ -197,6 +218,7 @@ static const struct robot_t giboulee_robot = { 500, 0 }, /** Sensor update function. */ simu_sensor_update_giboulee, + NULL, NO_CORNER, }; /* AquaJim arm model, with a RE40G and a 1:4 + 15:80 ratio gearbox. */ @@ -267,6 +289,7 @@ static const struct robot_t aquajim_robot = { 250, 250 }, /** Sensor update function. */ simu_sensor_update_aquajim, + NULL, NO_CORNER, }; /* Marcel elevator model, with a Faulhaber 2657 and a 1:9.7 ratio gearbox. */ @@ -338,6 +361,41 @@ static const struct robot_t marcel_robot = { 256, 250 }, /** Sensor update function. */ simu_sensor_update_marcel, + NULL, NO_CORNER, +}; + +/* Robospierre, APBTeam 2011. */ +static const struct robot_t robospierre_robot = +{ + /* Main motors. */ + &faulhaber_2657_model, + /* Number of steps on the main motors encoders. */ + 2500, + /* Wheel radius (m). */ + 0.065 / 2, + /* Distance between the wheels (m). */ + 0.16, + /* Weight of the robot (kg). */ + 10.0, + /* Distance of the gravity center from the center of motors axis (m). */ + 0.0, + /* Whether the encoder is mounted on the main motor (false) or not (true). */ + 1, + /** Encoder wheel radius (m). */ + 0.063 / 2, + /** Distance between the encoders wheels (m). */ + 0.28, + /** Auxiliary motors, NULL if not present. */ + { NULL, NULL }, + /** Number of steps for each auxiliary motor encoder. */ + { 0, 0 }, + /** Sensor update function. */ + NULL, + /** Table test function, return false if given robot point is not in + * table. */ + simu_table_test_robospierre, + /** Robot corners, from front left, then clockwise. */ + { { 150, 50 }, { 150, -50 }, { -150, -50 }, { -150, 50 } }, }; /* Table of models. */ @@ -352,6 +410,7 @@ static const struct { "giboulee", &giboulee_robot }, { "aquajim", &aquajim_robot }, { "marcel", &marcel_robot }, + { "robospierre", &robospierre_robot }, { 0, 0 } }; diff --git a/digital/asserv/src/asserv/models.host.h b/digital/asserv/src/asserv/models.host.h index 403e9cd9..d8f1dcb8 100644 --- a/digital/asserv/src/asserv/models.host.h +++ b/digital/asserv/src/asserv/models.host.h @@ -27,6 +27,8 @@ #define ECHANT_PERIOD (1.0 / (14745600.0 / 256 / 256)) +#define CORNERS_NB 4 + /** Define a robot and its peripherals. * Encoder characteristics are defined at gearbox output. */ struct robot_t @@ -55,6 +57,11 @@ struct robot_t int aux_encoder_steps[AC_ASSERV_AUX_NB]; /** Sensor update function. */ void (*sensor_update) (void); + /** Table test function, return false if given robot point is not in + * table. */ + int (*table_test) (double p_x, double p_y); + /** Robot corners, from front left, then clockwise. */ + double corners[CORNERS_NB][2]; }; /** Get a pointer to a model by name, or return 0. */ diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c index 10cef711..d9a97fe2 100644 --- a/digital/asserv/src/asserv/simu.host.c +++ b/digital/asserv/src/asserv/simu.host.c @@ -154,6 +154,16 @@ simu_pos_update (double dl, double dr, double footing) simu_pos_a = na; } +/** Compute a robot point absolute position. */ +static void +simu_compute_absolute_position (double p_x, double p_y, double *x, double *y) +{ + double c = cos (simu_pos_a); + double s = sin (simu_pos_a); + *x = simu_pos_x + c * p_x - s * p_y; + *y = simu_pos_y + s * p_x + c * p_y; +} + /** Update sensors for Giboulee. */ void simu_sensor_update_giboulee (void) @@ -175,10 +185,7 @@ simu_sensor_update_giboulee (void) for (i = 0; i < UTILS_COUNT (sensors); i++) { /* Compute absolute position. */ - x = simu_pos_x + cos (simu_pos_a) * sensors[i][0] - - sin (simu_pos_a) * sensors[i][1]; - y = simu_pos_y + sin (simu_pos_a) * sensors[i][0] - + cos (simu_pos_a) * sensors[i][1]; + simu_compute_absolute_position (sensors[i][0], sensors[i][1], &x, &y); if (x >= 0.0 && x < table_width && y >= 0.0 && y < table_height) PINC |= sensors_bit[i]; } @@ -217,10 +224,7 @@ simu_sensor_update_aquajim (void) for (i = 0; i < UTILS_COUNT (sensors); i++) { /* Compute absolute position. */ - x = simu_pos_x + cos (simu_pos_a) * sensors[i][0] - - sin (simu_pos_a) * sensors[i][1]; - y = simu_pos_y + sin (simu_pos_a) * sensors[i][0] - + cos (simu_pos_a) * sensors[i][1]; + simu_compute_absolute_position (sensors[i][0], sensors[i][1], &x, &y); cx = table_width / 2 - x; cy = table_height / 2 - y; ds = cx * cx + cy * cy; @@ -268,10 +272,7 @@ simu_sensor_update_marcel (void) for (i = 0; i < UTILS_COUNT (sensors); i++) { /* Compute absolute position. */ - x = simu_pos_x + cos (simu_pos_a) * sensors[i][0] - - sin (simu_pos_a) * sensors[i][1]; - y = simu_pos_y + sin (simu_pos_a) * sensors[i][0] - + cos (simu_pos_a) * sensors[i][1]; + simu_compute_absolute_position (sensors[i][0], sensors[i][1], &x, &y); if (x >= 0.0 && x < table_width && y >= 0.0 && y < table_height && (x < stand_x_min || x >= stand_x_max || y < stand_y)) PINC |= sensors_bit[i]; @@ -281,6 +282,18 @@ simu_sensor_update_marcel (void) PINC |= IO_BV (CONTACT_AUX1_ZERO_IO); } +/* Table test for Robospierre. */ +int +simu_table_test_robospierre (double p_x, double p_y) +{ + static const double table_width = 3000.0, table_height = 2100.0; + double x, y; + simu_compute_absolute_position (p_x, p_y, &x, &y); + if (x < 0 || y < 0 || x >= table_width || y >= table_height) + return 0; + return 1; +} + /** Do a simulation step. */ static void simu_step (void) @@ -306,6 +319,39 @@ simu_step (void) if (simu_robot->aux_motor[i]) motor_model_step (&simu_aux_model[i]); } + /* Update position. */ + double old_pos_x = simu_pos_x, old_pos_y = simu_pos_y, + old_pos_a = simu_pos_a; + simu_pos_update ((simu_left_model.th - old_left_th) + / simu_left_model.m.i_G * simu_robot->wheel_r * 1000, + (simu_right_model.th - old_right_th) + / simu_right_model.m.i_G * simu_robot->wheel_r * 1000, + simu_robot->footing * 1000); + /* Check robot is still on the table. */ + if (simu_robot->table_test) + { + static int old_out = 1; + int out = 0; + for (i = 0; i < CORNERS_NB; i++) + { + if (!simu_robot->table_test (simu_robot->corners[i][0], + simu_robot->corners[i][1])) + out = 1; + } + /* If out, cancel movement. */ + if (out && !old_out) + { + simu_pos_x = old_pos_x; + simu_pos_y = old_pos_y; + simu_pos_a = old_pos_a; + simu_left_model.th = old_left_th; + simu_right_model.th = old_right_th; + } + else + { + old_out = out; + } + } /* Modify counters. */ uint32_t counter_left_new; uint32_t counter_right_new; @@ -364,12 +410,6 @@ simu_step (void) simu_counter_aux[i] = 0; } } - /* Update position. */ - simu_pos_update ((simu_left_model.th - old_left_th) - / simu_left_model.m.i_G * simu_robot->wheel_r * 1000, - (simu_right_model.th - old_right_th) - / simu_right_model.m.i_G * simu_robot->wheel_r * 1000, - simu_robot->footing * 1000); /* Update sensors. */ if (simu_robot->sensor_update) simu_robot->sensor_update (); diff --git a/digital/asserv/src/asserv/simu.host.h b/digital/asserv/src/asserv/simu.host.h index 143d3430..93785186 100644 --- a/digital/asserv/src/asserv/simu.host.h +++ b/digital/asserv/src/asserv/simu.host.h @@ -47,4 +47,7 @@ simu_sensor_update_aquajim (void); void simu_sensor_update_marcel (void); +int +simu_table_test_robospierre (double p_x, double p_y); + #endif /* simu_host_h */ diff --git a/digital/asserv/src/asserv/test_motor_model.c b/digital/asserv/src/asserv/test_motor_model.c index 706aa31b..044bab2e 100644 --- a/digital/asserv/src/asserv/test_motor_model.c +++ b/digital/asserv/src/asserv/test_motor_model.c @@ -84,3 +84,9 @@ simu_sensor_update_marcel (void) { } +int +simu_table_test_robospierre (double p_x, double p_y) +{ + return 0; +} + -- cgit v1.2.3 From f753bc39682f980feb49b7ff1bb9e9594f986e6b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 19 May 2011 23:24:08 +0200 Subject: digital/asserv: add push the wall traj mode, closes #167 --- digital/ai/src/twi_master/asserv.c | 22 ++++++++++++++ digital/ai/src/twi_master/asserv.h | 6 ++++ digital/asserv/src/asserv/main.c | 21 +++++++++++++ digital/asserv/src/asserv/traj.c | 57 +++++++++++++++++++++++++++++++++++ digital/asserv/src/asserv/traj.h | 4 +++ digital/asserv/src/asserv/twi_proto.c | 18 +++++++++++ digital/asserv/tools/asserv/asserv.py | 19 ++++++++++-- 7 files changed, 145 insertions(+), 2 deletions(-) (limited to 'digital/ai') diff --git a/digital/ai/src/twi_master/asserv.c b/digital/ai/src/twi_master/asserv.c index d9ae9330..839b1d23 100644 --- a/digital/ai/src/twi_master/asserv.c +++ b/digital/ai/src/twi_master/asserv.c @@ -300,6 +300,28 @@ asserv_go_to_the_wall (uint8_t backward) twi_master_send_buffer (2); } +void +asserv_push_the_wall (uint8_t backward, uint32_t init_x, uint32_t init_y, + uint16_t init_a) +{ + if (init_x != (uint32_t) -1) + init_x = fixed_mul_f824 (init_x, asserv_scale_inv); + if (init_y != (uint32_t) -1) + init_y = fixed_mul_f824 (init_y, asserv_scale_inv); + uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE); + buffer[0] = 'G'; + buffer[1] = backward; + buffer[2] = v32_to_v8 (init_x, 2); + buffer[3] = v32_to_v8 (init_x, 1); + buffer[4] = v32_to_v8 (init_x, 0); + buffer[5] = v32_to_v8 (init_y, 2); + buffer[6] = v32_to_v8 (init_y, 1); + buffer[7] = v32_to_v8 (init_y, 0); + buffer[8] = v16_to_v8 (init_a, 1); + buffer[9] = v16_to_v8 (init_a, 0); + twi_master_send_buffer (10); +} + void asserv_move_motor0_absolute (uint16_t position, uint8_t speed) { diff --git a/digital/ai/src/twi_master/asserv.h b/digital/ai/src/twi_master/asserv.h index 2aa7a6f8..c9a52a11 100644 --- a/digital/ai/src/twi_master/asserv.h +++ b/digital/ai/src/twi_master/asserv.h @@ -198,6 +198,12 @@ asserv_goto_xya (uint32_t x, uint32_t y, int16_t a, uint8_t backward); void asserv_go_to_the_wall (uint8_t backward); +/** Push the wall and initialise position. Use -1 for coordinates to keep + * unchanged. */ +void +asserv_push_the_wall (uint8_t backward, uint32_t init_x, uint32_t init_y, + uint16_t init_a); + /** * Move the motor0. * Motor0 class command. diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index 2d53e2fa..677fea84 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -400,6 +400,27 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) break; traj_ftw_start_center (args[0], args[1], args[2]); break; + case c ('f', 12): + /* Push the wall. + * - b: 0: forward, 1: backward. + * - d: init_x, f24.8. + * - d: init_y, f24.8. + * - w: init_a, f0.16. + * - b: sequence number. */ + { + if (args[11] == state_main.sequence) + break; + int32_t angle; + if (args[9] == 0xff && args[10] == 0xff) + angle = -1; + else + angle = v8_to_v32 (0, args[9], args[10], 0); + traj_ptw_start (args[0], + v8_to_v32 (args[1], args[2], args[3], args[4]), + v8_to_v32 (args[5], args[6], args[7], args[8]), + angle, args[11]); + } + break; case c ('F', 1): /* Go to the dispenser. * - b: sequence number. */ diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c index 11618eca..e147ad5d 100644 --- a/digital/asserv/src/asserv/traj.c +++ b/digital/asserv/src/asserv/traj.c @@ -37,6 +37,7 @@ #include "pos.h" #include "speed.h" #include "postrack.h" +#include "pwm.h" #include "contacts.h" @@ -59,6 +60,8 @@ enum TRAJ_FTW, /* Go to the dispenser. */ TRAJ_GTD, + /* Push the wall. */ + TRAJ_PTW, /* Go to position. */ TRAJ_GOTO, /* Go to angle. */ @@ -99,6 +102,9 @@ static uint8_t traj_use_center; /** Center sensor delay. */ static uint8_t traj_center_delay; +/** Initial values for x, y and angle, or -1. */ +static int32_t traj_init_x, traj_init_y, traj_init_a; + /** Initialise computed factors. */ void traj_init (void) @@ -210,6 +216,54 @@ traj_ftw_start_center (uint8_t backward, uint8_t center_delay, uint8_t seq) state_start (&state_main, MODE_TRAJ, seq); } +/** Push the wall mode. */ +static void +traj_ptw (void) +{ + /* If blocking, the wall was found. */ + if (pos_theta.blocked_counter >= pos_theta.blocked_counter_limit) + { + /* Initialise position. */ + if (traj_init_x != -1) + postrack_x = traj_init_x; + if (traj_init_y != -1) + postrack_y = traj_init_y; + if (traj_init_a != -1) + postrack_a = traj_init_a; + /* Stop motor control. */ + pos_reset (&pos_theta); + pos_reset (&pos_alpha); + state_main.variant = 0; + state_main.mode = MODE_PWM; + pwm_set (&pwm_left, 0); + pwm_set (&pwm_right, 0); + state_finish (&state_main); + traj_mode = TRAJ_DONE; + } +} + +/** Start push the wall mode. Position is initialised unless -1. */ +void +traj_ptw_start (uint8_t backward, int32_t init_x, int32_t init_y, + int32_t init_a, uint8_t seq) +{ + int16_t speed; + traj_mode = TRAJ_PTW; + traj_init_x = init_x; + traj_init_y = init_y; + traj_init_a = init_a; + state_start (&state_main, MODE_TRAJ, seq); + /* Use slow speed, without alpha control. */ + speed = speed_theta.slow; + speed *= 256; + if (backward) + speed = -speed; + speed_theta.use_pos = speed_alpha.use_pos = 0; + speed_theta.cons = speed; + speed_alpha.cons = 0; + state_main.variant = 2; +} + /** Go to the dispenser mode. */ static void traj_gtd (void) @@ -392,6 +446,9 @@ traj_update (void) case TRAJ_FTW: traj_ftw (); break; + case TRAJ_PTW: + traj_ptw (); + break; case TRAJ_GTD: traj_gtd (); break; diff --git a/digital/asserv/src/asserv/traj.h b/digital/asserv/src/asserv/traj.h index 706fbdaf..ec345d0c 100644 --- a/digital/asserv/src/asserv/traj.h +++ b/digital/asserv/src/asserv/traj.h @@ -48,6 +48,10 @@ traj_ftw_start (uint8_t backward, uint8_t seq); void traj_ftw_start_center (uint8_t backward, uint8_t center_delay, uint8_t seq); +void +traj_ptw_start (uint8_t backward, int32_t init_x, int32_t init_y, + int32_t init_a, uint8_t seq); + void traj_gtd_start (uint8_t seq); diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c index ff15c82f..fcf0a9cc 100644 --- a/digital/asserv/src/asserv/twi_proto.c +++ b/digital/asserv/src/asserv/twi_proto.c @@ -168,6 +168,24 @@ twi_proto_callback (u8 *buf, u8 size) * - b: 0: forward, 1: backward. */ traj_ftw_start (buf[2], 0); break; + case c ('G', 9): + /* Push the wall. + * - b: 0: forward, 1: backward. + * - 3b: init_x. + * - 3b: init_y. + * - w: init_a. */ + { + int32_t angle; + if (buf[9] == 0xff && buf[10] == 0xff) + angle = -1; + else + angle = v8_to_v32 (0, buf[9], buf[10], 0); + traj_ptw_start (buf[2], + v8_to_v32 (buf[3], buf[4], buf[5], 0xff), + v8_to_v32 (buf[6], buf[7], buf[8], 0xff), + angle, 0); + } + break; case c ('g', 2): /* Go to the wall using center sensor with delay. * - b: 0: forward, 1: backward. diff --git a/digital/asserv/tools/asserv/asserv.py b/digital/asserv/tools/asserv/asserv.py index 997b05fb..4f386d7a 100644 --- a/digital/asserv/tools/asserv/asserv.py +++ b/digital/asserv/tools/asserv/asserv.py @@ -269,6 +269,15 @@ class Proto: self.proto.send ('f', 'BB', backward and 1 or 0, self.mseq) self.wait (self.finished, auto = True) + def ptw (self, backward = True, init_x = None, init_y = None, + init_a = None): + """Push the wall.""" + self.mseq += 1 + self.proto.send ('f', 'BllHB', backward and 1 or 0, + self._dist_f248 (init_x), self._dist_f248 (init_y), + self._angle_f16 (init_a), self.mseq) + self.wait (self.finished, auto = True) + def set_simu_pos (self, x, y, a): """Set simulated position.""" self.proto.send ('h', 'chhh', 'X', int (round (x)), int (round (y)), @@ -381,10 +390,16 @@ class Proto: return int (round (d / self.param['scale'])) def _dist_f248 (self, d): - return int (round ((1 << 8) * d / self.param['scale'])) + if d is None: + return -1 + else: + return int (round ((1 << 8) * d / self.param['scale'])) def _angle_f16 (self, a): - return int (round ((1 << 16) * a / (2 * math.pi))) & 0xffff + if a is None: + return 0xffff + else: + return int (round ((1 << 16) * a / (2 * math.pi))) & 0xffff def _angle_f824 (self, a): return int (round ((1 << 24) * a / (2 * math.pi))) -- cgit v1.2.3 From 2277c9a7aa41c347ba04fca8e202c77b43618802 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 20 May 2011 08:27:02 +0200 Subject: digital/{ai,io-hub}: add init FSM --- digital/ai/src/fsm/init.c | 188 +++++++++++++++++++++++++++++ digital/ai/tools/robospierre.py | 5 +- digital/io-hub/src/robospierre/Makefile | 2 +- digital/io-hub/src/robospierre/bot.h | 10 ++ digital/io-hub/src/robospierre/clamp.c | 6 +- digital/io-hub/src/robospierre/init_defs.h | 50 ++++++++ digital/io-hub/src/robospierre/main.c | 14 ++- 7 files changed, 265 insertions(+), 10 deletions(-) create mode 100644 digital/ai/src/fsm/init.c create mode 100644 digital/io-hub/src/robospierre/init_defs.h (limited to 'digital/ai') diff --git a/digital/ai/src/fsm/init.c b/digital/ai/src/fsm/init.c new file mode 100644 index 00000000..1f933ce1 --- /dev/null +++ b/digital/ai/src/fsm/init.c @@ -0,0 +1,188 @@ +/* init.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 "asserv.h" +#include "contact.h" +#include "chrono.h" + +#define FSM_NAME AI +#include "fsm.h" +#include "fsm_queue.h" + +#include "bot.h" +#include "init_defs.h" + +/* + * Initialise robot position with a calibration procedure. + */ + +FSM_STATES ( + /* Initial state, waiting for jack. */ + INIT_START, + /* After first jack insertion, waiting for removal to initialise actuators. */ + INIT_WAITING_FIRST_JACK_OUT, + /* Initialising actuators, then waiting for second jack insertion. */ + INIT_INITIALISING_ACTUATORS, + /* After second jack insertion, waiting the operator remove its + * hand. */ + INIT_WAITING_HANDS_OUT, + /* Finding the first wall. */ + INIT_FINDING_FIRST_WALL, + /* Going away from the wall. */ + INIT_GOING_AWAY_FIRST_WALL, + /* Turning to face the other wall. */ + INIT_FACING_SECOND_WALL, + /* Waiting after rotation for robot to stabilize. */ + INIT_WAITING_AFTER_FACING_SECOND_WALL, + /* Finding the second wall. */ + INIT_FINDING_SECOND_WALL, + /* Going away from the wall. */ + INIT_GOING_AWAY_SECOND_WALL, +#ifdef INIT_START_POSITION_ANGLE + /* Facing the start position. */ + INIT_FACING_START_POSITION, +#endif + /* Going to start position. */ + INIT_GOING_TO_START_POSITION, + /* Waiting for the round start (waiting for the jack). */ + INIT_WAITING_SECOND_JACK_OUT, + /* Initialisation finished, nothing else to do. */ + INIT_FINISHED) + +FSM_EVENTS ( + /* XXX: temporarily here. */ + robot_move_success, + robot_move_failure, + /* Jack is inserted. */ + jack_inserted, + /* Jack is removed. */ + jack_removed, + /* Sent to initialise actuators. */ + init_actuators, + /* Sent to start round. */ + init_start_round) + +FSM_START_WITH (INIT_START) + +FSM_TRANS (INIT_START, jack_inserted, INIT_WAITING_FIRST_JACK_OUT) +{ + return FSM_NEXT (INIT_START, jack_inserted); +} + +FSM_TRANS (INIT_WAITING_FIRST_JACK_OUT, jack_removed, + INIT_INITIALISING_ACTUATORS) +{ + fsm_queue_post_event (FSM_EVENT (AI, init_actuators)); + return FSM_NEXT (INIT_WAITING_FIRST_JACK_OUT, jack_removed); +} + +FSM_TRANS (INIT_INITIALISING_ACTUATORS, jack_inserted, INIT_WAITING_HANDS_OUT) +{ + team_color = contact_get_color (); + return FSM_NEXT (INIT_INITIALISING_ACTUATORS, jack_inserted); +} + +FSM_TRANS_TIMEOUT (INIT_WAITING_HANDS_OUT, 225, INIT_FINDING_FIRST_WALL) +{ + asserv_set_speed (BOT_SPEED_INIT); + asserv_push_the_wall (INIT_FIRST_WALL_PUSH); + return FSM_NEXT_TIMEOUT (INIT_WAITING_HANDS_OUT); +} + +FSM_TRANS (INIT_FINDING_FIRST_WALL, robot_move_success, + INIT_GOING_AWAY_FIRST_WALL) +{ + asserv_move_linearly (INIT_FIRST_WALL_AWAY); + return FSM_NEXT (INIT_FINDING_FIRST_WALL, robot_move_success); +} + +FSM_TRANS (INIT_GOING_AWAY_FIRST_WALL, robot_move_success, + INIT_FACING_SECOND_WALL) +{ + asserv_goto_angle (INIT_SECOND_WALL_ANGLE); + return FSM_NEXT (INIT_GOING_AWAY_FIRST_WALL, robot_move_success); +} + +FSM_TRANS (INIT_FACING_SECOND_WALL, robot_move_success, + INIT_WAITING_AFTER_FACING_SECOND_WALL) +{ + return FSM_NEXT (INIT_FACING_SECOND_WALL, robot_move_success); +} + +FSM_TRANS_TIMEOUT (INIT_WAITING_AFTER_FACING_SECOND_WALL, 225 / 2, + INIT_FINDING_SECOND_WALL) +{ + asserv_push_the_wall (INIT_SECOND_WALL_PUSH); + return FSM_NEXT_TIMEOUT (INIT_WAITING_AFTER_FACING_SECOND_WALL); +} + +FSM_TRANS (INIT_FINDING_SECOND_WALL, robot_move_success, + INIT_GOING_AWAY_SECOND_WALL) +{ + asserv_move_linearly (INIT_SECOND_WALL_AWAY); + return FSM_NEXT (INIT_FINDING_SECOND_WALL, robot_move_success); +} + +#ifdef INIT_START_POSITION_ANGLE +FSM_TRANS (INIT_GOING_AWAY_SECOND_WALL, robot_move_success, + INIT_FACING_START_POSITION) +{ + asserv_goto_angle (INIT_START_POSITION_ANGLE); + return FSM_NEXT (INIT_GOING_AWAY_SECOND_WALL, robot_move_success); +} + +FSM_TRANS (INIT_FACING_START_POSITION, robot_move_success, + INIT_GOING_TO_START_POSITION) +{ + asserv_goto_xya (INIT_START_POSITION); + return FSM_NEXT (INIT_FACING_START_POSITION, robot_move_success); +} + +#else + +FSM_TRANS (INIT_GOING_AWAY_SECOND_WALL, robot_move_success, + INIT_GOING_TO_START_POSITION) +{ + asserv_goto_xya (INIT_START_POSITION); + return FSM_NEXT (INIT_GOING_AWAY_SECOND_WALL, robot_move_success); +} + +#endif + +FSM_TRANS (INIT_GOING_TO_START_POSITION, robot_move_success, + INIT_WAITING_SECOND_JACK_OUT) +{ + asserv_set_speed (BOT_SPEED_NORMAL); + return FSM_NEXT (INIT_GOING_TO_START_POSITION, robot_move_success); +} + +FSM_TRANS (INIT_WAITING_SECOND_JACK_OUT, jack_removed, INIT_FINISHED) +{ + chrono_init (); + fsm_queue_post_event (FSM_EVENT (AI, init_start_round)); + return FSM_NEXT (INIT_WAITING_SECOND_JACK_OUT, jack_removed); +} + diff --git a/digital/ai/tools/robospierre.py b/digital/ai/tools/robospierre.py index 8d5db276..e8384f6c 100644 --- a/digital/ai/tools/robospierre.py +++ b/digital/ai/tools/robospierre.py @@ -34,7 +34,8 @@ class Robot: self.io = io_hub.Proto (PopenIO (io_hub_cmd), proto_time, **io_hub.init.host['robospierre']) self.robot_start_pos = { - False: (700, 2100 - 250, math.radians (-270)), - True: (3000 - 700, 2100 - 250, math.radians (-270)) + # 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)) } diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index 20e9dee0..d69918f5 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -5,7 +5,7 @@ PROGS = io_hub # Sources to compile. io_hub_SOURCES = main.c \ clamp.c logistic.c \ - fsm.host.c fsm_AI_gen.avr.c fsm_queue.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 \ twi_master.c asserv.c mimot.c \ diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index e014321e..a986c015 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -34,6 +34,16 @@ # define BOT_SCALE 0.0415178942124 #endif +/** Distance between the front contact point and the robot center. */ +#define BOT_FRONT_CONTACT_DIST_MM 150 +/** Angle error at the front contact point. */ +#define BOT_FRONT_CONTACT_ANGLE_ERROR_DEG 0 + +/** Speed used for initialisation. */ +#define BOT_SPEED_INIT 0x10, 0x10, 0x10, 0x10 +/** Normal cruise speed. */ +#define BOT_SPEED_NORMAL 0x40, 0x40, 0x20, 0x20 + #ifdef HOST # define BOT_CLAMP_SLOT_FRONT_BOTTOM_ELEVATION_STEP 0 diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 6c063979..583cadb5 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -83,8 +83,6 @@ FSM_STATES ( CLAMP_MOVE_DST_CLAMP_OPENING) FSM_EVENTS ( - /* Here for the moment, to be moved later. */ - start, /* New element inside bottom slot. */ clamp_new_element, /* Order to drop elements. */ @@ -325,10 +323,10 @@ clamp_route (void) /* CLAMP FSM */ -FSM_TRANS (CLAMP_START, start, CLAMP_GOING_IDLE) +FSM_TRANS (CLAMP_START, init_actuators, CLAMP_GOING_IDLE) { clamp_move (CLAMP_SLOT_FRONT_MIDDLE); - return FSM_NEXT (CLAMP_START, start); + return FSM_NEXT (CLAMP_START, init_actuators); } FSM_TRANS (CLAMP_GOING_IDLE, clamp_move_success, CLAMP_IDLE) diff --git a/digital/io-hub/src/robospierre/init_defs.h b/digital/io-hub/src/robospierre/init_defs.h new file mode 100644 index 00000000..6ce6eaae --- /dev/null +++ b/digital/io-hub/src/robospierre/init_defs.h @@ -0,0 +1,50 @@ +#ifndef init_defs_h +#define init_defs_h +/* init_defs.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. + * + * }}} */ + +#include "playground.h" +#include "bot.h" + +/** Parameters to push the first wall. */ +#define INIT_FIRST_WALL_PUSH \ + 0, PG_X (BOT_FRONT_CONTACT_DIST_MM), 200, \ + PG_A_DEG (180 + BOT_FRONT_CONTACT_ANGLE_ERROR_DEG) +/** Parameters to go away from the first wall. */ +#define INIT_FIRST_WALL_AWAY -500 +/** Parameter to face the second wall. */ +#define INIT_SECOND_WALL_ANGLE PG_A_DEG (90) +/** Parameters to push the second wall. */ +#define INIT_SECOND_WALL_PUSH \ + 0, -1, PG_Y (PG_LENGTH - BOT_FRONT_CONTACT_DIST_MM), -1 +/** Parameters to go away from the second wall. */ +#define INIT_SECOND_WALL_AWAY -(200 - BOT_FRONT_CONTACT_DIST_MM) +/** Parameter to face the start position. */ +#define INIT_START_POSITION_ANGLE PG_A_DEG (0) +/** Start position. */ +#define INIT_START_POSITION \ + PG_X (200), PG_Y (PG_LENGTH - 200), PG_A_DEG (0), ASSERV_BACKWARD + +#endif /* init_defs_h */ diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 97d5bd40..f1b4d8de 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -110,9 +110,15 @@ main_event_to_fsm (void) /* Update FSM timeouts. */ FSM_HANDLE_TIMEOUT_E (AI); /* Motor status. */ - asserv_status_e mimot_motor0_status, mimot_motor1_status; + asserv_status_e robot_move_status, mimot_motor0_status, + mimot_motor1_status; + robot_move_status = asserv_move_cmd_status (); mimot_motor0_status = mimot_motor0_cmd_status (); mimot_motor1_status = mimot_motor1_cmd_status (); + if (robot_move_status == success) + FSM_HANDLE_E (AI, robot_move_success); + else if (robot_move_status == failure) + FSM_HANDLE_E (AI, robot_move_failure); if (mimot_motor0_status == success && mimot_motor1_status == success) FSM_HANDLE_E (AI, clamp_elevation_rotation_success); @@ -123,9 +129,11 @@ main_event_to_fsm (void) /* Clamp specific events. */ if (clamp_handle_event ()) return; - /* Jack, XXX to be changed! */ + /* Jack. */ if (!contact_get_jack ()) - FSM_HANDLE_E (AI, start); + FSM_HANDLE_E (AI, jack_inserted); + else + FSM_HANDLE_E (AI, jack_removed); /* Events from the event queue. */ if (fsm_queue_poll ()) { -- cgit v1.2.3 From 9bd92331b124a2123b3f9fb6bf84510c81ccce5e Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 21 May 2011 13:04:28 +0200 Subject: digital/ai/src/fsm: fix AVR timeout code --- digital/ai/src/fsm/fsm.host.c | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) (limited to 'digital/ai') diff --git a/digital/ai/src/fsm/fsm.host.c b/digital/ai/src/fsm/fsm.host.c index b7b6ab4e..ad791272 100644 --- a/digital/ai/src/fsm/fsm.host.c +++ b/digital/ai/src/fsm/fsm.host.c @@ -1451,18 +1451,19 @@ fsm_build_gen_avr_c (fsm_build_t *fsm, uint embedded_strings) fprintf (f, "\tint out = 0;\n"); fprintf (f, "\tfor (i = 0; i < fsm_%s_max_active_states; i++)\n\t{\n", fsm->name); - fprintf (f, "\t\tif (fsm_%s_timeout_counters[i] > 0)\n", + fprintf (f, "\t\tif (fsm_%s_timeout_counters[i] > 0)\n\t\t{\n", fsm->name); fprintf (f, "\t\t\tfsm_%s_timeout_counters[i]--;\n", fsm->name); - fprintf (f, "\t\tif (fsm_%s_timeout_counters[i] == 0)\n\t\t{\n", + fprintf (f, "\t\t\tif (fsm_%s_timeout_counters[i] == 0)\n\t\t\t{\n", fsm->name); - fprintf (f, "\t\t\tfsm_%s_handle (fsm_%s_timeout_events[fsm_%s_active_states[i]]);\n", + fprintf (f, "\t\t\t\tfsm_%s_handle (fsm_%s_timeout_events[fsm_%s_active_states[i]]);\n", fsm->name, fsm->name, fsm->name); - fprintf (f, "\t\t\tout = 1;\n"); - fprintf (f, "\t\t}\n\n"); + fprintf (f, "\t\t\t\tout = 1;\n"); + fprintf (f, "\t\t\t}\n"); + fprintf (f, "\t\t}\n"); fprintf (f, "\t}\n"); fprintf (f, "\treturn out;\n"); fprintf (f, "}\n\n"); -- cgit v1.2.3 From 8bd4ff291b5b76a636a68c6101bc1e0b7638b35c Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Sat, 21 May 2011 02:00:27 +0200 Subject: digital/ai: add function to set chrono as desired --- digital/ai/src/utils/chrono.c | 7 +++++++ digital/ai/src/utils/chrono.h | 8 ++++++++ 2 files changed, 15 insertions(+) (limited to 'digital/ai') diff --git a/digital/ai/src/utils/chrono.c b/digital/ai/src/utils/chrono.c index a1212e19..85851547 100644 --- a/digital/ai/src/utils/chrono.c +++ b/digital/ai/src/utils/chrono.c @@ -148,3 +148,10 @@ chrono_end_match (uint8_t block) ; #endif } + +void +chrono_set_timer (uint32_t elapsed_time) +{ + if (chrono_enabled_) + chrono_ov_count_ = elapsed_time / TIMER_PERIOD_MS; +} diff --git a/digital/ai/src/utils/chrono.h b/digital/ai/src/utils/chrono.h index 38bf3d61..580529cf 100644 --- a/digital/ai/src/utils/chrono.h +++ b/digital/ai/src/utils/chrono.h @@ -96,4 +96,12 @@ chrono_remaining_time (void); void chrono_end_match (uint8_t block); +/** + * Set timer at desired value. + * This function should be used for tests purpose only. + * @param elapsed_time elapsed time since beginning. + */ +void +chrono_set_timer (uint32_t elapsed_time); + #endif /* chrono_h */ -- 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 'digital/ai') 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 2f37578582ba9831122d55e189fe4cce41aa758a Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 23 May 2011 20:40:28 +0200 Subject: digital/ai/tools: use new door and clamp command --- digital/ai/tools/test_simu_control_robospierre.py | 13 +++---------- digital/io-hub/tools/io_hub/io_hub.py | 6 ++++++ 2 files changed, 9 insertions(+), 10 deletions(-) (limited to 'digital/ai') diff --git a/digital/ai/tools/test_simu_control_robospierre.py b/digital/ai/tools/test_simu_control_robospierre.py index acd3ca7c..77f017c2 100644 --- a/digital/ai/tools/test_simu_control_robospierre.py +++ b/digital/ai/tools/test_simu_control_robospierre.py @@ -106,10 +106,7 @@ class TestSimuControl (TestSimu): self.asserv.goto_angle (a) def clamp_command (self): - if self.clamp_var.get (): - self.io.pwm_set_timed (2, -0x3ff, 255, 0) - else: - self.io.pwm_set_timed (2, 0x3ff, 255, 0) + self.io.clamp_openclose (not self.clamp_var.get ()) def elevation_up_command (self): self.mimot.speed_pos ('a0', self.ELEVATION_STROKE / 2) @@ -131,12 +128,8 @@ class TestSimuControl (TestSimu): 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) + for i in (0, 2, 3, 5): + self.io.door (i, not self.doors_var.get ()) def drop_command (self): if self.drop_var.get (): diff --git a/digital/io-hub/tools/io_hub/io_hub.py b/digital/io-hub/tools/io_hub/io_hub.py index d6ae8618..aafc2d1d 100644 --- a/digital/io-hub/tools/io_hub/io_hub.py +++ b/digital/io-hub/tools/io_hub/io_hub.py @@ -60,6 +60,12 @@ class Proto: else: raise ValueError + def door (self, pos, open_): + self.proto.send ('d', 'BB', pos, (0, 1)[open_]) + + def clamp_openclose (self, open_): + self.proto.send ('d', 'BB', 0xff, (0, 1)[open_]) + def close (self): self.reset () self.proto.wait (lambda: True) -- cgit v1.2.3 From 579d7d9ad8caf4299c5c672b724d056d9e9d9797 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 24 May 2011 01:33:18 +0200 Subject: digital/io-hub: add move FSM --- digital/ai/tools/test_simu_control_robospierre.py | 9 +- digital/io-hub/src/robospierre/Makefile | 2 +- digital/io-hub/src/robospierre/bot.h | 4 + digital/io-hub/src/robospierre/main.c | 17 +- digital/io-hub/src/robospierre/main.h | 31 ++ digital/io-hub/src/robospierre/move.c | 496 ++++++++++++++++++++++ digital/io-hub/src/robospierre/move.h | 59 +++ digital/io-hub/tools/io_hub/io_hub.py | 3 + 8 files changed, 617 insertions(+), 4 deletions(-) create mode 100644 digital/io-hub/src/robospierre/main.h create mode 100644 digital/io-hub/src/robospierre/move.c create mode 100644 digital/io-hub/src/robospierre/move.h (limited to 'digital/ai') diff --git a/digital/ai/tools/test_simu_control_robospierre.py b/digital/ai/tools/test_simu_control_robospierre.py index 77f017c2..0d52219e 100644 --- a/digital/ai/tools/test_simu_control_robospierre.py +++ b/digital/ai/tools/test_simu_control_robospierre.py @@ -91,12 +91,19 @@ class TestSimuControl (TestSimu): self.backward_button = Checkbutton (self.control_frame, text = 'Backward', variable = self.backward_var) self.backward_button.pack () + self.goto_var = IntVar () + self.goto_button = Checkbutton (self.control_frame, + text = 'Goto FSM', variable = self.goto_var) + self.goto_button.pack () self.table_view.bind ('<1>', self.move) self.table_view.bind ('<3>', self.orient) def move (self, ev): pos = self.table_view.screen_coord ((ev.x, ev.y)) - self.asserv.goto (pos[0], pos[1], self.backward_var.get ()) + if self.goto_var.get (): + self.io.goto (pos[0], pos[1], self.backward_var.get ()) + else: + self.asserv.goto (pos[0], pos[1], self.backward_var.get ()) def orient (self, ev): x, y = self.table_view.screen_coord ((ev.x, ev.y)) diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index dd4bfe5c..05b3d92c 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -5,7 +5,7 @@ PROGS = io_hub # Sources to compile. io_hub_SOURCES = main.c \ clamp.c logistic.c \ - radar_defs.c radar.c path.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 \ contact.avr.c contact.host.c \ diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index d68b0c6b..05c2b436 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -34,6 +34,10 @@ # define BOT_SCALE 0.0415178942124 #endif +/** Distance from the robot axis to the front. */ +#define BOT_SIZE_FRONT 150 +/** Distance from the robot axis to the back. */ +#define BOT_SIZE_BACK 150 /** Distance from the robot axis to the side. */ #define BOT_SIZE_SIDE 190 diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index b1abfd8a..b1e85b72 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -52,6 +52,7 @@ #include "clamp.h" #include "logistic.h" #include "path.h" +#include "move.h" #include "bot.h" @@ -160,7 +161,9 @@ main_event_to_fsm (void) /* Post the event */ FSM_HANDLE_VAR_E (AI, save_event); } - + /* Check obstables. */ + if (move_check_obstacles ()) + return; } /** Main (and infinite) loop. */ @@ -191,7 +194,7 @@ main_loop (void) position_t robot_pos; asserv_get_position (&robot_pos); main_obstacles_nb = radar_update (&robot_pos, main_obstacles_pos); - //move_obstacles_update (); + move_obstacles_update (); simu_send_pos_report (main_obstacles_pos, main_obstacles_nb, 0); } /* Update AI modules. */ @@ -252,6 +255,16 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) v8_to_v16 (args[3], args[4]), v8_to_v16 (args[5], args[6])); break; + case c ('m', 5): + /* Go to position. + * - 2w: x, y. + * - 1b: backward. */ + { + vect_t position = { v8_to_v16 (args[0], args[1]), + v8_to_v16 (args[2], args[3]) }; + move_start_noangle (position, args[4], 0); + } + break; case c ('c', 1): /* Move clamp. * - 1b: position. */ diff --git a/digital/io-hub/src/robospierre/main.h b/digital/io-hub/src/robospierre/main.h new file mode 100644 index 00000000..529aa0b2 --- /dev/null +++ b/digital/io-hub/src/robospierre/main.h @@ -0,0 +1,31 @@ +#ifndef main_h +#define main_h +/* main.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. + * + * }}} */ + +extern vect_t main_obstacles_pos[2]; +extern uint8_t main_obstacles_nb; + +#endif /* main_h */ diff --git a/digital/io-hub/src/robospierre/move.c b/digital/io-hub/src/robospierre/move.c new file mode 100644 index 00000000..18759c14 --- /dev/null +++ b/digital/io-hub/src/robospierre/move.c @@ -0,0 +1,496 @@ +/* move.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 "move.h" + +#include "main.h" +#include "asserv.h" + +#define FSM_NAME AI +#include "fsm.h" +#include "fsm_queue.h" + +#include "radar.h" +#include "path.h" + +#include "modules/utils/utils.h" + +#include + +/** Move context. */ +struct move_t +{ + /** Final position. */ + position_t final; + /** Use angle consign for final point. */ + uint8_t with_angle; + /** Next step. */ + vect_t step; + /** Next step angle. */ + uint16_t step_angle; + /** Next step with_angle. */ + uint8_t step_with_angle; + /** Next step backward. */ + uint8_t step_backward; + /** Non zero means this is a tricky move, slow down, and minimize + * turns. */ + uint8_t slow; + /** Backward direction allowed flag. */ + uint8_t backward_movement_allowed; + /** Try again counter. */ + uint8_t try_again_counter; + /** Dirty fix to know this is the final move. */ + uint8_t final_move; + /** Distance to remove from path. */ + int16_t shorten; +}; + +/* Global context. */ +struct move_t move_data; + +void +move_start (position_t position, uint8_t backward) +{ + /* Set parameters. */ + move_data.final = position; + move_data.with_angle = 1; + move_data.backward_movement_allowed = backward; + move_data.final_move = 0; + move_data.shorten = 0; + /* Reset try counter. */ + move_data.try_again_counter = 3; + /* Start the FSM. */ + FSM_HANDLE (AI, move_start); +} + +void +move_start_noangle (vect_t position, uint8_t backward, int16_t shorten) +{ + /* Set parameters. */ + move_data.final.v = position; + move_data.with_angle = 0; + move_data.backward_movement_allowed = backward; + move_data.final_move = 0; + move_data.shorten = shorten; + /* Reset try counter. */ + move_data.try_again_counter = 3; + /* Start the FSM. */ + FSM_HANDLE (AI, move_start); +} + +void +move_obstacles_update (void) +{ + uint8_t i; + for (i = 0; i < main_obstacles_nb; i++) + path_obstacle (i, main_obstacles_pos[i], MOVE_OBSTACLE_RADIUS, 0, + MOVE_OBSTACLE_VALIDITY); +} + +uint8_t +move_check_obstacles (void) +{ + if (FSM_CAN_HANDLE (AI, obstacle_in_front)) + { + position_t robot_pos; + asserv_get_position (&robot_pos); + if (radar_blocking (&robot_pos.v, &move_data.step, main_obstacles_pos, + main_obstacles_nb)) + if (FSM_HANDLE (AI, obstacle_in_front)) + return 1; + } + return 0; +} + +FSM_STATES ( + /* Waiting for the start order. */ + MOVE_IDLE, + /* Rotating towards next point. */ + MOVE_ROTATING, + /* Moving to a position (intermediate or final). */ + MOVE_MOVING, + /* Moving backward to go away from what is blocking the bot. */ + MOVE_MOVING_BACKWARD_TO_TURN_FREELY, + /* Waiting for obstacle to disappear. */ + MOVE_WAIT_FOR_CLEAR_PATH) + +FSM_EVENTS ( + /* Initialize the FSM and start the movement directly. */ + move_start, + /* Movement success. */ + move_success, + /* Movement failure. */ + move_failure, + /* The bot has seen something (front is the same when going backward). */ + obstacle_in_front) + +FSM_START_WITH (MOVE_IDLE) + +/** Go to current step, low level function. */ +static void +move_go (void) +{ + vect_t dst = move_data.step; + /* Modify final point if requested. */ + if (move_data.final_move && move_data.shorten) + { + /* Compute a vector from destination to robot with lenght + * 'shorten'. */ + position_t robot_position; + asserv_get_position (&robot_position); + vect_t v = robot_position.v; + vect_sub (&v, &move_data.step); + int16_t d = vect_norm (&v); + if (d > move_data.shorten) + { + vect_scale_f824 (&v, 0x1000000 / d * move_data.shorten); + vect_translate (&dst, &v); + } + } + if (move_data.step_with_angle) + asserv_goto_xya (dst.x, dst.y, move_data.step_angle, + move_data.step_backward); + else + asserv_goto (dst.x, dst.y, move_data.step_backward); +} + +/** Go or rotate toward position, returns 1 for linear move, 2 for angular + * move. */ +static uint8_t +move_go_or_rotate (vect_t dst, uint16_t angle, uint8_t with_angle, + uint8_t backward) +{ + position_t robot_position; + /* Remember step. */ + move_data.step = dst; + move_data.step_angle = angle; + move_data.step_with_angle = with_angle; + move_data.step_backward = backward; + /* Compute angle to destination. */ + asserv_get_position (&robot_position); + vect_t v = dst; vect_sub (&v, &robot_position.v); + uint16_t dst_angle = atan2 (v.y, v.x) * ((1l << 16) / (2 * M_PI)); + if (backward & ASSERV_BACKWARD) + dst_angle += 0x8000; + if ((backward & ASSERV_REVERT_OK) + && (dst_angle ^ robot_position.a) & 0x8000) + dst_angle += 0x8000; + int16_t diff = dst_angle - robot_position.a; + /* Move or rotate. */ + if (UTILS_ABS (diff) < 0x1000) + { + move_go (); + return 1; + } + else + { + asserv_goto_angle (dst_angle); + return 2; + } +} + +/** Go to next position computed by path module, to be called by + * move_path_init and move_path_next. Returns 1 for linear move, 2 for angular + * move. */ +static uint8_t +move_go_to_next (vect_t dst) +{ + uint8_t r; + /* If it is not the last position. */ + if (dst.x != move_data.final.v.x || dst.y != move_data.final.v.y) + { + /* Not final position. */ + move_data.final_move = 0; + /* Goto without angle. */ + r = move_go_or_rotate (dst, 0, 0, move_data.backward_movement_allowed + | (move_data.slow ? ASSERV_REVERT_OK : 0)); + } + else + { + /* Final position. */ + move_data.final_move = 1; + /* Goto with angle if requested. */ + r = move_go_or_rotate (dst, move_data.final.a, move_data.with_angle, + move_data.backward_movement_allowed); + } + /* Next time, do not use slow. */ + move_data.slow = 0; + return r; +} + +/** Update and go to first position, return non zero if a path is found, 1 for + * linear move, 2 for angular move. */ +static uint8_t +move_path_init (void) +{ + uint8_t found; + vect_t dst; + /* Get the current position */ + position_t current_pos; + asserv_get_position (¤t_pos); + /* Give the current position of the bot to the path module */ + path_endpoints (current_pos.v, move_data.final.v); + /* Update the path module */ + move_data.slow = 0; + path_update (); + found = path_get_next (&dst); + /* If not found, try to escape. */ + if (!found) + { + move_data.slow = 1; + path_escape (8); + path_update (); + found = path_get_next (&dst); + } + /* If found, go. */ + if (found) + { + return move_go_to_next (dst); + } + else + { + /* Error, not final move. */ + move_data.final_move = 0; + return 0; + } +} + +/** Go to next position in path. Returns 1 for linear move, 2 for angular + * move. */ +static uint8_t +move_path_next (void) +{ + vect_t dst; + path_get_next (&dst); + return move_go_to_next (dst); +} + +FSM_TRANS (MOVE_IDLE, move_start, + path_found_rotate, MOVE_ROTATING, + path_found, MOVE_MOVING, + no_path_found, MOVE_IDLE) +{ + uint8_t next = move_path_init (); + if (next) + { + if (next == 2) + return FSM_NEXT (MOVE_IDLE, move_start, path_found_rotate); + else + return FSM_NEXT (MOVE_IDLE, move_start, path_found); + } + else + { + fsm_queue_post_event (FSM_EVENT (AI, move_failure)); + return FSM_NEXT (MOVE_IDLE, move_start, no_path_found); + } +} + +FSM_TRANS (MOVE_ROTATING, + robot_move_success, + MOVE_MOVING) +{ + move_go (); + return FSM_NEXT (MOVE_ROTATING, robot_move_success); +} + +FSM_TRANS (MOVE_ROTATING, + robot_move_failure, + MOVE_MOVING) +{ + move_go (); + return FSM_NEXT (MOVE_ROTATING, robot_move_failure); +} + +FSM_TRANS_TIMEOUT (MOVE_ROTATING, 1250, + MOVE_MOVING) +{ + move_go (); + return FSM_NEXT_TIMEOUT (MOVE_ROTATING); +} + +FSM_TRANS (MOVE_MOVING, robot_move_success, + done, MOVE_IDLE, + path_found_rotate, MOVE_ROTATING, + path_found, MOVE_MOVING, + no_path_found, MOVE_IDLE) +{ + if (move_data.final_move) + { + fsm_queue_post_event (FSM_EVENT (AI, move_success)); + return FSM_NEXT (MOVE_MOVING, robot_move_success, done); + } + else + { + uint8_t next = move_path_next (); + if (next == 2) + return FSM_NEXT (MOVE_MOVING, robot_move_success, path_found_rotate); + else + return FSM_NEXT (MOVE_MOVING, robot_move_success, path_found); + } + //return FSM_NEXT (MOVE_MOVING, robot_move_success, no_path_found); +} + +static void +move_moving_backward_to_turn_freely (void) +{ + move_data.final_move = 0; + /* Assume there is an obstacle in front of the robot. */ + position_t robot_pos; + asserv_get_position (&robot_pos); + vect_t obstacle_pos; + int16_t dist = asserv_get_last_moving_direction () == DIRECTION_FORWARD + ? BOT_SIZE_FRONT + MOVE_REAL_OBSTACLE_RADIUS + : -(BOT_SIZE_BACK + MOVE_REAL_OBSTACLE_RADIUS); + vect_from_polar_uf016 (&obstacle_pos, dist, robot_pos.a); + vect_translate (&obstacle_pos, &robot_pos.v); + path_obstacle (0, obstacle_pos, MOVE_OBSTACLE_RADIUS, 0, + MOVE_OBSTACLE_VALIDITY); + /* Move backward to turn freely. */ + asserv_move_linearly (asserv_get_last_moving_direction () + == DIRECTION_FORWARD ? -300 : 300); +} + +FSM_TRANS (MOVE_MOVING, + robot_move_failure, + MOVE_MOVING_BACKWARD_TO_TURN_FREELY) +{ + move_moving_backward_to_turn_freely (); + return FSM_NEXT (MOVE_MOVING, robot_move_failure); +} + +FSM_TRANS_TIMEOUT (MOVE_MOVING, 2500, + MOVE_MOVING_BACKWARD_TO_TURN_FREELY) +{ + move_moving_backward_to_turn_freely (); + return FSM_NEXT_TIMEOUT (MOVE_MOVING); +} + +FSM_TRANS (MOVE_MOVING, obstacle_in_front, + tryagain, MOVE_WAIT_FOR_CLEAR_PATH, + tryout, MOVE_IDLE) +{ + move_data.final_move = 0; + asserv_stop_motor (); + if (--move_data.try_again_counter == 0) + { + fsm_queue_post_event (FSM_EVENT (AI, move_failure)); + return FSM_NEXT (MOVE_MOVING, obstacle_in_front, tryout); + } + else + return FSM_NEXT (MOVE_MOVING, obstacle_in_front, tryagain); +} + +FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, + tryout, MOVE_IDLE, + path_found_rotate, MOVE_ROTATING, + path_found, MOVE_MOVING, + no_path_found, MOVE_IDLE) +{ + if (--move_data.try_again_counter == 0) + { + fsm_queue_post_event (FSM_EVENT (AI, move_failure)); + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, tryout); + } + else + { + uint8_t next = move_path_init (); + if (next) + { + if (next == 2) + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, path_found_rotate); + else + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, path_found); + } + else + { + fsm_queue_post_event (FSM_EVENT (AI, move_failure)); + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, no_path_found); + } + } +} + +FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, + tryout, MOVE_IDLE, + path_found_rotate, MOVE_ROTATING, + path_found, MOVE_MOVING, + no_path_found_tryagain, MOVE_WAIT_FOR_CLEAR_PATH, + no_path_found_tryout, MOVE_IDLE) +{ + if (--move_data.try_again_counter == 0) + { + fsm_queue_post_event (FSM_EVENT (AI, move_failure)); + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, tryout); + } + else + { + uint8_t next = move_path_init (); + if (next) + { + if (next == 2) + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, path_found_rotate); + else + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, path_found); + } + else + { + if (--move_data.try_again_counter == 0) + { + fsm_queue_post_event (FSM_EVENT (AI, move_failure)); + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, no_path_found_tryout); + } + else + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, no_path_found_tryagain); + } + } +} + +FSM_TRANS_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, 250, + path_found_rotate, MOVE_ROTATING, + path_found, MOVE_MOVING, + no_path_found_tryagain, MOVE_WAIT_FOR_CLEAR_PATH, + no_path_found_tryout, MOVE_IDLE) +{ + /* Try to move. */ + uint8_t next = move_path_init (); + if (next) + { + if (next == 2) + return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, path_found_rotate); + else + return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, path_found); + } + else + { + /* Error, no new position, should we try again? */ + if (--move_data.try_again_counter == 0) + { + fsm_queue_post_event (FSM_EVENT (AI, move_failure)); + return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, no_path_found_tryout); + } + else + return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, no_path_found_tryagain); + } +} + diff --git a/digital/io-hub/src/robospierre/move.h b/digital/io-hub/src/robospierre/move.h new file mode 100644 index 00000000..877002fd --- /dev/null +++ b/digital/io-hub/src/robospierre/move.h @@ -0,0 +1,59 @@ +#ifndef move_h +#define move_h +/* move.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. + * + * }}} */ +#include "defs.h" +#include "bot.h" + +/** Real radius of an obstacle. */ +#define MOVE_REAL_OBSTACLE_RADIUS 150 + +/** Obstacle radius for the path module. + * It corresponds to the real radius of the obstacle plus the distance you + * want to add to avoid it. */ +#define MOVE_OBSTACLE_RADIUS (MOVE_REAL_OBSTACLE_RADIUS \ + + RADAR_CLEARANCE_MM \ + + BOT_SIZE_SIDE) + +/** Obstacle validity time (in term of number of cycles). */ +#define MOVE_OBSTACLE_VALIDITY (3 * 225) + +/** Go to a position, see asserv.h for backward argument. */ +void +move_start (position_t position, uint8_t backward); + +/** Go to a position, with no angle consign. */ +void +move_start_noangle (vect_t position, uint8_t backward, int16_t shorten); + +/** To be called when obstacles positions are computed. */ +void +move_obstacles_update (void); + +/** Check for blocking obstacles, return non zero if an event is handled. */ +uint8_t +move_check_obstacles (void); + +#endif /* move_h */ diff --git a/digital/io-hub/tools/io_hub/io_hub.py b/digital/io-hub/tools/io_hub/io_hub.py index aafc2d1d..06d237e9 100644 --- a/digital/io-hub/tools/io_hub/io_hub.py +++ b/digital/io-hub/tools/io_hub/io_hub.py @@ -44,6 +44,9 @@ class Proto: def pwm_set_timed (self, index, value, time, rest_value): self.proto.send ('w', 'BhHh', index, value, time, rest_value) + def goto (self, x, y, backward): + self.proto.send ('m', 'hhB', x, y, backward) + def clamp_move (self, pos): self.proto.send ('c', 'B', pos) -- cgit v1.2.3 From 4f55b190e1ea314d8ce9e56ecb42bacd26ed80ed Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 25 May 2011 01:13:10 +0200 Subject: digital/{ai,io-hub}: move events --- digital/ai/src/fsm/init.c | 3 --- digital/io-hub/src/robospierre/move.c | 4 ++++ 2 files changed, 4 insertions(+), 3 deletions(-) (limited to 'digital/ai') diff --git a/digital/ai/src/fsm/init.c b/digital/ai/src/fsm/init.c index 1f933ce1..51ba31ab 100644 --- a/digital/ai/src/fsm/init.c +++ b/digital/ai/src/fsm/init.c @@ -73,9 +73,6 @@ FSM_STATES ( INIT_FINISHED) FSM_EVENTS ( - /* XXX: temporarily here. */ - robot_move_success, - robot_move_failure, /* Jack is inserted. */ jack_inserted, /* Jack is removed. */ diff --git a/digital/io-hub/src/robospierre/move.c b/digital/io-hub/src/robospierre/move.c index 18759c14..7ce9b3c8 100644 --- a/digital/io-hub/src/robospierre/move.c +++ b/digital/io-hub/src/robospierre/move.c @@ -137,6 +137,10 @@ FSM_STATES ( MOVE_WAIT_FOR_CLEAR_PATH) FSM_EVENTS ( + /* Report from asserv after a successful move command. */ + robot_move_success, + /* Report from asserv after a failed move command. */ + robot_move_failure, /* Initialize the FSM and start the movement directly. */ move_start, /* Movement success. */ -- cgit v1.2.3 From 22d384a1c0fc9464c25db92550210c4d633c97e1 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 27 May 2011 00:21:28 +0200 Subject: digital/ai/src/fsm: fix timeout assignment --- digital/ai/src/fsm/fsm.host.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) (limited to 'digital/ai') diff --git a/digital/ai/src/fsm/fsm.host.c b/digital/ai/src/fsm/fsm.host.c index ad791272..ed93221d 100644 --- a/digital/ai/src/fsm/fsm.host.c +++ b/digital/ai/src/fsm/fsm.host.c @@ -1356,7 +1356,9 @@ fsm_build_gen_avr_c (fsm_build_t *fsm, uint embedded_strings) fprintf (f, "\t\t\thandled = 1;\n"); if (fsm->timeouts != NULL) { - fprintf (f, "\t\t\tfsm_%s_timeout_counters[i] = fsm_%s_timeout_values[e];\n", + fprintf (f, "\t\t\tfsm_%s_timeout_counters[i] = " + "fsm_%s_timeout_values[fsm_%s_active_states[i]];\n", + fsm->name, fsm->name, fsm->name); } -- 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 'digital/ai') 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 'digital/ai') 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 00f78226167fcb06d8b0c4bddf97018c4c2175ef Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 30 May 2011 00:02:46 +0200 Subject: digital/{ai,io-hub}: fix wrong team color definition --- digital/ai/src/common/defs.h | 4 ++-- digital/io-hub/src/common/contact.avr.c | 2 +- digital/io-hub/src/common/contact.host.c | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) (limited to 'digital/ai') diff --git a/digital/ai/src/common/defs.h b/digital/ai/src/common/defs.h index beec5608..12c47584 100644 --- a/digital/ai/src/common/defs.h +++ b/digital/ai/src/common/defs.h @@ -53,8 +53,8 @@ typedef struct position_t position_t; /** Team color, determine the start zone side. */ enum team_color_e { - TEAM_COLOR_LEFT = 0, - TEAM_COLOR_RIGHT = 1 + TEAM_COLOR_RIGHT = 0, + TEAM_COLOR_LEFT = 1 }; /** Current team color, to be read at start up. */ diff --git a/digital/io-hub/src/common/contact.avr.c b/digital/io-hub/src/common/contact.avr.c index 9f6869b4..d6692566 100644 --- a/digital/io-hub/src/common/contact.avr.c +++ b/digital/io-hub/src/common/contact.avr.c @@ -73,7 +73,7 @@ contact_update (void) enum team_color_e contact_get_color (void) { - return !IO_GET (CONTACT_COLOR) ? TEAM_COLOR_LEFT : TEAM_COLOR_RIGHT; + return IO_GET (CONTACT_COLOR) ? TEAM_COLOR_LEFT : TEAM_COLOR_RIGHT; } uint8_t diff --git a/digital/io-hub/src/common/contact.host.c b/digital/io-hub/src/common/contact.host.c index 5e2309c4..cc302f46 100644 --- a/digital/io-hub/src/common/contact.host.c +++ b/digital/io-hub/src/common/contact.host.c @@ -49,7 +49,7 @@ contact_handle (void *user, mex_msg_t *msg) uint32_t contacts; mex_msg_pop (msg, "L", &contacts); ctx.all = contacts; - ctx.color_state = !(contacts & 1) ? TEAM_COLOR_LEFT : TEAM_COLOR_RIGHT; + ctx.color_state = (contacts & 1) ? TEAM_COLOR_LEFT : TEAM_COLOR_RIGHT; ctx.jack_state = (contacts & 2) ? 1 : 0; contacts >>= 2; #define CONTACT(io) do { \ -- cgit v1.2.3 From 31f85163b5981a78a2b6704168d0ae12929b53be Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 31 May 2011 09:39:16 +0200 Subject: digital/mimot: better find zero/limit --- digital/ai/src/twi_master/mimot.c | 34 ++++++++++++++++++++++++++++------ digital/ai/src/twi_master/mimot.h | 10 ++++++++++ digital/mimot/src/dirty/aux.c | 12 ++++++++---- digital/mimot/src/dirty/aux.h | 8 ++++++-- digital/mimot/src/dirty/main.c | 13 ++++++++++--- digital/mimot/src/dirty/twi_proto.c | 26 +++++++++++++++++--------- 6 files changed, 79 insertions(+), 24 deletions(-) (limited to 'digital/ai') diff --git a/digital/ai/src/twi_master/mimot.c b/digital/ai/src/twi_master/mimot.c index c3b82823..6159e6c9 100644 --- a/digital/ai/src/twi_master/mimot.c +++ b/digital/ai/src/twi_master/mimot.c @@ -165,20 +165,42 @@ mimot_move_motor1_absolute (uint16_t position, uint8_t speed) void mimot_motor0_zero_position (int8_t speed) +{ + mimot_motor0_find_zero (speed, 0, 0); +} + +void +mimot_motor1_zero_position (int8_t speed) +{ + mimot_motor1_find_zero (speed, 0, 0); +} + +void +mimot_motor0_find_zero (int8_t speed, uint8_t use_switch, + uint16_t reset_position) { uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); buffer[0] = 'B'; - buffer[1] = speed; - twi_master_send_buffer (2); + buffer[1] = 0; + buffer[2] = speed; + buffer[3] = use_switch; + buffer[4] = v16_to_v8 (reset_position, 1); + buffer[5] = v16_to_v8 (reset_position, 0); + twi_master_send_buffer (6); } void -mimot_motor1_zero_position (int8_t speed) +mimot_motor1_find_zero (int8_t speed, uint8_t use_switch, + uint16_t reset_position) { uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); - buffer[0] = 'C'; - buffer[1] = speed; - twi_master_send_buffer (2); + buffer[0] = 'B'; + buffer[1] = 1; + buffer[2] = speed; + buffer[3] = use_switch; + buffer[4] = v16_to_v8 (reset_position, 1); + buffer[5] = v16_to_v8 (reset_position, 0); + twi_master_send_buffer (6); } void diff --git a/digital/ai/src/twi_master/mimot.h b/digital/ai/src/twi_master/mimot.h index 97575b0d..c59c1608 100644 --- a/digital/ai/src/twi_master/mimot.h +++ b/digital/ai/src/twi_master/mimot.h @@ -92,6 +92,16 @@ mimot_motor0_zero_position (int8_t speed); void mimot_motor1_zero_position (int8_t speed); +/** Find zero position. */ +void +mimot_motor0_find_zero (int8_t speed, uint8_t use_switch, + uint16_t reset_position); + +/** Find zero position. */ +void +mimot_motor1_find_zero (int8_t speed, uint8_t use_switch, + uint16_t reset_position); + /** Clamp motor0. */ void mimot_motor0_clamp (int8_t speed, int16_t pwm); diff --git a/digital/mimot/src/dirty/aux.c b/digital/mimot/src/dirty/aux.c index e00e2c08..e36489ba 100644 --- a/digital/mimot/src/dirty/aux.c +++ b/digital/mimot/src/dirty/aux.c @@ -184,7 +184,7 @@ aux_traj_find_zero (struct aux_t *aux) { aux->speed->cons = 0; state_finish (aux->state); - aux->pos = 0; + aux->pos = aux->reset_pos; aux->traj_mode = AUX_TRAJ_DONE; } break; @@ -193,12 +193,14 @@ aux_traj_find_zero (struct aux_t *aux) /** Start find zero mode. */ void -aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, uint8_t seq) +aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, int16_t reset_pos, + uint8_t seq) { aux->traj_mode = AUX_TRAJ_FIND_ZERO_NOT; aux->speed->use_pos = 0; aux->speed->cons = speed << 8; state_start (aux->state, MODE_TRAJ, seq); + aux->reset_pos = reset_pos; } /** Find limit mode. */ @@ -225,7 +227,7 @@ aux_traj_find_limit (struct aux_t *aux) if (!--aux->wait) { state_finish (aux->state); - aux->pos = 0; + aux->pos = aux->reset_pos; aux->traj_mode = AUX_TRAJ_DONE; } break; @@ -234,13 +236,15 @@ aux_traj_find_limit (struct aux_t *aux) /** Start find limit mode. */ void -aux_traj_find_limit_start (struct aux_t *aux, int8_t speed, uint8_t seq) +aux_traj_find_limit_start (struct aux_t *aux, int8_t speed, int16_t reset_pos, + uint8_t seq) { aux->traj_mode = AUX_TRAJ_FIND_LIMIT; aux->speed->use_pos = 0; aux->speed->cons = speed << 8; state_start (aux->state, MODE_TRAJ, seq); aux->state->variant = 4; + aux->reset_pos = reset_pos; } /** Update trajectories for one motor. */ diff --git a/digital/mimot/src/dirty/aux.h b/digital/mimot/src/dirty/aux.h index f31b789a..7ec60617 100644 --- a/digital/mimot/src/dirty/aux.h +++ b/digital/mimot/src/dirty/aux.h @@ -50,6 +50,8 @@ struct aux_t uint8_t zero_bv; /** Handle blocking by aux instead of pos. */ uint8_t handle_blocking; + /** Reset position after zero is found. */ + int16_t reset_pos; }; extern struct aux_t aux[AC_ASSERV_AUX_NB]; @@ -68,10 +70,12 @@ aux_traj_clamp_start (struct aux_t *aux, int8_t speed, int16_t clampin_pwm, uint8_t seq); void -aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, uint8_t seq); +aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, int16_t reset_pos, + uint8_t seq); void -aux_traj_find_limit_start (struct aux_t *aux, int8_t speed, uint8_t seq); +aux_traj_find_limit_start (struct aux_t *aux, int8_t speed, int16_t reset_pos, + uint8_t seq); void aux_traj_update (void); diff --git a/digital/mimot/src/dirty/main.c b/digital/mimot/src/dirty/main.c index 5dcfd8c7..2b55259b 100644 --- a/digital/mimot/src/dirty/main.c +++ b/digital/mimot/src/dirty/main.c @@ -287,15 +287,22 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) aux_traj_clamp_start (auxp, args[1], v8_to_v16 (args[2], args[3]), args[4]); break; - case c ('y', 3): + case c ('y', 6): /* Auxiliary find zero. * - b: aux index. * - b: speed. + * - b: use switch. + * - w: reset position. * - b: sequence number. */ if (!auxp) { proto_send0 ('?'); return; } - if (args[2] == state->sequence) + if (args[5] == state->sequence) break; - aux_traj_find_limit_start (auxp, args[1], args[2]); + if (args[2]) + aux_traj_find_zero_start (auxp, args[1], + v8_to_v16 (args[3], args[4]), args[5]); + else + aux_traj_find_limit_start (auxp, args[1], + v8_to_v16 (args[3], args[4]), args[5]); break; case c ('a', 2): /* Set all acknoledge. diff --git a/digital/mimot/src/dirty/twi_proto.c b/digital/mimot/src/dirty/twi_proto.c index bda255ef..2f935fda 100644 --- a/digital/mimot/src/dirty/twi_proto.c +++ b/digital/mimot/src/dirty/twi_proto.c @@ -122,11 +122,6 @@ twi_proto_callback (u8 *buf, u8 size) speed_aux[0].max = buf[4]; aux_traj_goto_start (&aux[0], v8_to_v16 (buf[2], buf[3]), 0); break; - case c ('B', 1): - /* Find the zero position of the aux0. - * - b: speed. */ - aux_traj_find_limit_start (&aux[0], buf[2], 0); - break; case c ('c', 3): /* Move the aux1. * - w: new position. @@ -134,10 +129,23 @@ twi_proto_callback (u8 *buf, u8 size) speed_aux[1].max = buf[4]; aux_traj_goto_start (&aux[1], v8_to_v16 (buf[2], buf[3]), 0); break; - case c ('C', 1): - /* Find the zero position of the aux1. - * - b: speed. */ - aux_traj_find_limit_start (&aux[1], buf[2], 0); + case c ('B', 5): + /* Find the zero position. + * - b: aux index. + * - b: speed. + * - b: use switch. + * - w: reset position. */ + if (buf[2] < AC_ASSERV_AUX_NB) + { + if (buf[4]) + aux_traj_find_zero_start (&aux[buf[2]], buf[3], + v8_to_v16 (buf[5], buf[6]), 0); + else + aux_traj_find_limit_start (&aux[buf[2]], buf[3], + v8_to_v16 (buf[5], buf[6]), 0); + } + else + buf[0] = 0; break; case c ('l', 4): /* Clamp. -- cgit v1.2.3 From d5ca3805ed0eb4829b491352a31165b9fd58b15e Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 08:24:48 +0200 Subject: digital/io-hub: handle bumpers --- digital/ai/src/twi_master/mimot.c | 6 +++ digital/ai/src/twi_master/mimot.h | 4 ++ digital/io-hub/src/robospierre/main.c | 4 +- digital/io-hub/src/robospierre/pawn_sensor.c | 77 ++++++++++++++++++++++++++++ digital/io-hub/src/robospierre/pawn_sensor.h | 8 +++ digital/io-hub/src/robospierre/top.c | 28 ++++++++-- digital/mimot/src/dirty/simu.host.c | 2 +- 7 files changed, 123 insertions(+), 6 deletions(-) (limited to 'digital/ai') diff --git a/digital/ai/src/twi_master/mimot.c b/digital/ai/src/twi_master/mimot.c index 6159e6c9..8df84538 100644 --- a/digital/ai/src/twi_master/mimot.c +++ b/digital/ai/src/twi_master/mimot.c @@ -97,6 +97,12 @@ mimot_motor1_cmd_status (void) return none; } +uint8_t +mimot_get_input (void) +{ + return mimot_status.input_port; +} + uint16_t mimot_get_motor0_position (void) { diff --git a/digital/ai/src/twi_master/mimot.h b/digital/ai/src/twi_master/mimot.h index c59c1608..c9de7090 100644 --- a/digital/ai/src/twi_master/mimot.h +++ b/digital/ai/src/twi_master/mimot.h @@ -56,6 +56,10 @@ mimot_motor0_cmd_status (void); asserv_status_e mimot_motor1_cmd_status (void); +/** Return input port state. */ +uint8_t +mimot_get_input (void); + /** Get motor0 position in steps. */ uint16_t mimot_get_motor0_position (void); diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 1e12c427..9ae644e6 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -41,6 +41,7 @@ #include "pwm.h" #include "contact.h" #include "codebar.h" +#include "pawn_sensor.h" #include "radar.h" #define FSM_NAME AI @@ -207,6 +208,7 @@ main_loop (void) /* Update IO modules. */ pwm_update (); contact_update (); + pawn_sensor_update (); if (usdist_update ()) { position_t robot_pos; @@ -234,7 +236,7 @@ main_loop (void) } if (main_stats_contact_ && !--main_stats_contact_cpt_) { - proto_send1d ('P', contact_all ()); + proto_send1d ('P', contact_all () | (uint32_t) mimot_get_input () << 24); main_stats_contact_cpt_ = main_stats_contact_; } if (main_stats_codebar_ && !--main_stats_codebar_cpt_) diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c index ccb5eb5b..5ca18759 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.c +++ b/digital/io-hub/src/robospierre/pawn_sensor.c @@ -23,6 +23,7 @@ * * }}} */ #include "common.h" +#include "defs.h" #include "pawn_sensor.h" #include "asserv.h" @@ -33,6 +34,12 @@ #include "bot.h" #include "playground.h" #include "codebar.h" +#include "mimot.h" +#include "main.h" + +#define FSM_NAME AI +#include "fsm.h" +#include "fsm_queue.h" #include "modules/utils/utils.h" #include "modules/math/geometry/distance.h" @@ -49,8 +56,18 @@ struct pawn_sensor_t vect_t active_position; }; +/** Pawn sensor general context. */ +struct pawn_sensor_general_t +{ + /** Last bumped element position. */ + vect_t last_bumped; + /** Bumper triggered, wait until the next one. */ + uint16_t bump_wait; +}; + /** Global contexts. */ struct pawn_sensor_t pawn_sensor_front, pawn_sensor_back; +struct pawn_sensor_general_t pawn_sensor_global; static uint8_t pawn_sensor_get_type (uint8_t direction) @@ -132,3 +149,63 @@ pawn_sensor_get (uint8_t direction) return 0; } +void +pawn_sensor_bumper (uint8_t bumped, uint16_t dx, uint16_t dy) +{ + uint8_t i; + if (bumped) + { + /* Compute pawn position. */ + position_t robot_pos; + asserv_get_position (&robot_pos); + vect_t bumped_pawn; + bumped_pawn.x = dx; + bumped_pawn.y = dy; + vect_rotate_uf016 (&bumped_pawn, robot_pos.a); + vect_translate (&bumped_pawn, &robot_pos.v); + /* Do not count if out of the table. */ + if (bumped_pawn.x < 400 + BOT_ELEMENT_RADIUS + && bumped_pawn.x >= PG_WIDTH - 400 - BOT_ELEMENT_RADIUS + && bumped_pawn.y < BOT_ELEMENT_RADIUS + && bumped_pawn.y >= PG_WIDTH - BOT_ELEMENT_RADIUS) + return; + /* Do not count the opponent as a pawn. */ + for (i = 0; i < main_obstacles_nb; i++) + { + uint16_t dist = distance_point_point (&bumped_pawn, + &main_obstacles_pos[i]); + if (dist < 300 + BOT_ELEMENT_RADIUS) + return; + } + /* OK, take it. */ + pawn_sensor_global.last_bumped = bumped_pawn; + fsm_queue_post_event (FSM_EVENT (AI, top_bumper)); + pawn_sensor_global.bump_wait = 3 * 250; + } +} + +void +pawn_sensor_update (void) +{ +#define BUMPER_FRONT_LEFT _BV (6) +#define BUMPER_FRONT_RIGHT _BV (7) +#define BUMPER_BACK_RIGHT _BV (5) +#define BUMPER_BACK_LEFT _BV (4) + if (pawn_sensor_global.bump_wait) + pawn_sensor_global.bump_wait--; + else + { + uint8_t bumpers = mimot_get_input (); + pawn_sensor_bumper (!(bumpers & BUMPER_FRONT_LEFT), 120, 265); + pawn_sensor_bumper (!(bumpers & BUMPER_FRONT_RIGHT), 120, -265); + pawn_sensor_bumper (!(bumpers & BUMPER_BACK_RIGHT), -120, -265); + pawn_sensor_bumper (!(bumpers & BUMPER_BACK_LEFT), -120, 265); + } +} + +vect_t +pawn_sensor_get_last_bumped (void) +{ + return pawn_sensor_global.last_bumped; +} + diff --git a/digital/io-hub/src/robospierre/pawn_sensor.h b/digital/io-hub/src/robospierre/pawn_sensor.h index 14ce2d62..04ea1a2f 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.h +++ b/digital/io-hub/src/robospierre/pawn_sensor.h @@ -30,4 +30,12 @@ uint8_t pawn_sensor_get (uint8_t direction); +/** Update pawn sensors. */ +void +pawn_sensor_update (void); + +/** Return last bumped pawn. */ +vect_t +pawn_sensor_get_last_bumped (void); + #endif /* pawn_sensor_h */ diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 9cdefc3e..52bb05ae 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -33,6 +33,7 @@ #include "logistic.h" #include "move.h" #include "chrono.h" +#include "pawn_sensor.h" /* * Here is the top FSM. This FSM is suppose to give life to the robot with an @@ -70,6 +71,10 @@ FSM_STATES ( /* Dropping, clearing so that doors can be closed. */ TOP_DROP_CLEARING) +FSM_EVENTS ( + /* Bumpers have seen something. */ + top_bumper) + FSM_START_WITH (TOP_START) /** Top context. */ @@ -127,6 +132,15 @@ top_prepare_level (void) return 1; } +static void +top_go_this_element (vect_t pos, int16_t shorten) +{ + ctx.go_to_element_direction = logistic_global.collect_direction; + uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD + ? 0 : ASSERV_BACKWARD; + move_start_noangle (pos, backward, shorten); +} + static uint8_t top_go_element (void) { @@ -142,10 +156,7 @@ top_go_element (void) logistic_global.prepare = top_prepare_level (); } vect_t element_pos = element_get_pos (ctx.target_element_id); - ctx.go_to_element_direction = logistic_global.collect_direction; - uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD - ? 0 : ASSERV_BACKWARD; - move_start_noangle (element_pos, backward, 0); + top_go_this_element (element_pos, 0); return 1; } @@ -312,6 +323,15 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, clamp_working, TOP_WAITING_CLAMP) return FSM_NEXT (TOP_GOING_TO_ELEMENT, clamp_working); } +FSM_TRANS (TOP_GOING_TO_ELEMENT, top_bumper, TOP_GOING_TO_ELEMENT) +{ + if (!ctx.broken) + logistic_global.prepare = top_prepare_level (); + move_stop (); + top_go_this_element (pawn_sensor_get_last_bumped (), BOT_ELEMENT_RADIUS - 50); + return FSM_NEXT (TOP_GOING_TO_ELEMENT, top_bumper); +} + FSM_TRANS (TOP_WAITING_CLAMP, clamp_done, drop, TOP_GOING_TO_DROP, element, TOP_GOING_TO_ELEMENT) diff --git a/digital/mimot/src/dirty/simu.host.c b/digital/mimot/src/dirty/simu.host.c index 8afd73ca..d27ffe5f 100644 --- a/digital/mimot/src/dirty/simu.host.c +++ b/digital/mimot/src/dirty/simu.host.c @@ -156,7 +156,7 @@ simu_sensor_update_marcel (void) void simu_sensor_update_robospierre (void) { - PINC = 0; + PINC = 0xf0; if (simu_aux_model[0].th < 120.0 * 5.0 / 6.0 * simu_aux_model[0].m.i_G) PINC |= IO_BV (CONTACT_AUX0_ZERO_IO); } -- cgit v1.2.3 From 09193b811d682c035fca4a15caf0728c1d36a9c1 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 08:35:10 +0200 Subject: digital/ai/src/fsm: add proto debug --- digital/ai/src/fsm/fsm.host.c | 3 +++ 1 file changed, 3 insertions(+) (limited to 'digital/ai') diff --git a/digital/ai/src/fsm/fsm.host.c b/digital/ai/src/fsm/fsm.host.c index ed93221d..50336722 100644 --- a/digital/ai/src/fsm/fsm.host.c +++ b/digital/ai/src/fsm/fsm.host.c @@ -1144,6 +1144,7 @@ fsm_build_gen_avr_c (fsm_build_t *fsm, uint embedded_strings) /* Introduction. */ fprintf (f, "/* This file has been generated, do not edit. */\n\n"); fprintf (f, "#include \"fsm_%s_gen.h\"\n\n", fsm->name); + fprintf (f, "#include \"modules/proto/proto.h\"\n\n"); /* Gen state strings. */ if (embedded_strings) @@ -1348,6 +1349,8 @@ fsm_build_gen_avr_c (fsm_build_t *fsm, uint embedded_strings) fsm->name, fsm->name); fprintf (f, "\t\t{\n"); + fprintf (f, "\t\t\tproto_send2b ('F', fsm_%s_active_states[i], e);\n", + fsm->name); fprintf (f, "\t\t\tfsm_%s_active_states[i] = fsm_%s_read_trans (e, " "fsm_%s_active_states[i])();\n", fsm->name, -- 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 'digital/ai') 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 From dcb79f383269440ec2c5a54b6d7792fbb0110d5a Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 5 Mar 2012 21:47:19 +0100 Subject: digital/ai/tools: fix test_simu_control scripts --- digital/ai/tools/test_simu.py | 11 ++++++----- digital/ai/tools/test_simu_control_marcel.py | 7 +++++-- digital/ai/tools/test_simu_control_robospierre.py | 7 +++++-- 3 files changed, 16 insertions(+), 9 deletions(-) (limited to 'digital/ai') diff --git a/digital/ai/tools/test_simu.py b/digital/ai/tools/test_simu.py index 50eaf265..1b804e22 100644 --- a/digital/ai/tools/test_simu.py +++ b/digital/ai/tools/test_simu.py @@ -49,7 +49,7 @@ class ObstacleWithBeacon (obstacle_view.RoundObstacle): class TestSimu (InterNode): """Interface, with simulated programs.""" - def __init__ (self, robot_class, robot_nb = 1): + def __init__ (self, robot_class, robot_nb = 1, color_switch = True): # Hub. self.hub = mex.hub.Hub (min_clients = 1 + robot_class.client_nb * robot_nb) @@ -86,10 +86,11 @@ class TestSimu (InterNode): 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) + if 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 () diff --git a/digital/ai/tools/test_simu_control_marcel.py b/digital/ai/tools/test_simu_control_marcel.py index 2a43b7bb..1d5563e0 100644 --- a/digital/ai/tools/test_simu_control_marcel.py +++ b/digital/ai/tools/test_simu_control_marcel.py @@ -28,8 +28,11 @@ import math class TestSimuControl (TestSimu): """Interface with extra control.""" - def __init__ (self, robot_class): - TestSimu.__init__ (self, robot_class) + def __init__ (self, robot_class, *args): + TestSimu.__init__ (self, robot_class, *args, color_switch = False) + self.io = self.robots[0].io + self.asserv = self.robots[0].asserv + self.mimot = self.robots[0].mimot def create_widgets (self): TestSimu.create_widgets (self) diff --git a/digital/ai/tools/test_simu_control_robospierre.py b/digital/ai/tools/test_simu_control_robospierre.py index 1795e17f..c1a16c3c 100644 --- a/digital/ai/tools/test_simu_control_robospierre.py +++ b/digital/ai/tools/test_simu_control_robospierre.py @@ -32,8 +32,11 @@ class TestSimuControl (TestSimu): ROTATION_STROKE = 0x233e - def __init__ (self, robot_class): - TestSimu.__init__ (self, robot_class) + def __init__ (self, robot_class, *args): + TestSimu.__init__ (self, robot_class, *args, color_switch = False) + self.io = self.robots[0].io + self.asserv = self.robots[0].asserv + self.mimot = self.robots[0].mimot def create_widgets (self): TestSimu.create_widgets (self) -- cgit v1.2.3