summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2011-05-24 01:33:18 +0200
committerNicolas Schodet2011-05-24 01:33:18 +0200
commit579d7d9ad8caf4299c5c672b724d056d9e9d9797 (patch)
tree5f966b2d6741bc9d6dcf5d33baebc317b763b3bb
parent9377154ec66848759806d0fe924ded961c9f392b (diff)
digital/io-hub: add move FSM
-rw-r--r--digital/ai/tools/test_simu_control_robospierre.py9
-rw-r--r--digital/io-hub/src/robospierre/Makefile2
-rw-r--r--digital/io-hub/src/robospierre/bot.h4
-rw-r--r--digital/io-hub/src/robospierre/main.c17
-rw-r--r--digital/io-hub/src/robospierre/main.h31
-rw-r--r--digital/io-hub/src/robospierre/move.c496
-rw-r--r--digital/io-hub/src/robospierre/move.h59
-rw-r--r--digital/io-hub/tools/io_hub/io_hub.py3
8 files changed, 617 insertions, 4 deletions
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 <math.h>
+
+/** 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 (&current_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)