summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--digital/ai/tools/test_simu_control_robospierre.py10
-rw-r--r--digital/io-hub/src/robospierre/Makefile13
-rw-r--r--digital/io-hub/src/robospierre/bot.h39
-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/main.c45
-rw-r--r--digital/io-hub/tools/io_hub/io_hub.py3
7 files changed, 348 insertions, 1 deletions
diff --git a/digital/ai/tools/test_simu_control_robospierre.py b/digital/ai/tools/test_simu_control_robospierre.py
index 5300918a..b1cccf4b 100644
--- a/digital/ai/tools/test_simu_control_robospierre.py
+++ b/digital/ai/tools/test_simu_control_robospierre.py
@@ -62,6 +62,13 @@ class TestSimuControl (TestSimu):
text = 'Rotation ccw', padx = 0, pady = 0,
command = self.rotation_ccw_command)
self.rotation_ccw_button.pack ()
+ self.clamp_pos_scale = Scale (self.control_frame, orient = HORIZONTAL,
+ from_ = 0, to = 6)
+ self.clamp_pos_scale.pack ()
+ self.clamp_move_button = Button (self.control_frame,
+ text = 'Move clamp', padx = 0, pady = 0,
+ command = self.clamp_move_command)
+ self.clamp_move_button.pack ()
self.table_view.bind ('<1>', self.move)
self.table_view.bind ('<3>', self.orient)
@@ -94,6 +101,9 @@ class TestSimuControl (TestSimu):
def rotation_ccw_command (self):
self.mimot.speed_pos ('a1', -self.ROTATION_STROKE / 2)
+ def clamp_move_command (self):
+ self.io.clamp_move (self.clamp_pos_scale.get ())
+
def change_color (self, *dummy):
pass
diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile
index a95d7586..21411631 100644
--- a/digital/io-hub/src/robospierre/Makefile
+++ b/digital/io-hub/src/robospierre/Makefile
@@ -4,6 +4,8 @@ BASE = ../../../avr
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 \
@@ -11,7 +13,7 @@ io_hub_SOURCES = main.c \
# Modules needed for IO.
MODULES = proto uart twi utils \
math/fixed math/geometry
-AI_MODULES = twi_master common utils
+AI_MODULES = twi_master common utils fsm
# Configuration file.
CONFIGFILE = avrconfig.h
AVR_MCU = at90usb1287
@@ -25,4 +27,13 @@ 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/bot.h b/digital/io-hub/src/robospierre/bot.h
index fa65ca07..9043a62c 100644
--- a/digital/io-hub/src/robospierre/bot.h
+++ b/digital/io-hub/src/robospierre/bot.h
@@ -34,4 +34,43 @@
# 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/main.c b/digital/io-hub/src/robospierre/main.c
index 980ab138..3a32a1ae 100644
--- a/digital/io-hub/src/robospierre/main.c
+++ b/digital/io-hub/src/robospierre/main.c
@@ -39,6 +39,14 @@
#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. */
@@ -76,6 +84,27 @@ main_init (void)
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. */
@@ -150,6 +179,11 @@ 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 ('c', 1):
+ /* Move clamp.
+ * - 1b: position. */
+ clamp_move (args[0]);
+ break;
/* Stats commands.
* - b: interval between stats. */
case c ('A', 1):
@@ -173,6 +207,17 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
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 ();
diff --git a/digital/io-hub/tools/io_hub/io_hub.py b/digital/io-hub/tools/io_hub/io_hub.py
index 36e5e4df..a363e5f6 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 clamp_move (self, pos):
+ self.proto.send ('c', 'B', pos)
+
def close (self):
self.reset ()
self.proto.wait (lambda: True)