summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/robospierre
diff options
context:
space:
mode:
authorNicolas Schodet2011-05-08 16:15:06 +0200
committerNicolas Schodet2011-05-08 16:15:06 +0200
commit0feb542a8cf7630a5d442387727414f5251aad83 (patch)
tree02092fbede27ef619ee63466f671513a889fb2ea /digital/io-hub/src/robospierre
parent4cd9c4458eaa49c269171145f4bc2a184948eae1 (diff)
parentd385a9eacb2e8580239f101f96356f7ed216dd97 (diff)
Merge branch 'master' into efrei-robotique
Conflicts: digital/mimot/src/dirty/counter_ext.avr.c digital/mimot/tools/mimot/init.py
Diffstat (limited to 'digital/io-hub/src/robospierre')
-rw-r--r--digital/io-hub/src/robospierre/Makefile39
-rw-r--r--digital/io-hub/src/robospierre/README24
-rw-r--r--digital/io-hub/src/robospierre/avrconfig.h115
-rw-r--r--digital/io-hub/src/robospierre/bot.h76
-rw-r--r--digital/io-hub/src/robospierre/clamp.c182
-rw-r--r--digital/io-hub/src/robospierre/clamp.h57
-rw-r--r--digital/io-hub/src/robospierre/contact_defs.h37
-rw-r--r--digital/io-hub/src/robospierre/main.c225
-rw-r--r--digital/io-hub/src/robospierre/simu.host.c64
-rw-r--r--digital/io-hub/src/robospierre/simu.host.h36
10 files changed, 855 insertions, 0 deletions
diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile
new file mode 100644
index 00000000..21411631
--- /dev/null
+++ b/digital/io-hub/src/robospierre/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 \
+ clamp.c \
+ fsm.host.c fsm_AI_gen.avr.c \
+ pwm.avr.c pwm.host.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 \
+ math/fixed math/geometry
+AI_MODULES = twi_master common utils fsm
+# 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/robospierre/README b/digital/io-hub/src/robospierre/README
new file mode 100644
index 00000000..eaa65ad4
--- /dev/null
+++ b/digital/io-hub/src/robospierre/README
@@ -0,0 +1,24 @@
+robospierre - Eurobot 2011 AI.
+
+AI & IO for the APBTeam Robot Robospierre.
+
+
+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.
diff --git a/digital/io-hub/src/robospierre/avrconfig.h b/digital/io-hub/src/robospierre/avrconfig.h
new file mode 100644
index 00000000..0fc2f11d
--- /dev/null
+++ b/digital/io-hub/src/robospierre/avrconfig.h
@@ -0,0 +1,115 @@
+#ifndef avrconfig_h
+#define avrconfig_h
+/* avrconfig.h */
+/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
+ *
+ * Copyright (C) 2008 Dufour Jérémy
+ *
+ * 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
+
+/* io-hub - io/ai board. */
+/** TWI address of the io board. */
+#define AC_IO_TWI_ADDRESS 10
+/** PWM setting. */
+#define AC_IOHUB_PWM \
+ PWM (1, A, B, 5, D, 4) \
+ PWM (1, B, B, 6, D, 5) \
+ PWM (1, C, B, 7, D, 6) \
+ PWM (3, A, C, 6, C, 7) \
+ PWM (3, B, C, 5, C, 3) \
+ PWM (3, C, C, 4, C, 2)
+
+#endif /* avrconfig_h */
diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h
new file mode 100644
index 00000000..9043a62c
--- /dev/null
+++ b/digital/io-hub/src/robospierre/bot.h
@@ -0,0 +1,76 @@
+#ifndef bot_h
+#define bot_h
+/* bot.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.
+ *
+ * }}} */
+
+/** Robot specific defines. */
+
+/** Scaling factor, millimeter per step. */
+#ifdef HOST
+# define BOT_SCALE 0.0395840674352314
+#else
+# define BOT_SCALE 0.0415178942124
+#endif
+
+#ifdef HOST
+
+# define BOT_CLAMP_SLOT_FRONT_BOTTOM_ELEVATION_STEP 0
+# define BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP (0x3b0b / 2)
+# define BOT_CLAMP_SLOT_FRONT_TOP_ELEVATION_STEP 0x3b0b
+# define BOT_CLAMP_SLOT_BACK_BOTTOM_ELEVATION_STEP 0
+# define BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP (0x3b0b / 2)
+# define BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP 0x3b0b
+# define BOT_CLAMP_SLOT_SIDE_ELEVATION_STEP 0x3b0b
+# define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP (0x3b0b / 2)
+# define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP (0x3b0b / 2)
+# define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP (0x3b0b / 2)
+
+# define BOT_CLAMP_BAY_FRONT_ROTATION_STEP 0
+# define BOT_CLAMP_BAY_BACK_ROTATION_STEP 0x11c6
+# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP (0x11c6 / 2)
+
+#else /* !HOST */
+
+# define BOT_CLAMP_SLOT_FRONT_BOTTOM_ELEVATION_STEP 0
+# define BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP 0x1d83
+# define BOT_CLAMP_SLOT_FRONT_TOP_ELEVATION_STEP 0x3288
+# define BOT_CLAMP_SLOT_BACK_BOTTOM_ELEVATION_STEP 0
+# define BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP 0x1d83
+# define BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP 0x3288
+# define BOT_CLAMP_SLOT_SIDE_ELEVATION_STEP 0x3288
+# define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP 0x1d83
+# define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP 0x1d83
+# define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP 0x1d83
+
+# define BOT_CLAMP_BAY_FRONT_ROTATION_STEP 0
+# define BOT_CLAMP_BAY_BACK_ROTATION_STEP 0x10e2
+# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP 0x820
+
+#endif /* !HOST */
+
+#define BOT_CLAMP_ELEVATION_SPEED 0x60
+#define BOT_CLAMP_ROTATION_SPEED 0x30
+
+#endif /* bot_h */
diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c
new file mode 100644
index 00000000..2f1b2471
--- /dev/null
+++ b/digital/io-hub/src/robospierre/clamp.c
@@ -0,0 +1,182 @@
+/* clamp.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 "clamp.h"
+
+#define FSM_NAME AI
+#include "fsm.h"
+
+#include "mimot.h"
+#include "bot.h"
+
+FSM_INIT
+
+FSM_STATES (
+ /* Wait order. */
+ CLAMP_IDLE,
+ /* Moving to a final or intermediary position. */
+ CLAMP_ROUTING)
+
+FSM_EVENTS (
+ /* Order to move the clamp. */
+ clamp_move,
+ /* Elevation and elevation motor success. */
+ clamp_elevation_rotation_success,
+ /* Elevation motor failure. */
+ clamp_elevation_failure,
+ /* Rotation motor failure. */
+ clamp_rotation_failure)
+
+FSM_START_WITH (CLAMP_IDLE)
+
+/** Clamp context. */
+struct clamp_t
+{
+ /* Current position. */
+ uint8_t pos_current;
+ /* Requested position. */
+ uint8_t pos_request;
+};
+
+/** Global context. */
+struct clamp_t clamp_global;
+#define ctx clamp_global
+
+/** Clamp positions. */
+static const uint16_t clamp_pos[][2] = {
+ { BOT_CLAMP_SLOT_FRONT_BOTTOM_ELEVATION_STEP,
+ BOT_CLAMP_BAY_FRONT_ROTATION_STEP },
+ { BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP,
+ BOT_CLAMP_BAY_FRONT_ROTATION_STEP },
+ { BOT_CLAMP_SLOT_FRONT_TOP_ELEVATION_STEP,
+ BOT_CLAMP_BAY_FRONT_ROTATION_STEP },
+ { BOT_CLAMP_SLOT_BACK_BOTTOM_ELEVATION_STEP,
+ BOT_CLAMP_BAY_BACK_ROTATION_STEP },
+ { BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP,
+ BOT_CLAMP_BAY_BACK_ROTATION_STEP },
+ { BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP,
+ BOT_CLAMP_BAY_BACK_ROTATION_STEP },
+ { BOT_CLAMP_SLOT_SIDE_ELEVATION_STEP,
+ BOT_CLAMP_BAY_SIDE_ROTATION_STEP },
+ { BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP,
+ BOT_CLAMP_BAY_FRONT_ROTATION_STEP },
+ { BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP,
+ BOT_CLAMP_BAY_BACK_ROTATION_STEP },
+ { BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP,
+ BOT_CLAMP_BAY_SIDE_ROTATION_STEP },
+};
+
+void
+clamp_move (uint8_t pos)
+{
+ if (pos != ctx.pos_current)
+ {
+ ctx.pos_request = pos;
+ FSM_HANDLE (AI, clamp_move);
+ }
+}
+
+/** Find next position and start motors. */
+static void
+clamp_route (void)
+{
+ uint8_t pos_new;
+ uint8_t pos_current = ctx.pos_current;
+ uint8_t pos_request = ctx.pos_request;
+ /* Compute new position. */
+ if (CLAMP_IS_SLOT_IN_FRONT_BAY (pos_current))
+ {
+ if (!CLAMP_IS_SLOT_IN_FRONT_BAY (pos_request))
+ pos_new = CLAMP_BAY_FRONT_LEAVE;
+ else
+ pos_new = pos_request;
+ }
+ else if (CLAMP_IS_SLOT_IN_BACK_BAY (pos_current))
+ {
+ if (!CLAMP_IS_SLOT_IN_BACK_BAY (pos_request))
+ pos_new = CLAMP_BAY_BACK_LEAVE;
+ else
+ pos_new = pos_request;
+ }
+ else if (pos_current == CLAMP_SLOT_SIDE)
+ {
+ pos_new = CLAMP_BAY_SIDE_ENTER_LEAVE;
+ }
+ else if (pos_current == CLAMP_BAY_FRONT_LEAVE)
+ {
+ if (pos_request == CLAMP_SLOT_SIDE)
+ pos_new = CLAMP_BAY_SIDE_ENTER_LEAVE;
+ else
+ pos_new = CLAMP_SLOT_BACK_MIDDLE;
+ }
+ else if (pos_current == CLAMP_BAY_BACK_LEAVE)
+ {
+ if (pos_request == CLAMP_SLOT_SIDE)
+ pos_new = CLAMP_BAY_SIDE_ENTER_LEAVE;
+ else
+ pos_new = CLAMP_SLOT_FRONT_MIDDLE;
+ }
+ else
+ {
+ assert (pos_current == CLAMP_BAY_SIDE_ENTER_LEAVE);
+ if (pos_request == CLAMP_SLOT_SIDE)
+ pos_new = pos_request;
+ else if (CLAMP_IS_SLOT_IN_FRONT_BAY (pos_request))
+ pos_new = CLAMP_SLOT_FRONT_MIDDLE;
+ else
+ pos_new = CLAMP_SLOT_BACK_MIDDLE;
+ }
+ /* Run motors. */
+ mimot_move_motor0_absolute (clamp_pos[pos_new][0],
+ BOT_CLAMP_ELEVATION_SPEED);
+ mimot_move_motor1_absolute (clamp_pos[pos_new][1],
+ BOT_CLAMP_ROTATION_SPEED);
+ /* Remember new position. */
+ ctx.pos_current = pos_new;
+}
+
+FSM_TRANS (CLAMP_IDLE, clamp_move, CLAMP_ROUTING)
+{
+ clamp_route ();
+ return FSM_NEXT (CLAMP_IDLE, clamp_move);
+}
+
+FSM_TRANS (CLAMP_ROUTING, clamp_elevation_rotation_success,
+ done, CLAMP_IDLE,
+ next, CLAMP_ROUTING)
+{
+ if (ctx.pos_current == ctx.pos_request)
+ {
+ return FSM_NEXT (CLAMP_ROUTING, clamp_elevation_rotation_success,
+ done);
+ }
+ else
+ {
+ clamp_route ();
+ return FSM_NEXT (CLAMP_ROUTING, clamp_elevation_rotation_success,
+ next);
+ }
+}
+
diff --git a/digital/io-hub/src/robospierre/clamp.h b/digital/io-hub/src/robospierre/clamp.h
new file mode 100644
index 00000000..104e3cb8
--- /dev/null
+++ b/digital/io-hub/src/robospierre/clamp.h
@@ -0,0 +1,57 @@
+#ifndef clamp_h
+#define clamp_h
+/* clamp.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.
+ *
+ * }}} */
+
+enum {
+ /** Slot positions. */
+ CLAMP_SLOT_FRONT_BOTTOM,
+ CLAMP_SLOT_FRONT_MIDDLE,
+ CLAMP_SLOT_FRONT_TOP,
+ CLAMP_SLOT_BACK_BOTTOM,
+ CLAMP_SLOT_BACK_MIDDLE,
+ CLAMP_SLOT_BACK_TOP,
+ CLAMP_SLOT_SIDE,
+ /** Leave the front bay, ready to enter side tunnel. */
+ CLAMP_BAY_FRONT_LEAVE,
+ /** Leave the back bay, ready to enter side tunnel. */
+ CLAMP_BAY_BACK_LEAVE,
+ /* Enter the side bay. Position on the side, above wheels. */
+ CLAMP_BAY_SIDE_ENTER_LEAVE,
+};
+
+/** Is slot in front bay? */
+#define CLAMP_IS_SLOT_IN_FRONT_BAY(slot) \
+ ((slot) <= CLAMP_SLOT_FRONT_TOP)
+
+/** Is slot in back bay? */
+#define CLAMP_IS_SLOT_IN_BACK_BAY(slot) \
+ ((slot) >= CLAMP_SLOT_BACK_BOTTOM && (slot) <= CLAMP_SLOT_BACK_TOP)
+
+/** Move clamp to given position. */
+void
+clamp_move (uint8_t pos);
+
+#endif /* clamp_h */
diff --git a/digital/io-hub/src/robospierre/contact_defs.h b/digital/io-hub/src/robospierre/contact_defs.h
new file mode 100644
index 00000000..55b34354
--- /dev/null
+++ b/digital/io-hub/src/robospierre/contact_defs.h
@@ -0,0 +1,37 @@
+#ifndef contact_defs_h
+#define contact_defs_h
+/* contact_defs.h */
+/* robospierre - Eurobot 2011 AI. {{{
+ *
+ * Copyright (C) 2011 Nicolas Schodet
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+
+#define CONTACT_COLOR A, 7
+#define CONTACT_JACK F, 7
+#define CONTACT_EX1 E, 0
+#define CONTACT_EX2 E, 1
+
+#define CONTACT_LIST \
+ CONTACT (CONTACT_EX1) \
+ CONTACT (CONTACT_EX2)
+
+#endif /* contact_defs_h */
diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c
new file mode 100644
index 00000000..3a32a1ae
--- /dev/null
+++ b/digital/io-hub/src/robospierre/main.c
@@ -0,0 +1,225 @@
+/* main.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 "modules/utils/utils.h"
+#include "modules/uart/uart.h"
+#include "modules/proto/proto.h"
+
+#include "timer.h"
+#include "chrono.h"
+#include "simu.host.h"
+
+#include "asserv.h"
+#include "mimot.h"
+#include "twi_master.h"
+
+#include "pwm.h"
+#include "contact.h"
+
+#define FSM_NAME AI
+#include "fsm.h"
+#ifdef HOST
+# include <string.h>
+#endif
+
+#include "clamp.h"
+
+#include "io.h"
+
+/** Our color. */
+enum team_color_e team_color;
+
+/** 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_;
+
+/** Main initialisation. */
+static void
+main_init (void)
+{
+ /* Serial port */
+ uart0_init ();
+ /* Enable interrupts */
+ sei ();
+ /* Main timer */
+ timer_init ();
+ timer_wait ();
+ /* TWI communications */
+ asserv_init ();
+ mimot_init ();
+ twi_master_init ();
+ /* IO modules. */
+ pwm_init ();
+ contact_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 mimot_motor0_status, mimot_motor1_status;
+ mimot_motor0_status = mimot_motor0_cmd_status ();
+ mimot_motor1_status = mimot_motor1_cmd_status ();
+ if (mimot_motor0_status == success
+ && mimot_motor1_status == success)
+ FSM_HANDLE_E (AI, clamp_elevation_rotation_success);
+ else if (mimot_motor0_status == failure)
+ FSM_HANDLE_E (AI, clamp_elevation_failure);
+ else if (mimot_motor1_status == failure)
+ FSM_HANDLE_E (AI, clamp_rotation_failure);
+}
+
+/** 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. */
+ pwm_update ();
+ contact_update ();
+ /* 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 ());
+ main_stats_contact_cpt_ = main_stats_contact_;
+ }
+ }
+}
+
+/** 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 ('w', 3):
+ /* Set PWM.
+ * - 1b: index.
+ * - 1w: value. */
+ pwm_set (args[0], v8_to_v16 (args[1], args[2]));
+ break;
+ case c ('w', 7):
+ /* Set timed PWM.
+ * - 1b: index.
+ * - 1w: value.
+ * - 1w: time.
+ * - 1w: rest value. */
+ pwm_set_timed (args[0], v8_to_v16 (args[1], args[2]),
+ v8_to_v16 (args[3], args[4]),
+ v8_to_v16 (args[5], args[6]));
+ break;
+ case c ('c', 1):
+ /* Move clamp.
+ * - 1b: position. */
+ clamp_move (args[0]);
+ 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;
+ 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/robospierre/simu.host.c b/digital/io-hub/src/robospierre/simu.host.c
new file mode 100644
index 00000000..8f4019aa
--- /dev/null
+++ b/digital/io-hub/src/robospierre/simu.host.c
@@ -0,0 +1,64 @@
+/* simu.host.c - Host simulation. */
+/* 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 "simu.host.h"
+
+#include "modules/utils/utils.h"
+#include "modules/host/host.h"
+#include "modules/host/mex.h"
+#include "io.h"
+
+/** AVR registers. */
+uint8_t PINE;
+
+/** Initialise simulation. */
+void
+simu_init (void)
+{
+ const char *mex_instance;
+ mex_node_connect ();
+ mex_instance = host_get_instance ("io-hub0", 0);
+}
+
+/** 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;
+}
+
diff --git a/digital/io-hub/src/robospierre/simu.host.h b/digital/io-hub/src/robospierre/simu.host.h
new file mode 100644
index 00000000..4b461e86
--- /dev/null
+++ b/digital/io-hub/src/robospierre/simu.host.h
@@ -0,0 +1,36 @@
+#ifndef simu_host_h
+#define simu_host_h
+/* simu.host.h - Host simulation. */
+/* 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.
+ *
+ * }}} */
+
+#ifdef HOST
+
+extern uint8_t PINE;
+
+#else /* !defined (HOST) */
+
+#endif /* !defined (HOST) */
+
+#endif /* simu_host_h */