summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--digital/io-hub/src/guybrush/Makefile39
-rw-r--r--digital/io-hub/src/guybrush/README24
-rw-r--r--digital/io-hub/src/guybrush/avrconfig.h135
-rw-r--r--digital/io-hub/src/guybrush/bot.h58
-rw-r--r--digital/io-hub/src/guybrush/contact_defs.h35
-rw-r--r--digital/io-hub/src/guybrush/init_defs.h50
-rw-r--r--digital/io-hub/src/guybrush/main.c286
-rw-r--r--digital/io-hub/src/guybrush/main.h31
-rw-r--r--digital/io-hub/src/guybrush/move.c536
-rw-r--r--digital/io-hub/src/guybrush/move.h63
-rw-r--r--digital/io-hub/src/guybrush/path.c380
-rw-r--r--digital/io-hub/src/guybrush/path.h78
-rw-r--r--digital/io-hub/src/guybrush/playground_2012.h32
-rw-r--r--digital/io-hub/src/guybrush/radar_defs.c46
-rw-r--r--digital/io-hub/src/guybrush/radar_defs.h41
-rw-r--r--digital/io-hub/src/guybrush/simu.host.c107
-rw-r--r--digital/io-hub/src/guybrush/simu.host.h46
-rw-r--r--digital/io-hub/src/guybrush/top.c59
18 files changed, 2046 insertions, 0 deletions
diff --git a/digital/io-hub/src/guybrush/Makefile b/digital/io-hub/src/guybrush/Makefile
new file mode 100644
index 00000000..535fba17
--- /dev/null
+++ b/digital/io-hub/src/guybrush/Makefile
@@ -0,0 +1,39 @@
+# Base directory of AVR.
+BASE = ../../../avr
+# Name of the program to build.
+PROGS = io_hub
+# Sources to compile.
+io_hub_SOURCES = main.c top.c \
+ radar_defs.c radar.c path.c move.c \
+ init.c fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \
+ contact.avr.c contact.host.c \
+ twi_master.c asserv.c mimot.c \
+ chrono.c timer.avr.c simu.host.c
+# Modules needed for IO.
+MODULES = proto uart twi utils \
+ adc devices/usdist \
+ math/fixed math/geometry path/astar
+AI_MODULES = twi_master common utils fsm move
+# Configuration file.
+CONFIGFILE = avrconfig.h
+AVR_MCU = at90usb1287
+# Optimize for speed.
+OPTIMIZE = -O2
+HOST_LIBS = -lm
+
+vpath %.c ../common
+vpath %.h ../common
+vpath %.c $(AI_MODULES:%=../../../ai/src/%)
+vpath %.h $(AI_MODULES:%=../../../ai/src/%)
+INCLUDES += -I. -I../common $(AI_MODULES:%=-I../../../ai/src/%)
+
+EXTRA_CLEAN_FILES = fsm_AI_gen.h fsm_AI_gen.avr.c
+
+include $(BASE)/make/Makefile.gen
+
+# FSM generation.
+obj/main.avr.o: fsm_AI_gen.h
+fsm_AI_gen.avr.c: fsm_AI_gen.h
+fsm_AI_gen.h: io_hub.host
+ ./$< --gen
+ mv fsm_AI_gen.c fsm_AI_gen.avr.c
diff --git a/digital/io-hub/src/guybrush/README b/digital/io-hub/src/guybrush/README
new file mode 100644
index 00000000..d03aff14
--- /dev/null
+++ b/digital/io-hub/src/guybrush/README
@@ -0,0 +1,24 @@
+guybrush - Eurobot 2012 AI.
+
+AI & IO for the APBTeam Robot Guybrush.
+
+
+Copyright (C) 2012 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.
diff --git a/digital/io-hub/src/guybrush/avrconfig.h b/digital/io-hub/src/guybrush/avrconfig.h
new file mode 100644
index 00000000..ab581210
--- /dev/null
+++ b/digital/io-hub/src/guybrush/avrconfig.h
@@ -0,0 +1,135 @@
+#ifndef avrconfig_h
+#define avrconfig_h
+/* avrconfig.h */
+/* guybrush - Eurobot 2012 AI. {{{
+ *
+ * Copyright (C) 2012 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.
+ *
+ * }}} */
+
+/* global */
+/** AVR Frequency : 1000000, 1843200, 2000000, 3686400, 4000000, 7372800,
+ * 8000000, 11059200, 14745600, 16000000, 18432000, 20000000. */
+#define AC_FREQ 16000000
+
+/* uart - UART module. */
+/** Select hardware uart for primary uart: 0, 1 or -1 to disable. */
+#define AC_UART0_PORT 1
+/** Baudrate: 2400, 4800, 9600, 14400, 19200, 28800, 38400, 57600, 76800,
+ * 115200, 230400, 250000, 500000, 1000000. */
+#define AC_UART0_BAUDRATE 38400
+/** Send mode:
+ * - POLLING: no interrupts.
+ * - RING: interrupts, ring buffer. */
+#define AC_UART0_SEND_MODE RING
+/** Recv mode, same as send mode. */
+#define AC_UART0_RECV_MODE RING
+/** Character size: 5, 6, 7, 8, 9 (only 8 implemented). */
+#define AC_UART0_CHAR_SIZE 8
+/** Parity : ODD, EVEN, NONE. */
+#define AC_UART0_PARITY EVEN
+/** Stop bits : 1, 2. */
+#define AC_UART0_STOP_BITS 1
+/** Send buffer size, should be power of 2 for RING mode. */
+#define AC_UART0_SEND_BUFFER_SIZE 32
+/** Recv buffer size, should be power of 2 for RING mode. */
+#define AC_UART0_RECV_BUFFER_SIZE 32
+/** If the send buffer is full when putc:
+ * - DROP: drop the new byte.
+ * - WAIT: wait until there is room in the send buffer. */
+#define AC_UART0_SEND_BUFFER_FULL WAIT
+/** In HOST compilation:
+ * - STDIO: use stdin/out.
+ * - PTS: use pseudo terminal. */
+#define AC_UART0_HOST_DRIVER STDIO
+/** Same thing for secondary port. */
+#define AC_UART1_PORT -1
+#define AC_UART1_BAUDRATE 115200
+#define AC_UART1_SEND_MODE RING
+#define AC_UART1_RECV_MODE RING
+#define AC_UART1_CHAR_SIZE 8
+#define AC_UART1_PARITY EVEN
+#define AC_UART1_STOP_BITS 1
+#define AC_UART1_SEND_BUFFER_SIZE 32
+#define AC_UART1_RECV_BUFFER_SIZE 32
+#define AC_UART1_SEND_BUFFER_FULL WAIT
+#define AC_UART1_HOST_DRIVER PTS
+
+/* proto - Protocol module. */
+/** Maximum argument size. */
+#define AC_PROTO_ARGS_MAX_SIZE 12
+/** Callback function name. */
+#define AC_PROTO_CALLBACK proto_callback
+/** Putchar function name. */
+#define AC_PROTO_PUTC uart0_putc
+/** Support for quote parameter. */
+#define AC_PROTO_QUOTE 1
+
+/* twi - TWI module. */
+/** Driver to implement TWI: HARD, SOFT, or USI. */
+#define AC_TWI_DRIVER HARD
+/** Do not use interrupts. */
+#define AC_TWI_NO_INTERRUPT 0
+/** TWI frequency, should really be 100 kHz. */
+#define AC_TWI_FREQ 100000
+/** Enable slave part. */
+#define AC_TWI_SLAVE_ENABLE 0
+/** Enable master part. */
+#define AC_TWI_MASTER_ENABLE 1
+/** Master transfer completion callback, optionally defined by the user, called
+ * at end of master transfer. */
+#undef AC_TWI_MASTER_DONE
+/** Use internal pull up. */
+#define AC_TWI_PULL_UP 0
+
+/* usdist - Analog US distance sensor. */
+/** Number of sensors. */
+#define AC_USDIST_NB 4
+/** Measuring period, in number of update call. */
+#define AC_USDIST_PERIOD 1
+/** List of space separated sensor definition, see usdist.h. */
+#define AC_USDIST_SENSORS \
+ USDIST_SENSOR (0, A, 0) \
+ USDIST_SENSOR (1, A, 1) \
+ USDIST_SENSOR (2, A, 2) \
+ USDIST_SENSOR (3, A, 3)
+
+/* path - Path finding module. */
+/** Report path found for debug. */
+#define AC_PATH_REPORT defined (HOST)
+/** Report function name. */
+#define AC_PATH_REPORT_CALLBACK simu_send_path
+/** Number of possible obstacles. */
+#define AC_PATH_OBSTACLES_NB 2
+
+/* astar - A* path finding module. */
+/** Neighbor callback. */
+#define AC_ASTAR_NEIGHBOR_CALLBACK path_astar_neighbor_callback
+/** Heuristic callback. */
+#define AC_ASTAR_HEURISTIC_CALLBACK path_astar_heuristic_callback
+
+/* io-hub - io/ai board. */
+/** TWI address of the io board. */
+#define AC_IO_TWI_ADDRESS 10
+/** PWM setting. */
+#undef AC_IOHUB_PWM
+
+#endif /* avrconfig_h */
diff --git a/digital/io-hub/src/guybrush/bot.h b/digital/io-hub/src/guybrush/bot.h
new file mode 100644
index 00000000..65b4cb09
--- /dev/null
+++ b/digital/io-hub/src/guybrush/bot.h
@@ -0,0 +1,58 @@
+#ifndef bot_h
+#define bot_h
+/* bot.h */
+/* guybrush - Eurobot 2012 AI. {{{
+ *
+ * Copyright (C) 2012 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.
+ *
+ * }}} */
+
+/** Robot specific defines. */
+
+/** Scaling factor, millimeter per step. */
+#ifdef HOST
+# define BOT_SCALE 0.0395840674352314
+#else
+# define BOT_SCALE 0.0317975134344
+#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
+
+/** 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. */
+#ifdef HOST
+# define BOT_SPEED_INIT 0x20, 0x20, 0x20, 0x20
+#else
+# define BOT_SPEED_INIT 0x10, 0x10, 0x10, 0x10
+#endif
+/** Normal cruise speed. */
+#define BOT_SPEED_NORMAL 0x50, 0x60, 0x20, 0x20
+
+#endif /* bot_h */
diff --git a/digital/io-hub/src/guybrush/contact_defs.h b/digital/io-hub/src/guybrush/contact_defs.h
new file mode 100644
index 00000000..55773d78
--- /dev/null
+++ b/digital/io-hub/src/guybrush/contact_defs.h
@@ -0,0 +1,35 @@
+#ifndef contact_defs_h
+#define contact_defs_h
+/* contact_defs.h */
+/* guybrush - Eurobot 2012 AI. {{{
+ *
+ * Copyright (C) 2012 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 CONTACT_COLOR E, 3
+#define CONTACT_JACK E, 5
+#define CONTACT_STRAT E, 1
+
+#define CONTACT_LIST \
+ CONTACT (CONTACT_STRAT)
+
+#endif /* contact_defs_h */
diff --git a/digital/io-hub/src/guybrush/init_defs.h b/digital/io-hub/src/guybrush/init_defs.h
new file mode 100644
index 00000000..128d694e
--- /dev/null
+++ b/digital/io-hub/src/guybrush/init_defs.h
@@ -0,0 +1,50 @@
+#ifndef init_defs_h
+#define init_defs_h
+/* init_defs.h */
+/* guybrush - Eurobot 2012 AI. {{{
+ *
+ * Copyright (C) 2012 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_2012.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/guybrush/main.c b/digital/io-hub/src/guybrush/main.c
new file mode 100644
index 00000000..92f9c33b
--- /dev/null
+++ b/digital/io-hub/src/guybrush/main.c
@@ -0,0 +1,286 @@
+/* main.c */
+/* guybrush - Eurobot 2012 AI. {{{
+ *
+ * Copyright (C) 2012 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 "modules/utils/utils.h"
+#include "modules/uart/uart.h"
+#include "modules/proto/proto.h"
+
+#include "modules/devices/usdist/usdist.h"
+
+#include "timer.h"
+#include "chrono.h"
+#include "simu.host.h"
+
+#include "asserv.h"
+#include "mimot.h"
+#include "twi_master.h"
+
+#include "contact.h"
+#include "radar.h"
+
+#define FSM_NAME AI
+#include "fsm.h"
+#ifdef HOST
+# include <string.h>
+#endif
+#include "fsm_queue.h"
+
+#include "path.h"
+#include "move.h"
+
+#include "bot.h"
+
+#include "io.h"
+
+#ifndef HOST
+# include <avr/wdt.h>
+#endif
+
+/** 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_;
+
+/** Contact stats counters. */
+static uint8_t main_stats_contact_, main_stats_contact_cpt_;
+
+/** US sensors stats counters. */
+static uint8_t main_stats_usdist_, main_stats_usdist_cpt_;
+
+/** Main initialisation. */
+static void
+main_init (void)
+{
+#ifndef HOST
+ /* Disable watchdog (enabled in bootloader). */
+ MCUSR &= ~(1 << WDRF);
+ wdt_disable ();
+#endif
+ /* Serial port */
+ uart0_init ();
+ /* Enable interrupts */
+ sei ();
+ /* Main timer */
+ timer_init ();
+ timer_wait ();
+ /* TWI communications */
+ asserv_init ();
+ mimot_init ();
+ twi_master_init ();
+ /* IO modules. */
+ contact_init ();
+ usdist_init ();
+ /* AI modules. */
+ path_init ();
+ /* Initialization done. */
+ proto_send0 ('z');
+}
+
+/** Main events management. */
+void
+main_event_to_fsm (void)
+{
+ /* If an event is handled, stop generating any other event, because a
+ * transition may have invalidated the current robot state. */
+#define FSM_HANDLE_E(fsm, event) \
+ do { if (FSM_HANDLE (fsm, event)) return; } while (0)
+#define FSM_HANDLE_VAR_E(fsm, event) \
+ do { if (FSM_HANDLE_VAR (fsm, event)) return; } while (0)
+#define FSM_HANDLE_TIMEOUT_E(fsm) \
+ do { if (FSM_HANDLE_TIMEOUT (fsm)) return; } while (0)
+ /* Update FSM timeouts. */
+ FSM_HANDLE_TIMEOUT_E (AI);
+ /* Motor 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);
+ /* Jack. */
+ if (!contact_get_jack ())
+ FSM_HANDLE_E (AI, jack_inserted);
+ else
+ FSM_HANDLE_E (AI, jack_removed);
+ /* Events from the event queue. */
+ if (fsm_queue_poll ())
+ {
+ /* We must post the event at the end of this block because if it is
+ * handled, the function will return and every instruction after will
+ * never be executed. */
+ uint8_t save_event = fsm_queue_pop_event ();
+ /* Post the event */
+ FSM_HANDLE_VAR_E (AI, save_event);
+ }
+ /* Check obstables. */
+ if (move_check_obstacles ())
+ return;
+}
+
+/** Main (and infinite) loop. */
+static void
+main_loop (void)
+{
+ while (1)
+ {
+ /* Wait until next cycle. */
+ timer_wait ();
+ /* Update chrono. */
+ chrono_update ();
+ /* Is match over? */
+ if (chrono_is_match_over ())
+ {
+ /* End it and block here indefinitely. */
+ chrono_end_match (42);
+ return;
+ }
+ /* Handle commands from UART. */
+ while (uart0_poll ())
+ proto_accept (uart0_getc ());
+ /* Update IO modules. */
+ contact_update ();
+ if (usdist_update ())
+ {
+ 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. */
+ path_decay ();
+ /* Only manage events if slaves are synchronised. */
+ if (twi_master_sync ())
+ main_event_to_fsm ();
+ /* Send stats if requested. */
+ if (main_stats_asserv_ && !--main_stats_asserv_cpt_)
+ {
+ /* Get current position */
+ position_t cur_pos;
+ asserv_get_position (&cur_pos);
+ /* Send stats */
+ proto_send3w ('A', cur_pos.v.x, cur_pos.v.y, cur_pos.a);
+ /* Reset stats counter */
+ main_stats_asserv_cpt_ = main_stats_asserv_;
+ }
+ if (main_stats_contact_ && !--main_stats_contact_cpt_)
+ {
+ proto_send1d ('P', contact_all () | (uint32_t) mimot_get_input () << 24);
+ main_stats_contact_cpt_ = main_stats_contact_;
+ }
+ if (main_stats_usdist_ && !--main_stats_usdist_cpt_)
+ {
+ proto_send4w ('U', usdist_mm[0], usdist_mm[1], usdist_mm[2],
+ usdist_mm[3]);
+ main_stats_usdist_cpt_ = main_stats_usdist_;
+ }
+ }
+}
+
+/** Handle received commands. */
+void
+proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
+{
+#define c(cmd, size) (cmd << 8 | size)
+ switch (c (cmd, size))
+ {
+ case c ('z', 0):
+ /* Reset */
+ utils_reset ();
+ break;
+ case c ('j', 0):
+ /* Simulate jack insertion. */
+ fsm_queue_post_event (FSM_EVENT (AI, jack_inserted));
+ 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 ('w', 0):
+ /* Disable all motor control. */
+ mimot_motor0_free ();
+ mimot_motor1_free ();
+ asserv_free_motor ();
+ break;
+ /* Stats commands.
+ * - b: interval between stats. */
+ case c ('A', 1):
+ /* Position stats. */
+ main_stats_asserv_ = main_stats_asserv_cpt_ = args[0];
+ break;
+ case c ('P', 1):
+ /* Contact stats. */
+ main_stats_contact_ = main_stats_contact_cpt_ = args[0];
+ break;
+ case c ('U', 1):
+ /* US sensors stats. */
+ main_stats_usdist_ = main_stats_usdist_cpt_ = args[0];
+ break;
+ default:
+ /* Unknown commands */
+ proto_send0 ('?');
+ return;
+ }
+ /* When no error, acknowledge commands */
+ proto_send (cmd, size, args);
+#undef c
+}
+
+int
+main (int argc, char **argv)
+{
+#ifdef HOST
+ /* Produce AVR's FSM headers. */
+ int i;
+ if (argc > 1)
+ for (i = 1; i < argc; i++)
+ if (strcmp (argv[i], "--gen") == 0)
+ {
+ FSM_GENERATE (AVR, 0);
+ return 0;
+ }
+#endif
+ avr_init (argc, argv);
+ main_init ();
+ main_loop ();
+ return 0;
+}
diff --git a/digital/io-hub/src/guybrush/main.h b/digital/io-hub/src/guybrush/main.h
new file mode 100644
index 00000000..617096ff
--- /dev/null
+++ b/digital/io-hub/src/guybrush/main.h
@@ -0,0 +1,31 @@
+#ifndef main_h
+#define main_h
+/* main.h */
+/* guybrush - Eurobot 2012 AI. {{{
+ *
+ * Copyright (C) 2012 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/guybrush/move.c b/digital/io-hub/src/guybrush/move.c
new file mode 100644
index 00000000..3c4c7f46
--- /dev/null
+++ b/digital/io-hub/src/guybrush/move.c
@@ -0,0 +1,536 @@
+/* move.c */
+/* guybrush - Eurobot 2012 AI. {{{
+ *
+ * Copyright (C) 2012 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_queue_post_event (FSM_EVENT (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_queue_post_event (FSM_EVENT (AI, move_start));
+}
+
+void
+move_stop (void)
+{
+ /* Stop the FSM. */
+ fsm_queue_post_event (FSM_EVENT (AI, move_stop));
+}
+
+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 (
+ /* 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,
+ /* Stop movement. */
+ move_stop,
+ /* 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;
+ asserv_get_position (&robot_position);
+ uint16_t robot_angle = robot_position.a;
+ if (backward & ASSERV_BACKWARD)
+ robot_angle += 0x8000;
+ /* 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. */
+ 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;
+ int16_t diff_angle = dst_angle - robot_angle;
+ if ((backward & ASSERV_REVERT_OK)
+ && (diff_angle > 0x4000 || diff_angle < -0x4000))
+ dst_angle += 0x8000;
+ int16_t diff = dst_angle - robot_angle;
+ /* 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_ROTATING, move_stop, MOVE_IDLE)
+{
+ asserv_stop_motor ();
+ return FSM_NEXT (MOVE_ROTATING, move_stop);
+}
+
+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, move_stop, MOVE_IDLE)
+{
+ asserv_stop_motor ();
+ return FSM_NEXT (MOVE_MOVING, move_stop);
+}
+
+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 (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, move_stop, MOVE_IDLE)
+{
+ asserv_stop_motor ();
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, move_stop);
+}
+
+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);
+ }
+}
+
+FSM_TRANS (MOVE_WAIT_FOR_CLEAR_PATH, move_stop, MOVE_IDLE)
+{
+ return FSM_NEXT (MOVE_WAIT_FOR_CLEAR_PATH, move_stop);
+}
+
diff --git a/digital/io-hub/src/guybrush/move.h b/digital/io-hub/src/guybrush/move.h
new file mode 100644
index 00000000..dcb7cb88
--- /dev/null
+++ b/digital/io-hub/src/guybrush/move.h
@@ -0,0 +1,63 @@
+#ifndef move_h
+#define move_h
+/* move.h */
+/* guybrush - Eurobot 2012 AI. {{{
+ *
+ * Copyright (C) 2012 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);
+
+/** Stop movement. */
+void
+move_stop (void);
+
+/** 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/src/guybrush/path.c b/digital/io-hub/src/guybrush/path.c
new file mode 100644
index 00000000..bd9668c5
--- /dev/null
+++ b/digital/io-hub/src/guybrush/path.c
@@ -0,0 +1,380 @@
+/* path.c */
+/* guybrush - Eurobot 2012 AI. {{{
+ *
+ * Copyright (C) 2012 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 "defs.h"
+#include "path.h"
+#include "bot.h"
+#include "playground_2012.h"
+
+#include "modules/path/astar/astar.h"
+#include "modules/utils/utils.h"
+#include "modules/math/geometry/distance.h"
+
+#define PATH_DEBUG 0
+
+#if PATH_DEBUG
+#include "debug.host.h"
+#endif
+
+/**
+ * This year, due to the large number of obstacles, a grid like structure is
+ * used for path finding on the playground. The A* algorithm is used to find
+ * path along nodes.
+ *
+ * TODO: this is a placeholder for the future algorithm.
+ */
+
+/** Number of possible obstacles. */
+#define PATH_OBSTACLES_NB AC_PATH_OBSTACLES_NB
+
+/** Number of fixed nodes. */
+#define PATH_FIXED_NODES_NB (1)
+
+/** Number of nodes in search graph, last two nodes are destination and source
+ * nodes. */
+#define PATH_NODES_NB (PATH_FIXED_NODES_NB + 2)
+
+/** Index of destination node. */
+#define PATH_DST_NODE_INDEX PATH_FIXED_NODES_NB
+
+/** Index of source node. */
+#define PATH_SRC_NODE_INDEX (PATH_DST_NODE_INDEX + 1)
+
+/** Information on a node. */
+struct path_node_t
+{
+ /** Whether this node can be used. */
+ uint8_t usable;
+};
+
+/** Context. */
+struct path_t
+{
+ /** List of obstacles. */
+ struct path_obstacle_t obstacles[PATH_OBSTACLES_NB];
+ /** Escape factor, 0 if none. */
+ uint8_t escape_factor;
+ /** List of nodes used for A*. */
+ struct astar_node_t astar_nodes[PATH_NODES_NB];
+ /** Cache of whether a node is blocked. */
+ uint8_t valid[PATH_FIXED_NODES_NB];
+ /** Position of end points. */
+ vect_t endpoints[2];
+ /** Whether the last update was a success. */
+ uint8_t found;
+ /** Which node to look at for next step. */
+ uint8_t get;
+};
+static struct path_t path;
+
+/** Static information on nodes. */
+static const struct path_node_t path_nodes[PATH_FIXED_NODES_NB] = {
+ /* {{{ */
+ { 1 },
+ /* }}} */
+};
+
+/** Compute position of a node. */
+static void
+path_pos (uint8_t node, vect_t *pos)
+{
+ assert (node < PATH_NODES_NB);
+ if (node < PATH_FIXED_NODES_NB)
+ {
+ /* Temporary nonsense node. */
+ pos->x = PG_WIDTH / 2;
+ pos->y = PG_LENGTH / 2;
+ }
+ else
+ {
+ *pos = path.endpoints[node - PATH_FIXED_NODES_NB];
+ }
+}
+
+/** Return 1 if the direct path between a and b nodes is blocked, also compute
+ * distance. */
+static uint8_t
+path_blocking (uint8_t a, uint8_t b, int16_t *dp)
+{
+ uint8_t i;
+ vect_t va;
+ vect_t vb;
+ uint8_t escape_factor = 0;
+ uint8_t factor = 1;
+ uint8_t blocking = 0;
+ if (a == PATH_SRC_NODE_INDEX || b == PATH_SRC_NODE_INDEX)
+ escape_factor = path.escape_factor;
+ path_pos (a, &va);
+ path_pos (b, &vb);
+ /* Test for a blocking obstacle. */
+ for (i = 0; i < PATH_OBSTACLES_NB && !blocking; i++)
+ {
+ if (path.obstacles[i].valid)
+ {
+ uint16_t d = distance_segment_point (&va, &vb,
+ &path.obstacles[i].c);
+ if (d < path.obstacles[i].r)
+ blocking = 1;
+ }
+ }
+ /* Compute distance. */
+ int16_t d = distance_point_point (&va, &vb);
+ if (d == 0)
+ {
+ *dp = 0;
+ return 0;
+ }
+ /* Handle escaping. */
+ /* TODO: do not escape through a totem! */
+ if (blocking)
+ {
+ if (escape_factor)
+ {
+ *dp = d * escape_factor;
+ return 0;
+ }
+ else
+ return 1;
+ }
+ /* No blocking. */
+ *dp = d * factor;
+ return 0;
+}
+
+/** Update the cache of blocked nodes. */
+static void
+path_blocked_update (void)
+{
+ uint8_t i, j;
+ for (i = 0; i < PATH_FIXED_NODES_NB; i++)
+ {
+ uint8_t valid = 1;
+ /* First, gather information from tables. */
+ if (!path_nodes[i].usable)
+ valid = 0;
+ else
+ {
+ vect_t pos;
+ path_pos (i, &pos);
+ /* Then, test for obstacles. */
+ for (j = 0; j < PATH_OBSTACLES_NB; j++)
+ {
+ if (path.obstacles[j].valid)
+ {
+ vect_t v = pos; vect_sub (&v, &path.obstacles[j].c);
+ uint32_t dsq = vect_dot_product (&v, &v);
+ uint32_t r = path.obstacles[j].r;
+ if (dsq <= r * r)
+ {
+ valid = 0;
+ break;
+ }
+ }
+ }
+ }
+ /* Update cache. */
+ path.valid[i] = valid;
+ }
+}
+
+void
+path_init (void)
+{
+}
+
+void
+path_endpoints (vect_t s, vect_t d)
+{
+ path.endpoints[0] = d;
+ path.endpoints[1] = s;
+}
+
+void
+path_escape (uint8_t factor)
+{
+ path.escape_factor = factor;
+}
+
+void
+path_obstacle (uint8_t i, vect_t c, uint16_t r, uint8_t factor,
+ uint16_t valid)
+{
+ assert (i < AC_PATH_OBSTACLES_NB);
+ assert (factor == 0);
+ path.obstacles[i].c = c;
+ path.obstacles[i].r = r;
+ path.obstacles[i].valid = valid;
+}
+
+void
+path_decay (void)
+{
+ uint8_t i;
+ for (i = 0; i < PATH_OBSTACLES_NB; i++)
+ {
+ if (path.obstacles[i].valid
+ && path.obstacles[i].valid != PATH_OBSTACLE_VALID_ALWAYS)
+ path.obstacles[i].valid--;
+ }
+}
+
+void
+path_update (void)
+{
+ path_blocked_update ();
+ path.found = astar (path.astar_nodes, PATH_NODES_NB, PATH_DST_NODE_INDEX,
+ PATH_SRC_NODE_INDEX);
+ path.get = PATH_SRC_NODE_INDEX;
+#if AC_PATH_REPORT
+ if (path.found)
+ {
+ uint8_t n, len = 0;
+ vect_t points[PATH_NODES_NB];
+ for (n = path.get; n != PATH_DST_NODE_INDEX; n = path.astar_nodes[n].prev)
+ path_pos (n, &points[len++]);
+ path_pos (n, &points[len++]);
+ AC_PATH_REPORT_CALLBACK (points, len, path.obstacles,
+ PATH_OBSTACLES_NB);
+ }
+#endif
+}
+
+uint8_t
+path_get_next (vect_t *p)
+{
+ if (path.found)
+ {
+ assert (path.get != PATH_DST_NODE_INDEX);
+ uint8_t prev = path.get;
+ vect_t pp;
+ path_pos (prev, &pp);
+ uint8_t next = path.astar_nodes[path.get].prev;
+ path.get = next;
+ path_pos (next, p);
+ while (next != 0xff)
+ {
+ /* Try to remove useless points. */
+ uint8_t next = path.astar_nodes[path.get].prev;
+ if (next == 0xff || next == PATH_DST_NODE_INDEX)
+ break;
+ vect_t np;
+ path_pos (next, &np);
+ vect_t vnp = np; vect_sub (&vnp, &pp);
+ vect_t vp = *p; vect_sub (&vp, &pp);
+ if (vect_normal_dot_product (&vp, &vnp) == 0)
+ {
+ path.get = next;
+ *p = np;
+ }
+ else
+ break;
+ }
+ return 1;
+ }
+ else
+ return 0;
+}
+
+/** Neighbors callback for fixed nodes. */
+static uint8_t
+path_astar_neighbor_callback_fixed (uint8_t node,
+ struct astar_neighbor_t *neighbors)
+{
+ uint8_t neighbors_nb = 0;
+ int16_t d;
+ /* Check if direct path OK. */
+ if (!path_blocking (node, PATH_SRC_NODE_INDEX, &d))
+ {
+ /* Add this neighbor. */
+ neighbors[neighbors_nb].node = PATH_SRC_NODE_INDEX;
+ neighbors[neighbors_nb].weight = d + 1;
+ neighbors_nb++;
+ }
+#if PATH_DEBUG
+ for (i = 0; i < neighbors_nb; i++)
+ DPRINTF (" n %d %d\n", neighbors[i].node, neighbors[i].weight);
+#endif
+ return neighbors_nb;
+}
+
+/** Neighbors callback for endpoints. */
+static uint8_t
+path_astar_neighbor_callback_endpoints (uint8_t node,
+ struct astar_neighbor_t *neighbors)
+{
+ uint8_t neighbors_nb = 0;
+ uint8_t i;
+ assert (node == PATH_DST_NODE_INDEX);
+ /* Select neighbors in the fixed nodes. */
+ for (i = 0; i < PATH_FIXED_NODES_NB; i++)
+ {
+ /* Discard blocking nodes. */
+ if (!path.valid[i])
+ continue;
+ /* Check if there is an obstacle along the path. */
+ int16_t d;
+ if (path_blocking (PATH_DST_NODE_INDEX, i, &d))
+ continue;
+ /* Add this neighbor. */
+ neighbors[neighbors_nb].node = i;
+ neighbors[neighbors_nb].weight = d + 1;
+ neighbors_nb++;
+ }
+ /* Check if direct path OK. */
+ int16_t d;
+ if (!path_blocking (PATH_DST_NODE_INDEX, PATH_SRC_NODE_INDEX, &d))
+ {
+ /* Add this neighbor. */
+ neighbors[neighbors_nb].node = PATH_SRC_NODE_INDEX;
+ neighbors[neighbors_nb].weight = d + 1;
+ neighbors_nb++;
+ }
+#if PATH_DEBUG
+ for (i = 0; i < neighbors_nb; i++)
+ DPRINTF (" n %d %d\n", neighbors[i].node, neighbors[i].weight);
+#endif
+ return neighbors_nb;
+}
+
+uint8_t
+path_astar_neighbor_callback (uint8_t node,
+ struct astar_neighbor_t *neighbors)
+{
+#if PATH_DEBUG
+ DPRINTF ("neighbor %d\n", node);
+#endif
+ if (node < PATH_FIXED_NODES_NB)
+ return path_astar_neighbor_callback_fixed (node, neighbors);
+ else
+ return path_astar_neighbor_callback_endpoints (node, neighbors);
+}
+
+uint16_t
+path_astar_heuristic_callback (uint8_t node)
+{
+ vect_t pos;
+ path_pos (node, &pos);
+ return distance_point_point (&pos, &path.endpoints[0]);
+}
diff --git a/digital/io-hub/src/guybrush/path.h b/digital/io-hub/src/guybrush/path.h
new file mode 100644
index 00000000..35f5cd69
--- /dev/null
+++ b/digital/io-hub/src/guybrush/path.h
@@ -0,0 +1,78 @@
+#ifndef path_h
+#define path_h
+/* path.h */
+/* guybrush - Eurobot 2012 AI. {{{
+ *
+ * Copyright (C) 2012 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"
+
+/** This implement a interface similar to the path module, but adapted for the
+ * special grid of Eurobot 2012. See real path modules for interface comments. */
+
+/** Infinite validity for an obstacle. */
+#define PATH_OBSTACLE_VALID_ALWAYS 0xffff
+
+/** Obstacle. */
+struct path_obstacle_t
+{
+ /** Center. */
+ vect_t c;
+ /** Radius. */
+ uint16_t r;
+ /** Validity counter, when this is zero, the obstacle is ignored. */
+ uint16_t valid;
+};
+
+void
+path_init (void);
+
+void
+path_endpoints (vect_t s, vect_t d);
+
+void
+path_escape (uint8_t factor);
+
+void
+path_obstacle (uint8_t i, vect_t c, uint16_t r, uint8_t factor,
+ uint16_t valid);
+
+void
+path_decay (void);
+
+void
+path_update (void);
+
+uint8_t
+path_get_next (vect_t *p);
+
+#if AC_PATH_REPORT
+
+/** Report computed path. */
+void
+AC_PATH_REPORT_CALLBACK (vect_t *points, uint8_t len,
+ struct path_obstacle_t *obstacles,
+ uint8_t obstacles_nb);
+
+#endif
+
+#endif /* path_h */
diff --git a/digital/io-hub/src/guybrush/playground_2012.h b/digital/io-hub/src/guybrush/playground_2012.h
new file mode 100644
index 00000000..474b28c2
--- /dev/null
+++ b/digital/io-hub/src/guybrush/playground_2012.h
@@ -0,0 +1,32 @@
+#ifndef playground_2012_h
+#define playground_2012_h
+/* playground_2012.h */
+/* guybrush - Eurobot 2012 AI. {{{
+ *
+ * Copyright (C) 2012 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 EUROBOT 2012
+
+#include "playground.h"
+
+#endif /* playground_2012_h */
diff --git a/digital/io-hub/src/guybrush/radar_defs.c b/digital/io-hub/src/guybrush/radar_defs.c
new file mode 100644
index 00000000..8fd626d6
--- /dev/null
+++ b/digital/io-hub/src/guybrush/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_2012.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/guybrush/radar_defs.h b/digital/io-hub/src/guybrush/radar_defs.h
new file mode 100644
index 00000000..5f6a2a73
--- /dev/null
+++ b/digital/io-hub/src/guybrush/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/guybrush/simu.host.c b/digital/io-hub/src/guybrush/simu.host.c
new file mode 100644
index 00000000..6e523228
--- /dev/null
+++ b/digital/io-hub/src/guybrush/simu.host.c
@@ -0,0 +1,107 @@
+/* simu.host.c - Host simulation. */
+/* guybrush - Eurobot 2012 AI. {{{
+ *
+ * Copyright (C) 2012 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 "simu.host.h"
+
+#include "modules/utils/utils.h"
+#include "modules/host/host.h"
+#include "modules/host/mex.h"
+#include "modules/adc/adc.h"
+#include "modules/path/path.h"
+#include "io.h"
+
+/** AVR registers. */
+uint8_t PORTA, DDRA, PINA, PINE, PINF;
+
+/** Message types. */
+uint8_t simu_mex_pos_report;
+uint8_t simu_mex_path;
+
+static void
+simu_adc_handle (void *user, mex_msg_t *msg)
+{
+ uint8_t index;
+ uint16_t value;
+ mex_msg_pop (msg, "BH", &index, &value);
+ adc_values[index] = value;
+}
+
+/** Initialise simulation. */
+void
+simu_init (void)
+{
+ const char *mex_instance;
+ mex_node_connect ();
+ mex_instance = host_get_instance ("io-hub0", 0);
+ uint8_t mtype = mex_node_reservef ("%s:adc", mex_instance);
+ mex_node_register (mtype, simu_adc_handle, 0);
+ simu_mex_pos_report = mex_node_reservef ("%s:pos-report", mex_instance);
+ simu_mex_path = mex_node_reservef ("%s:path", mex_instance);
+}
+
+/** Make a simulation step. */
+void
+simu_step (void)
+{
+}
+
+void
+timer_init (void)
+{
+ simu_init ();
+}
+
+uint8_t
+timer_wait (void)
+{
+ mex_node_wait_date (mex_node_date () + 4);
+ simu_step ();
+ return 0;
+}
+
+/** Send computed path. */
+void
+simu_send_path (vect_t *points, uint8_t len,
+ struct path_obstacle_t *obstacles, uint8_t obstacles_nb)
+{
+ int i;
+ mex_msg_t *m;
+ m = mex_msg_new (simu_mex_path);
+ for (i = 0; i < len; i++)
+ mex_msg_push (m, "hh", points[i].x, points[i].y);
+ mex_node_send (m);
+}
+
+void
+simu_send_pos_report (vect_t *pos, uint8_t pos_nb, uint8_t id)
+{
+ 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/guybrush/simu.host.h b/digital/io-hub/src/guybrush/simu.host.h
new file mode 100644
index 00000000..a419e857
--- /dev/null
+++ b/digital/io-hub/src/guybrush/simu.host.h
@@ -0,0 +1,46 @@
+#ifndef simu_host_h
+#define simu_host_h
+/* simu.host.h - Host simulation. */
+/* guybrush - Eurobot 2012 AI. {{{
+ *
+ * Copyright (C) 2012 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"
+
+#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/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c
new file mode 100644
index 00000000..15c3854d
--- /dev/null
+++ b/digital/io-hub/src/guybrush/top.c
@@ -0,0 +1,59 @@
+/* top.c */
+/* guybrush - Eurobot 2012 AI. {{{
+ *
+ * Copyright (C) 2012 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 "io.h"
+
+#include "playground_2012.h"
+#include "asserv.h"
+
+#define FSM_NAME AI
+#include "fsm.h"
+
+#include "move.h"
+#include "chrono.h"
+#include "contact.h"
+
+/*
+ * Here is the top FSM. This FSM is suppose to give life to the robot with an
+ * impression of intelligence... Well...
+ */
+
+FSM_INIT
+
+FSM_STATES (
+ /* Initial state. */
+ TOP_START)
+
+FSM_START_WITH (TOP_START)
+
+/** Top context. */
+struct top_t
+{
+};
+
+/** Global context. */
+struct top_t top_global;
+#define ctx top_global
+