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 +++++++++++++++++++++-- 1 file changed, 27 insertions(+), 2 deletions(-) (limited to 'digital/ai/tools/test_simu_control_robospierre.py') 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 -- 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/tools/test_simu_control_robospierre.py') 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 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/tools/test_simu_control_robospierre.py') 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/tools/test_simu_control_robospierre.py') 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 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/tools/test_simu_control_robospierre.py') 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 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/tools/test_simu_control_robospierre.py') 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