summaryrefslogtreecommitdiff
path: root/digital
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
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')
-rw-r--r--digital/ai/src/common/debug.host.h (renamed from digital/io/src/debug.host.h)10
-rw-r--r--digital/ai/src/common/defs.h (renamed from digital/io/src/defs.h)12
-rw-r--r--digital/ai/src/common/playground.h64
-rw-r--r--digital/ai/src/fsm/fsm.h566
-rw-r--r--digital/ai/src/fsm/fsm.host.c1571
-rw-r--r--digital/ai/src/fsm/fsm_queue.c71
-rw-r--r--digital/ai/src/fsm/fsm_queue.h41
-rw-r--r--digital/ai/src/twi_master/asserv.c (renamed from digital/io/src/asserv.c)27
-rw-r--r--digital/ai/src/twi_master/asserv.h (renamed from digital/io/src/asserv.h)15
-rw-r--r--digital/ai/src/twi_master/mimot.c (renamed from digital/io/src/mimot.c)2
-rw-r--r--digital/ai/src/twi_master/mimot.h (renamed from digital/io/src/mimot.h)2
-rw-r--r--digital/ai/src/twi_master/twi_master.c (renamed from digital/io/src/twi_master.c)2
-rw-r--r--digital/ai/src/twi_master/twi_master.h (renamed from digital/io/src/twi_master.h)2
-rw-r--r--digital/ai/src/utils/chrono.c (renamed from digital/io/src/chrono.c)18
-rw-r--r--digital/ai/src/utils/chrono.h (renamed from digital/io/src/chrono.h)10
-rw-r--r--digital/ai/src/utils/timer.avr.c (renamed from digital/io/src/main_timer.avr.c)30
-rw-r--r--digital/ai/src/utils/timer.h (renamed from digital/io/src/main_timer.h)44
-rw-r--r--digital/ai/tools/marcel.py39
-rw-r--r--digital/ai/tools/robospierre.py39
-rw-r--r--digital/ai/tools/test_simu.py (renamed from digital/io/tools/test_simu.py)77
-rw-r--r--digital/ai/tools/test_simu_control_marcel.py (renamed from digital/io/tools/test_simu_control.py)13
-rw-r--r--digital/ai/tools/test_simu_control_robospierre.py111
-rw-r--r--digital/asserv/src/asserv/counter_ext.avr.c4
-rw-r--r--digital/asserv/tools/asserv/init.py14
-rw-r--r--digital/asserv/tools/inter_asserv.py16
-rw-r--r--digital/asserv/tools/step.py11
-rw-r--r--digital/asserv/tools/test_goto.py12
-rw-r--r--digital/asserv/tools/write_eeprom.py11
-rw-r--r--digital/avr/common/io.h19
-rw-r--r--digital/avr/modules/math/geometry/distance.c4
-rw-r--r--digital/avr/modules/math/geometry/distance.h4
-rw-r--r--digital/avr/modules/twi/twi_hard.avr.c6
-rw-r--r--digital/avr/modules/uart/uart.avr.c16
-rw-r--r--digital/avr/modules/utils/utils.avr.h1
-rw-r--r--digital/io-hub/src/common/contact.avr.c98
-rw-r--r--digital/io-hub/src/common/contact.h55
-rw-r--r--digital/io-hub/src/common/contact.host.c98
-rw-r--r--digital/io-hub/src/common/pwm.avr.c133
-rw-r--r--digital/io-hub/src/common/pwm.h60
-rw-r--r--digital/io-hub/src/common/pwm.host.c109
-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
-rw-r--r--digital/io-hub/tools/io_hub/__init__.py2
-rw-r--r--digital/io-hub/tools/io_hub/init.py6
-rw-r--r--digital/io-hub/tools/io_hub/io_hub.py57
-rw-r--r--digital/io-hub/tools/io_hub/mex.py119
-rw-r--r--digital/io/src/Makefile27
-rw-r--r--digital/io/src/ai.conf12
-rw-r--r--digital/io/src/ai_hola_cb.c165
-rw-r--r--digital/io/src/ai_init_cb.c213
-rw-r--r--digital/io/src/ai_loader_cb.c636
-rw-r--r--digital/io/src/ai_move_cb.c648
-rw-r--r--digital/io/src/ai_top_cb.c434
-rw-r--r--digital/io/src/bot.h19
-rw-r--r--digital/io/src/fsm.c137
-rw-r--r--digital/io/src/fsm.h126
-rw-r--r--digital/io/src/fsm_templates/template_cb.h10
-rw-r--r--digital/io/src/fsm_templates/template_cb_decl.h6
-rw-r--r--digital/io/src/fsm_templates/template_cb_impl.c9
-rw-r--r--digital/io/src/fsm_templates/template_fsm.c55
-rw-r--r--digital/io/src/fsm_templates/template_fsm.h52
-rw-r--r--digital/io/src/fsm_templates/template_hola_cb_skel.c11
-rw-r--r--digital/io/src/fsm_templates/template_init_cb_skel.c11
-rw-r--r--digital/io/src/fsm_templates/template_loader_cb_skel.c11
-rw-r--r--digital/io/src/fsm_templates/template_move_cb_skel.c11
-rw-r--r--digital/io/src/fsm_templates/template_top_cb_skel.c11
-rw-r--r--digital/io/src/hola.c115
-rw-r--r--digital/io/src/hola.fsm59
-rw-r--r--digital/io/src/init.c206
-rw-r--r--digital/io/src/init.fsm106
-rw-r--r--digital/io/src/loader.c613
-rw-r--r--digital/io/src/loader.fsm227
-rw-r--r--digital/io/src/main.c129
-rw-r--r--digital/io/src/main.h4
-rw-r--r--digital/io/src/move.c605
-rw-r--r--digital/io/src/move.fsm137
-rw-r--r--digital/io/src/playground.h96
-rw-r--r--digital/io/src/playground_2010.h46
-rw-r--r--digital/io/src/radar.c3
-rw-r--r--digital/io/src/simu.host.c4
-rw-r--r--digital/io/src/switch.avr.c2
-rw-r--r--digital/io/src/switch.h3
-rw-r--r--digital/io/src/top.c360
-rw-r--r--digital/io/src/top.fsm162
-rw-r--r--digital/io/src/trace.trc2
-rw-r--r--digital/io/src/usdist.c2
-rw-r--r--digital/io/src/usdist.h2
-rw-r--r--digital/io/tools/dump_usdist.py7
-rw-r--r--digital/io/tools/io/init.py14
-rw-r--r--digital/io/tools/io/io.py7
-rw-r--r--digital/mimot/src/dirty/counter_ext.avr.c2
-rw-r--r--digital/mimot/tools/inter_mimot.py16
-rw-r--r--digital/mimot/tools/mimot/init.py14
-rw-r--r--digital/mimot/tools/write_eeprom.py17
102 files changed, 6338 insertions, 3702 deletions
diff --git a/digital/io/src/debug.host.h b/digital/ai/src/common/debug.host.h
index 4930b937..3fb7e23f 100644
--- a/digital/io/src/debug.host.h
+++ b/digital/ai/src/common/debug.host.h
@@ -1,7 +1,7 @@
#ifndef debug_host_h
#define debug_host_h
/* debug.host.h */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
+/* ai - Robot Artificial Intelligence. {{{
*
* Copyright (C) 2008 Dufour Jérémy
*
@@ -26,17 +26,17 @@
* }}} */
/**
- * @file Debug printf functions.
+ * Debug printf functions.
* This module include a macro to add some debug functions to printf a message
* under host target. It does nothing under avr target.
*/
#ifdef HOST
-#include <stdio.h>
-#define DPRINTF(format, args...) \
+# include <stdio.h>
+# define DPRINTF(format, args...) \
do { fprintf (stderr, (format), ## args); } while (0)
#else /* HOST */
-#define DPRINTF(format, args...)
+# define DPRINTF(format, args...)
#endif /* HOST */
#endif /* debug_host_h */
diff --git a/digital/io/src/defs.h b/digital/ai/src/common/defs.h
index d80ff96d..f16e9e52 100644
--- a/digital/io/src/defs.h
+++ b/digital/ai/src/common/defs.h
@@ -1,7 +1,7 @@
#ifndef defs_h
#define defs_h
/* defs.h */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
+/* ai - Robot Artificial Intelligence. {{{
*
* Copyright (C) 2010 Nicolas Schodet
*
@@ -43,4 +43,14 @@ typedef struct position_t position_t;
/** Convert degrees to an angle usable in position_t. */
#define POSITION_A_DEG(a) G_ANGLE_UF016_DEG (a)
+/** Team color, determine the start zone side. */
+enum team_color_e
+{
+ TEAM_COLOR_LEFT = 0,
+ TEAM_COLOR_RIGHT = 1
+};
+
+/** Current team color, to be read at start up. */
+extern enum team_color_e team_color;
+
#endif /* defs_h */
diff --git a/digital/ai/src/common/playground.h b/digital/ai/src/common/playground.h
new file mode 100644
index 00000000..18c81ecf
--- /dev/null
+++ b/digital/ai/src/common/playground.h
@@ -0,0 +1,64 @@
+#ifndef playground_h
+#define playground_h
+/* playground.h */
+/* ai - Robot Artificial Intelligence. {{{
+ *
+ * Copyright (C) 2011 Nicolas Schodet
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+#include "defs.h"
+
+/**
+ * Define playground constant data and playground macro to define position and
+ * angle depending of the current team color.
+ */
+
+/** Playground width, mm. */
+#define PG_WIDTH 3000
+
+/** Playground length, mm. */
+#define PG_LENGTH 2100
+
+/** Distance to playground borders for general movements, mm. */
+#define PG_BORDER_DISTANCE 250
+
+/** Return X coordinate, applying symmetry according to team color. */
+#define PG_X(x) (team_color ? (x) : PG_WIDTH - (x))
+
+/** Same as PG_Y, but for Y coordinate. Actually nothing is done, there is no
+ * symmetry. */
+#define PG_Y(y) (y)
+
+/** Return an angle, applying symmetry according to team color. Takes degrees as
+ * input. */
+#define PG_A_DEG(a) \
+ (team_color ? POSITION_A_DEG (a) : POSITION_A_DEG (180 - (a)))
+
+/** Initialiser for position_t applying symmetry according to color. Takes
+ * degrees for angle. */
+#define PG_POSITION_DEG(x, y, a) \
+ { { PG_X (x), PG_Y (y) }, PG_A_DEG (a) }
+
+/** Initialiser for vect_t applying symmetry according to color. */
+#define PG_VECT(x, y) \
+ (vect_t) { PG_X (x), PG_Y (y) }
+
+#endif /* playground_h */
diff --git a/digital/ai/src/fsm/fsm.h b/digital/ai/src/fsm/fsm.h
new file mode 100644
index 00000000..6b376d30
--- /dev/null
+++ b/digital/ai/src/fsm/fsm.h
@@ -0,0 +1,566 @@
+/*
+ AngFSM - AngFSM - Almost Non Generated Finite State Machine
+
+ Copyright 2011 Jerome Jutteau
+
+Contact:
+ * email: j.jutteau _AT_ gmail _DOT_ com
+ * website: http://fuu.im/angfsm/
+
+ This file is part of AngFSM.
+
+ AngFSM 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 3 of the License, or
+ (at your option) any later version.
+
+ AngFSM 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 AngFSM. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <inttypes.h>
+
+#ifndef _FSM_GEN_
+#define _FSM_GEN_
+
+#ifndef FSM_NAME
+#define FSM_NAME FSM
+#endif
+
+#define XSTR(s) STR(s)
+#define STR(s) #s
+#define FSM_PASTE(a, b) a ## b
+#define FSM_PASTE_EXPAND(a, b) FSM_PASTE (a, b)
+#define FSM_PASTE3_EXPAND(a, b, c)\
+ FSM_PASTE_EXPAND (a, FSM_PASTE_EXPAND (b, c))
+#define FIRST(first, others...) first
+
+#ifdef __AVR_ARCH__
+/* Include generated header. */
+#include XSTR (FSM_PASTE_EXPAND (fsm_, FSM_NAME)_gen.h)
+
+/* Disable init macro. */
+#define FSM_INIT
+
+/* Disable code generation macro. */
+#define FSM_GENERATE(arch, string)
+
+/* Disable FSM_IMPORT macro. */
+#define FSM_IMPORT(fsm_name)
+
+/** Immediatly handle an event by the fsm. */
+#define FSM_HANDLE(fsm, event) fsm_##fsm##_handle (FSM_EVENT_##fsm##_##event )
+
+/** Says if an event can actually be handle or not by the fsm. */
+#define FSM_CAN_HANDLE(fsm, event) fsm_##fsm##_can_handle \
+ (FSM_EVENT_##fsm##_##event)
+
+/** Reset fsm to it's initial state. */
+#define FSM_RESET(fsm) fsm_##fsm##_init ();
+
+/** Disable dot generation. */
+#define FSM_GEN_DOT(fsm)
+
+/** Disable state defining
+ *
+ * Define states of the fsm, can be called several times to add other states. */
+#define FSM_STATES(states...)
+
+/** Disable event defining */
+#define FSM_EVENTS(events...)
+
+/** Disable macro. */
+#define FSM_START_WITH(starters...)
+
+#define FSM_TRANS(state, event, output_branches...)\
+ FSM_PASTE3_EXPAND (fsm_, FSM_NAME,_branch_t)\
+FSM_PASTE3_EXPAND (fsm_, FSM_NAME,_trans_func_##state##_##event) ()
+
+#define FSM_NEXT(state, event, branch...)\
+ FSM_PASTE3_EXPAND (FSM_BRANCH_, FSM_NAME,_##state##_##event##_##branch)
+
+/** Transform an event in uint16_t. */
+#define FSM_EVENT(fsm, event) FSM_EVENT_##fsm##_##event
+
+/** Handle event from uint16_t. */
+#define FSM_HANDLE_VAR(fsm, event) fsm_##fsm##_handle (event)
+
+/* Can we handle event from uint16_t ? */
+#define FSM_CAN_HANDLE_VAR(fsm, event) fsm_##fsm##_can_handle (event)
+
+/* Time out macros. */
+#define FSM_TRANS_TIMEOUT(state, timeout, output_branches...)\
+ FSM_TRANS (state, state##_TIMEOUT, output_branches)
+
+#define FSM_NEXT_TIMEOUT(state, branch...)\
+ FSM_NEXT (state, state##_TIMEOUT, branch)
+
+#define FSM_HANDLE_TIMEOUT(fsm_name) fsm_##fsm_name##_handle_timeout ()
+
+#else /* Compiling for HOST */
+#include <stdlib.h>
+
+/* Describe an event. */
+typedef struct
+{
+ /* Name of the event. */
+ char *var_name;
+ /* Unique code of the event. */
+ uint code;
+} fsm_build_event_t;
+
+/* Chain of events. */
+typedef struct fsm_build_event_chain_t
+{
+ fsm_build_event_t event;
+ struct fsm_build_event_chain_t *next;
+} fsm_build_event_chain_t;
+
+/* Describe a state. Same as event. */
+typedef fsm_build_event_t fsm_build_state_t;
+
+/* Chain of state */
+typedef struct fsm_build_state_chain_t
+{
+ fsm_build_state_t state;
+ struct fsm_build_state_chain_t *next;
+} fsm_build_state_chain_t;
+
+/**
+ * Describe a branch. It is the output of a transition.
+ * Branches can be chained when there are several possible output state for
+ * one event.
+ */
+typedef struct fsm_build_branch_chain_t
+{
+ /* Pointer to the output state of the branch. */
+ fsm_build_state_t *state;
+ /* Name of the branch. */
+ char *name;
+ /* Pointer to the next branch when we have the choice of the output */
+ struct fsm_build_branch_chain_t *next;
+} fsm_build_branch_chain_t;
+
+/**
+ * Describe a transition.
+ * When an event occurs, an active state may react to this event an execute a
+ * transition. The transition function returns the new state which become
+ * active.
+ */
+typedef struct
+{
+ /* Active state. */
+ fsm_build_state_t *state;
+ /* Active event. */
+ fsm_build_event_t *event;
+ /* Possible states the transition will return. */
+ fsm_build_branch_chain_t *output_branches;
+} fsm_build_trans_t;
+
+/* Chain of transitions. */
+typedef struct fsm_build_trans_chain_t
+{
+ fsm_build_trans_t trans;
+ struct fsm_build_trans_chain_t *next;
+} fsm_build_trans_chain_t;
+
+/* Pointer to a transition function. */
+typedef fsm_build_state_t* (*fsm_build_run_strans_func_t)(void);
+
+/* Chain of transitions with associated function's pointer. */
+typedef struct fsm_trans_func_chain_t
+{
+ fsm_build_run_strans_func_t func;
+ fsm_build_trans_t trans;
+ struct fsm_trans_func_chain_t *next;
+} fsm_trans_func_chain_t;
+
+/* Timeout structure. */
+typedef struct fsm_build_timeout_t
+{
+ uint timeout;
+ fsm_build_trans_t *trans;
+} fsm_build_timeout_t;
+
+/* Chain of fsm_timeout_t. */
+typedef struct fsm_build_timeout_chain_t
+{
+ fsm_build_timeout_t timeout;
+ struct fsm_build_timeout_chain_t *next;
+} fsm_build_timeout_chain_t;
+
+/* Data needed for fsm execution (for host). */
+typedef struct
+{
+ /* This table store pointers of aff all transition functions. The first
+ * dimension correspond to the events and the second to the states. If
+ * there is no transition of a event/state couple, NULL is set. This
+ * permit to react quickly to an event during execution but is not the
+ * best for memory cost.
+ */
+ fsm_build_run_strans_func_t **trans_table;
+ /* Store pointer to active states. */
+ fsm_build_state_t **active_states;
+ /* This array has the size of active_states and store the event who has
+ * triggered the corresponding active state. This is used for dot
+ * generation for visual purposes.
+ */
+ fsm_build_event_t **events_before_active_state;
+ /* Store all pointers of transition functions. */
+ fsm_trans_func_chain_t *func_pool;
+ /* Array of counters for timeout events.
+ * -1 mean counter is off. */
+ int *timeout_counters;
+} fsm_build_run_t;
+
+/* Store all Finite State Machine (fsm) informations. */
+typedef struct
+{
+ /* All events. */
+ fsm_build_event_chain_t *events;
+ /* All states. */
+ fsm_build_state_chain_t *states;
+ /* All transitions. */
+ fsm_build_trans_chain_t *trans;
+ /* Name of the fsm. */
+ char *name;
+ /* Maximal number of active states. */
+ uint max_active_states;
+ /* Total number of events. */
+ uint event_nb;
+ /* Total number of states. */
+ uint state_nb;
+ /* First active states. */
+ fsm_build_state_chain_t *starters;
+ /* All timeout. */
+ fsm_build_timeout_chain_t *timeouts;
+ /* Data for running purposes. */
+ fsm_build_run_t run;
+} fsm_build_t;
+
+/* Store all fsm in a chain. */
+typedef struct fsm_build_chain_t
+{
+ fsm_build_t *fsm;
+ struct fsm_build_chain_t *next;
+} fsm_build_chain_t;
+
+/* Store all fsm. */
+extern fsm_build_chain_t *fsm_build_all_fsm;
+/* Function to initialize fsm_build_all_fsm. */
+void fsm_build_init_all_fsm () __attribute__((constructor(101)));
+
+/** Create the fsm and functions who will be executed before and after main
+ * function. This permits initilialization of the fsm. */
+extern fsm_build_t FSM_PASTE_EXPAND (fsm_, FSM_NAME);
+void FSM_PASTE_EXPAND (fsm_build_init_, FSM_NAME)() __attribute__((constructor(102)));
+void FSM_PASTE_EXPAND (fsm_build_run_init_, FSM_NAME)() __attribute__((constructor(107)));
+void FSM_PASTE_EXPAND (fsm_build_free_, FSM_NAME)() __attribute__((destructor));
+
+#define FSM_INIT\
+ fsm_build_t FSM_PASTE_EXPAND (fsm_, FSM_NAME);\
+void FSM_PASTE_EXPAND (fsm_build_init_, FSM_NAME)()\
+{\
+ fsm_build_init (FSM_PASTE_EXPAND (&fsm_, FSM_NAME), XSTR(FSM_NAME));\
+}\
+void FSM_PASTE_EXPAND (fsm_build_run_init_, FSM_NAME)()\
+{\
+ fsm_build_run_init (& FSM_PASTE_EXPAND (fsm_, FSM_NAME));\
+}\
+void FSM_PASTE_EXPAND (fsm_build_free_, FSM_NAME)()\
+{\
+ fsm_build_free (& FSM_PASTE_EXPAND (fsm_, FSM_NAME));\
+}
+
+#define FSM_IMPORT(fsm_name)\
+ extern fsm_build_t fsm_##fsm_name;
+
+/** Generate source ou header files for the specific architecture */
+#define FSM_GENERATE(arch, string) fsm_build_gen (#arch, string);
+
+/** Immediatly handle an event by the fsm. */
+#define FSM_HANDLE(fsm, event) fsm_build_handle_string (&fsm_##fsm , #event)
+
+/** Says if an event can actually be handle or not by the fsm. */
+#define FSM_CAN_HANDLE(fsm, event) fsm_build_can_handle_string (&fsm_##fsm, #event)
+
+/** Reset fsm to it's initial state. */
+#define FSM_RESET(fsm) fsm_build_reset (&fsm_##fsm)
+
+/** Generate dot. */
+#define FSM_GEN_DOT(fsm) fsm_build_gen_dot (&fsm_##fsm)
+
+/** Define states of the fsm, can be called several times to add other states. */
+#define FSM_STATES(states...)\
+ void FSM_PASTE3_EXPAND (fsm_build_states_, FSM_NAME, FIRST (states)) () __attribute__((constructor(103)));\
+void FSM_PASTE3_EXPAND (fsm_build_states_, FSM_NAME, FIRST (states)) ()\
+{\
+ fsm_build_states (& FSM_PASTE_EXPAND (fsm_, FSM_NAME), #states);\
+}
+
+/** Define events of the fsm, can be called several times to add other events. */
+#define FSM_EVENTS(events...)\
+ void FSM_PASTE3_EXPAND (fsm_build_events_, FSM_NAME, FIRST (events)) () __attribute__((constructor(103)));\
+void FSM_PASTE3_EXPAND (fsm_build_events_, FSM_NAME, FIRST (events)) ()\
+{\
+ fsm_build_events (& FSM_PASTE_EXPAND (fsm_, FSM_NAME), #events);\
+}
+
+/** Define wich state(s) will be active at the beginning. */
+#define FSM_START_WITH(starters...)\
+ void FSM_PASTE3_EXPAND (fsm_build_start_with_, FSM_NAME, FIRST (starters))() __attribute__((constructor(104)));\
+void FSM_PASTE3_EXPAND (fsm_build_start_with_, FSM_NAME, FIRST (starters))()\
+{\
+ fsm_build_start_with (& FSM_PASTE_EXPAND (fsm_, FSM_NAME), #starters);\
+}
+
+/**
+ * Define a transition by giving a state, an event and:
+ * - a event if the transition returns only one state.
+ * - branches if the transition returns different states.
+ * See examples for reel usage.
+ */
+#define FSM_TRANS(state, event, output_branches...)\
+ fsm_build_state_t* FSM_PASTE3_EXPAND (fsm_trans_, FSM_NAME,_##state##_##event) ();\
+void FSM_PASTE3_EXPAND (fsm_build_trans_, FSM_NAME,_##state##_##event)() __attribute__((constructor(105)));\
+void FSM_PASTE3_EXPAND (fsm_build_trans_, FSM_NAME,_##state##_##event)()\
+{\
+ fsm_build_trans (& FSM_PASTE_EXPAND(fsm_, FSM_NAME), #state, #event,\
+#output_branches,\
+ & FSM_PASTE3_EXPAND (fsm_trans_, FSM_NAME,_##state##_##event));\
+}\
+fsm_build_state_t* FSM_PASTE3_EXPAND (fsm_trans_, FSM_NAME,_##state##_##event) ()
+
+/**
+ * Used to return next state by giving the actual transition informations and
+ * the branch (if there are several branches).
+ * Not directly returning the state can avoid some errors. *
+ */
+#define FSM_NEXT(state, event, branch...)\
+ fsm_build_get_next_state (& FSM_PASTE_EXPAND (fsm_, FSM_NAME), #state, #event, #branch)
+
+/**
+ * Define a transition when a state times out.
+ * You have to provide a state, a timeout value and:
+ * - a event if the transition returns only one state.
+ * - branches if the transition returns different states.
+ * See examples for reel usage.
+ */
+#define FSM_TRANS_TIMEOUT(state, timeout, output_branches...)\
+ void FSM_PASTE3_EXPAND (fsm_build_timeout_, FSM_NAME,_##state##_TIMEOUT)() __attribute__((constructor(106)));\
+void FSM_PASTE3_EXPAND (fsm_build_timeout_, FSM_NAME,_##state##_TIMEOUT)()\
+{\
+ fsm_build_timeout (& FSM_PASTE_EXPAND(fsm_, FSM_NAME), #state, XSTR(state##_TIMEOUT), timeout);\
+}\
+FSM_EVENTS (state##_TIMEOUT)\
+FSM_TRANS (state, state##_TIMEOUT, output_branches)
+
+/**
+ * Used to return next state after a timeout.
+ * Same as FSM_NEXT but without specifying an event.
+ */
+#define FSM_NEXT_TIMEOUT(state, branch...)\
+ FSM_NEXT (state, state##_TIMEOUT, branch)
+
+/** Used to handle timeout events. */
+#define FSM_HANDLE_TIMEOUT(fsm_name) fsm_build_handle_timeout (& FSM_PASTE_EXPAND (fsm_, FSM_NAME))
+
+/** Transform an event in uint16_t. */
+#define FSM_EVENT(fsm_name, event) fsm_build_get_event_code (& FSM_PASTE_EXPAND (fsm_, FSM_NAME), #event)
+
+/** Handle event from uint16_t. */
+#define FSM_HANDLE_VAR(fsm, event) fsm_build_handle_integer (& FSM_PASTE_EXPAND (fsm_, FSM_NAME), event)
+
+/* Can we handle event from uint16_t ? */
+#define FSM_CAN_HANDLE_VAR(fSM, event) fsm_build_can_handle_integer (& FSM_PASTE_EXPAND (fsm_, FSM_NAME), event)
+
+/**
+ * Parse a string who contains a list of arguments seperated by comma.
+ * It will create and store each parameters (without space and ") in an array
+ * and give the number of parsed arguments.
+ * \param string string to parse
+ * \param tab pointer to a table of strings where to store the array
+ * \param nb pointer where to store the number of parsed arguments
+ */
+void
+fsm_build_arg_parse (char *string, char ***tab, int *nb);
+
+/**
+ * Free an array of strings generated by fsm_build_arg_parse.
+ * \param tab pointer to the array of strings
+ * \param nb number of arguments stored in the array.
+ */
+void
+fsm_build_arg_free (char ***tab, int nb);
+
+/**
+ * This function is executed when a state is returned by a transition.
+ * \param fsm fsm
+ * \param trans transition where the return occurs
+ * \param branch branch to transition has chosen.
+ */
+void
+fsm_build_print (fsm_build_t *fsm,
+ fsm_build_trans_t* trans,
+ fsm_build_branch_chain_t* branch);
+
+/**
+ * Test the fsm and search for errors.
+ */
+void
+fsm_build_sanity_check (fsm_build_t *fsm);
+
+/** Reset the fsm to it's initial state. */
+void
+fsm_build_reset (fsm_build_t *fsm);
+
+/** Generate the dot file of the actual fsm state. */
+void
+fsm_build_gen_dot (fsm_build_t *fsm);
+
+/** Initialize the fsm. */
+void
+fsm_build_init (fsm_build_t *fsm, char *name);
+
+/** Initialize the running data of the fsm. */
+void
+fsm_build_run_init (fsm_build_t *fsm);
+
+/**
+ * Add some states to the fsm.
+ * \param fsm fsm
+ * \param states states separated by comma
+ **/
+void
+fsm_build_states (fsm_build_t *fsm, char *states);
+
+/**
+ * Add some events to the fsm.
+ * \param fsm fsm
+ * \param events events separated by comma
+ **/
+void
+fsm_build_events (fsm_build_t *fsm, char *events);
+
+/** Get event's pointer by giving it's name. */
+fsm_build_event_t*
+fsm_build_get_event (fsm_build_t *fsm, char *event);
+
+/** Get state's pointer by giving it's name. */
+fsm_build_state_t*
+fsm_build_get_state (fsm_build_t *fsm, char *state);
+
+/** Get event's pointer by giving it's code. */
+fsm_build_event_t*
+fsm_build_get_event_by_code (fsm_build_t *fsm, uint event);
+
+/** Get state's pointer by giving it's code. */
+fsm_build_state_t*
+fsm_build_get_state_by_code (fsm_build_t *fsm, uint state);
+
+/** Get event code as uint16_t */
+uint16_t
+fsm_build_get_event_code (fsm_build_t *fsm, char *event);
+
+/**
+ * Add a transition to the fsm.
+ * \param fsm fsm
+ * \param state state who will be active for the transition
+ * \param event event to react
+ * \param output_branches list of output branches seperated by comma or a
+ * single state (if there is only one branche).
+ * For example:
+ * - If the transition return a single state, it will be "stateX"
+ * - If the transition can return different states, it will be
+ * "branch1, state1, branch2, state2, branch3, state3" [...]
+ * \param trans_func pointer to the transition function.
+ */
+void
+fsm_build_trans (fsm_build_t *fsm,
+ char *state,
+ char *event,
+ char *output_branches,
+ fsm_build_run_strans_func_t trans_func);
+
+/* Create transition based on a timeout.
+ * \param fsm fsm
+ * \param state state who times out
+ * \param event name of the timeout (specific to this state and described in macro)
+ * \param timeout value of timeout
+ */
+void
+fsm_build_timeout (fsm_build_t *fsm, char *state, char *event, uint timeout);
+
+/**
+ * Define with which states the fsm will start.
+ * \param fsm fsm
+ * \param starters list of states seperated by comma
+ */
+void
+fsm_build_start_with (fsm_build_t *fsm, char *starters);
+
+/** Handle an event by the fsm.
+ * \param fsm fsm
+ * \param event event to handle with fsm
+ * \return zero if event is not handled at all, one otherwise
+ **/
+int
+fsm_build_handle (fsm_build_t *fsm, fsm_build_event_t *event);
+int
+fsm_build_handle_string (fsm_build_t *fsm, char *event);
+int
+fsm_build_handle_integer (fsm_build_t *fsm, uint16_t event);
+
+/**
+ * Say if the event can be handled or not.
+ * \param fsm fsm
+ * \param event event to test with the fsm
+ * \return zero if the event can be handled, non-zero otherwhise
+ */
+int
+fsm_build_can_handle (fsm_build_t *fsm, fsm_build_event_t *event);
+int
+fsm_build_can_handle_string (fsm_build_t *fsm, char *event);
+int
+fsm_build_can_handle_integer (fsm_build_t *fsm, uint16_t event);
+
+/**
+ * Handle timeout events of the fsm.
+ * \param fsm fsm
+ * \return one if an event has been handled, zero otherwise
+ */
+int
+fsm_build_handle_timeout (fsm_build_t *fsm);
+
+/** Give the state at the transition output. */
+fsm_build_state_t*
+fsm_build_get_next_state (fsm_build_t *fsm,
+ char *state,
+ char *event,
+ char *branch);
+
+/** Generate header file for target which provides more optimised version of
+ * fsm execution
+ * \param arch specify generated architecture
+ * \param embedded_strings ask to embed states and events strings (0 = not
+ * embedded)
+ */
+void
+fsm_build_gen (char *arch, uint embedded_strings);
+void
+fsm_build_gen_avr_h (fsm_build_t *fsm, uint embedded_strings);
+void
+fsm_build_gen_avr_c (fsm_build_t *fsm, uint embedded_strings);
+
+/** Free fsm allocated data. */
+void
+fsm_build_free (fsm_build_t *fsm);
+
+#endif /* Architectures */
+
+#endif /* #ifdef _FSM_GEN_ */
diff --git a/digital/ai/src/fsm/fsm.host.c b/digital/ai/src/fsm/fsm.host.c
new file mode 100644
index 00000000..bf8b761d
--- /dev/null
+++ b/digital/ai/src/fsm/fsm.host.c
@@ -0,0 +1,1571 @@
+/*
+ AngFSM - AngFSM - Almost Non Generated Finite State Machine
+
+ Copyright 2011 Jerome Jutteau
+
+Contact:
+ * email: j.jutteau _AT_ gmail _DOT_ com
+ * website: http://fuu.im/angfsm/
+
+ This file is part of AngFSM.
+
+ AngFSM 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 3 of the License, or
+ (at your option) any later version.
+
+ AngFSM 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 AngFSM. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "fsm.h"
+
+#include <assert.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+void
+fsm_build_arg_parse (char *string, char ***tab, int *nb)
+{
+ long i = 0;
+ long buff_cnt = 0;
+ char *buff;
+ int nb_cur = 0;
+ int param = 0;
+
+ assert (string && tab && nb);
+ if (strlen (string) == 0)
+ {
+ *nb = 0;
+ **tab = NULL;
+ return;
+ }
+ buff = strdup (string);
+
+ /* Count. */
+ *nb = 1;
+ for (i = 0; string[i] != '\0'; i++)
+ if (string[i] == ',')
+ (*nb)++;
+ *tab = (char **) malloc ((*nb) * sizeof (char *));
+
+ /* Fill. */
+ for (i = 0; i < (long) strlen (string) + 1; i++)
+ {
+ if (string[i] == ',' || string[i] == '\0')
+ {
+ param = 0;
+ buff[buff_cnt] = '\0';
+ (*tab)[nb_cur] = strdup (buff);
+ buff_cnt=0;
+ nb_cur++;
+ }
+ else if (string[i] != '"' && string[i] != ' ')
+ {
+ if (param == 0)
+ param = 1;
+ if (param == 2)
+ printf ("Parse error in \"%s\"\n", string);
+ buff[buff_cnt++] = string[i];
+ }
+ else if (string[i] == ' ')
+ {
+ if (param == 1)
+ param = 2;
+ }
+ }
+ free (buff);
+}
+
+void
+fsm_build_arg_free (char ***tab, int nb)
+{
+ assert (*tab);
+ int i;
+ for (i = 0; i < nb; i++)
+ {
+ assert ((*tab)[i]);
+ free ((*tab)[i]);
+ }
+ free (*tab);
+}
+
+void
+fsm_build_print (fsm_build_t *fsm,
+ fsm_build_trans_t *trans,
+ fsm_build_branch_chain_t *branch)
+{
+ //XXX What to do here ?
+ if (branch->name == NULL)
+ fprintf (stderr, "Transition: %s -- %s --> %s\n",
+ trans->state->var_name,
+ trans->event->var_name,
+ branch->state->var_name);
+ else
+ fprintf (stderr, "Transition: %s -- %s --> %s (%s)\n",
+ trans->state->var_name,
+ trans->event->var_name,
+ branch->name,
+ branch->state->var_name);
+}
+
+void
+fsm_build_sanity_check (fsm_build_t *fsm)
+{
+ fsm_build_event_chain_t *ec;
+ fsm_build_event_chain_t *ec_tmp;
+ fsm_build_state_chain_t *sc;
+ fsm_build_state_chain_t *sc_tmp;
+ fsm_build_trans_chain_t *tc;
+ fsm_build_trans_chain_t *tc_tmp;
+ fsm_build_branch_chain_t *bc;
+ uint b;
+
+ /* Sanity check 1: are all states has a different name ? */
+ sc = fsm->states;
+ while (sc != NULL)
+ {
+ sc_tmp = fsm->states;
+ while (sc_tmp != NULL)
+ {
+ if (sc != sc_tmp
+ && strcmp (sc->state.var_name, sc_tmp->state.var_name)
+ == 0)
+ assert (!"All states must have a different name.");
+ sc_tmp = sc_tmp->next;
+ }
+ sc = sc->next;
+ }
+
+ /* Sanity check 2: are all events has a different name ? */
+ ec = fsm->events;
+ while (ec != NULL)
+ {
+ ec_tmp = fsm->events;
+ while (ec_tmp != NULL)
+ {
+ if (ec != ec_tmp
+ && strcmp (ec->event.var_name, ec_tmp->event.var_name)
+ == 0)
+ assert (!"All events must have a different name.");
+ ec_tmp = ec_tmp->next;
+ }
+ ec = ec->next;
+ }
+
+ /* Sanity check 3: are all events used in a transition ? */
+ ec = fsm->events;
+ while (ec != NULL)
+ {
+ b = 0;
+ tc = fsm->trans;
+ while (tc != NULL)
+ {
+ if (&ec->event == tc->trans.event)
+ {
+ b = 1;
+ break;
+ }
+ tc = tc->next;
+ }
+ if (!b)
+ fprintf (stderr, "Warning, some events are not used in a transition (%s).\n",
+ ec->event.var_name);
+ ec = ec->next;
+ }
+
+ /* Sanity check 4: are all states used in a transition ? */
+ sc = fsm->states;
+ while (sc != NULL)
+ {
+ b = 0;
+ tc = fsm->trans;
+ while (tc != NULL)
+ {
+ if (&sc->state == tc->trans.state)
+ {
+ b = 1;
+ break;
+ }
+ /* This state may be a final state. */
+ bc = tc->trans.output_branches;
+ while (bc != NULL)
+ {
+ if (&sc->state == bc->state)
+ {
+ b = 1;
+ break;
+ }
+ bc = bc->next;
+ }
+
+ tc = tc->next;
+ }
+ if (!b)
+ fprintf (stderr, "Warning, some states are not used in a transition (%s).\n",
+ sc->state.var_name);
+ sc = sc->next;
+ }
+
+ /* Sanity check 5: are all states can be reached during execution ? */
+ fsm_build_state_chain_t *stack = NULL;
+ fsm_build_state_chain_t *tail = NULL;
+ uint i;
+ /* Initialise check array. */
+ uint *check_tab = malloc (fsm->state_nb * sizeof (uint));
+ memset (check_tab, 0, fsm->state_nb * sizeof (uint));
+ /* Initialize by adding first active states the stack. */
+ sc = fsm->starters;
+ while (sc != NULL)
+ {
+ sc_tmp = (fsm_build_state_chain_t *)
+ malloc (sizeof (fsm_build_state_chain_t));
+ sc_tmp->state = sc->state;
+ if (stack == NULL)
+ stack = sc_tmp;
+ if (tail != NULL)
+ tail->next = sc_tmp;
+ sc_tmp->next = NULL;
+ tail = sc_tmp;
+ sc = sc->next;
+ }
+ /* Get next states from the stack and check them. */
+ while (stack != NULL)
+ {
+ /* Check state. */
+ check_tab[stack->state.code] = 1;
+ /* Add to the tail of the stack th next states if they are not checked
+ * yet. */
+ tc = fsm->trans;
+ while (tc != NULL)
+ {
+ if (tc->trans.state->code == stack->state.code)
+ {
+ bc = tc->trans.output_branches;
+ while (bc != NULL)
+ {
+ if (!check_tab[bc->state->code])
+ {
+ check_tab[bc->state->code] = 1;
+ /* Add at the end of the stack. */
+ sc_tmp = (fsm_build_state_chain_t *)
+ malloc (sizeof (fsm_build_state_chain_t));
+ sc_tmp->state = *bc->state;
+ sc_tmp->next = NULL;
+ tail->next = sc_tmp;
+ tail = sc_tmp;
+ }
+ bc = bc->next;
+ }
+ }
+ tc = tc->next;
+ }
+ sc_tmp = stack->next;
+ free (stack);
+ stack = sc_tmp;
+ }
+ /* Check if some states are not reached. */
+ for (i = 0; i < fsm->state_nb; i++)
+ if (!check_tab[i])
+ fprintf (stderr, "Warning, some states won't be reachable during execution (%s).\n",
+ fsm_build_get_state_by_code (fsm, i)->var_name);
+ /* Free. */
+ free (check_tab);
+
+ /* Sanity check 6: Any transition how have the same state and same event ?
+ * Useless test ? Making the same state/event couple will cause a
+ * compilation error.
+ */
+ tc = fsm->trans;
+ while (tc != NULL)
+ {
+ tc_tmp = fsm->trans;
+ while (tc_tmp != NULL)
+ {
+ if (tc != tc_tmp
+ && tc->trans.state == tc_tmp->trans.state
+ && tc->trans.event == tc_tmp->trans.event)
+ assert (!"All transitions must have a different state/event\
+ couple.");
+ tc_tmp = tc_tmp->next;
+ }
+ tc = tc->next;
+ }
+
+ /* Sanity check 7: check that all first active states are unique. */
+ sc = fsm->starters;
+ while (sc != NULL)
+ {
+ uint b = 0;
+ sc_tmp = fsm->starters;
+ while (sc_tmp != NULL)
+ {
+ if (sc_tmp->state.code == sc->state.code)
+ b++;
+ if (b > 1)
+ assert (!"Your first active states must be unique.");
+ sc_tmp = sc_tmp->next;
+ }
+ sc = sc->next;
+ }
+
+ /* Sanity check 8: any transition output error ?
+ * for example, as we are in state s42, making a FSM_NEXT (s1, e2) will
+ * work but this is a user's mistake.
+ *
+ * TODO Find a way to check this.
+ **/
+}
+
+void
+fsm_build_reset (fsm_build_t *fsm)
+{
+ uint i;
+ fsm_build_state_chain_t *curs = fsm->starters;
+ for (i = 0; i < fsm->max_active_states; i++)
+ {
+ fsm->run.active_states[i] = NULL;
+ fsm->run.events_before_active_state[i] = NULL;
+ }
+ i = 0;
+ while (curs != NULL)
+ {
+ fsm->run.active_states[i++] = &curs->state;
+ curs = curs->next;
+ }
+}
+
+void
+fsm_build_gen_dot (fsm_build_t *fsm)
+{
+ static uint cpt = 1;
+ uint j;
+ fsm_build_trans_chain_t *tc;
+ fsm_build_branch_chain_t *bc;
+
+ uint fns = strlen (fsm->name) + strlen ("dot__.dot") + cpt + 1;
+ char *fn = malloc (fns * sizeof (char));
+ snprintf (fn, fns, "dot_%s_%u.dot", fsm->name, cpt++);
+ FILE *f = fopen(fn,"w");
+
+ /* Gen graph. */
+ fprintf (f, "digraph %s {\n", fsm->name);
+ tc = fsm->trans;
+ while (tc != NULL)
+ {
+ bc = tc->trans.output_branches;
+ while (bc != NULL)
+ {
+ fprintf (f, "\t%s -> %s [label=\"%s",
+ tc->trans.state->var_name,
+ bc->state->var_name,
+ tc->trans.event->var_name);
+ if (bc->name != NULL)
+ fprintf (f, " (%s)", bc->name);
+ fprintf (f, "\"");
+ /* If the next state is an active state we look if this this event
+ * was handled before. */
+ for (j = 0; j < fsm->max_active_states; j++)
+ if (fsm->run.active_states[j]
+ && fsm->run.active_states[j]->code == bc->state->code)
+ if (fsm->run.events_before_active_state[j]
+ && fsm->run.events_before_active_state[j]->code
+ == tc->trans.event->code)
+ fprintf (f, ", color=red");
+
+ fprintf (f, "];\n");
+ bc = bc->next;
+ }
+ tc = tc->next;
+ }
+ /* Colorise active states. */
+ for (j = 0; j < fsm->max_active_states; j++)
+ if (fsm->run.active_states[j])
+ fprintf (f, "\t%s [color=blue, fontcolor=white, style=filled];\n",
+ fsm->run.active_states[j]->var_name);
+
+ fprintf (f, "}\n\n");
+
+ /* Close file. */
+ fclose (f);
+
+ /* Free. */
+ free (fn);
+}
+
+/* All fsm. */
+fsm_build_chain_t *fsm_build_all_fsm;
+void fsm_build_init_all_fsm ()
+{
+ fsm_build_all_fsm = NULL;
+}
+
+/* Just initialise structures. */
+void
+fsm_build_init (fsm_build_t *fsm, char *name)
+{
+ assert (fsm);
+ fsm->events = NULL;
+ fsm->states = NULL;
+ fsm->trans = NULL;
+ fsm->name = name;
+ fsm->max_active_states = 0;
+ fsm->event_nb = 0;
+ fsm->state_nb = 0;
+ fsm->starters = NULL;
+ fsm->timeouts = NULL;
+
+ fsm->run.trans_table = NULL;
+ fsm->run.active_states = NULL;
+ fsm->run.events_before_active_state = NULL;
+ fsm->run.func_pool = NULL;
+ fsm->run.timeout_counters = NULL;
+
+ /* Store fsm. */
+ fsm_build_chain_t *niou = (fsm_build_chain_t *)
+ malloc (sizeof (fsm_build_chain_t));
+ niou->fsm = fsm;
+ niou->next = fsm_build_all_fsm;
+ fsm_build_all_fsm = niou;
+}
+
+/* Prepare the fsm to run (at very end). */
+void
+fsm_build_run_init (fsm_build_t *fsm)
+{
+ uint i,j;
+
+ /* Sanity check. */
+ fsm_build_sanity_check (fsm);
+
+ /* Create empty transition table. */
+ fsm->run.trans_table = (fsm_build_run_strans_func_t **)
+ malloc (fsm->event_nb * sizeof (fsm_build_run_strans_func_t *));
+ for (i = 0; i < fsm->event_nb; i++)
+ {
+ fsm->run.trans_table[i] = (fsm_build_run_strans_func_t *)
+ malloc (fsm->state_nb * sizeof (fsm_build_run_strans_func_t));
+ for (j = 0; j < fsm->state_nb; j++)
+ fsm->run.trans_table[i][j] = NULL;
+ }
+
+ /* Fill with transitions. */
+ fsm_trans_func_chain_t *fc = fsm->run.func_pool;
+ while (fc != NULL)
+ {
+ fsm->run.trans_table[fc->trans.event->code][fc->trans.state->code] =
+ fc->func;
+ fc = fc->next;
+ }
+
+ /* Free function pool. */
+ fsm_trans_func_chain_t *old_fc;
+ fc = fsm->run.func_pool;
+ while (fc != NULL)
+ {
+ old_fc = fc;
+ fc = fc->next;
+ free (old_fc);
+ }
+ fsm->run.func_pool = NULL;
+
+ /* Initialize active states. */
+ fsm->run.active_states = (fsm_build_state_t **) malloc
+ (fsm->max_active_states * sizeof (fsm_build_state_t *));
+ for (i = 0; i < fsm->max_active_states; i++)
+ fsm->run.active_states[i] = NULL;
+ fsm_build_state_chain_t *s = fsm->starters;
+ i = 0;
+ while (s != NULL)
+ {
+ fsm->run.active_states[i++] = &s->state;
+ s = s->next;
+ }
+
+ /* Initialize last events before active states. */
+ fsm->run.events_before_active_state = (fsm_build_event_t **) malloc
+ (fsm->max_active_states * sizeof (fsm_build_event_t *));
+ for (i = 0; i < fsm->max_active_states; i++)
+ fsm->run.events_before_active_state[i] = NULL;
+
+ /* Initialize timeout counters. */
+ fsm->run.timeout_counters = (int *)
+ malloc (fsm->max_active_states * sizeof (int));
+ for (i = 0; i < fsm->max_active_states; i++)
+ fsm->run.timeout_counters[i] = -1;
+ fsm_build_timeout_chain_t *toc = fsm->timeouts;
+ for (i = 0; i < fsm->max_active_states; i++)
+ while (toc != NULL)
+ {
+ if (fsm->run.active_states[i]->code
+ == toc->timeout.trans->state->code)
+ fsm->run.timeout_counters[i] = toc->timeout.timeout;
+ toc = toc->next;
+ }
+}
+
+void
+fsm_build_states (fsm_build_t *fsm, char *states)
+{
+ fsm_build_state_chain_t *head = fsm->states;
+ char **args;
+ int i, nb;
+
+ fsm_build_arg_parse (states, &args, &nb);
+ for (i = 0; i < nb; i++)
+ {
+ fsm_build_state_chain_t *s = (fsm_build_state_chain_t *)
+ malloc (sizeof (fsm_build_state_chain_t));
+ s->state.var_name = strdup (args[i]);
+ s->state.code = fsm->state_nb++;
+ s->next = head;
+ head = s;
+ }
+ fsm_build_arg_free (&args, nb);
+ fsm->states = head;
+}
+
+void
+fsm_build_events (fsm_build_t *fsm, char *events)
+{
+ fsm_build_event_chain_t *head = fsm->events;
+ char **args;
+ int i, nb;
+
+ if (strcmp (events, "_TIMEOUT_") == 0
+ && fsm_build_get_event (fsm, events) != NULL)
+ return;
+ fsm_build_arg_parse (events, &args, &nb);
+ for (i = 0; i < nb; i++)
+ {
+ fsm_build_event_chain_t *e = (fsm_build_event_chain_t *)
+ malloc (sizeof (fsm_build_event_chain_t));
+ e->event.var_name = strdup (args[i]);
+ e->event.code = fsm->event_nb++;
+ e->next = head;
+ head = e;
+ }
+ fsm_build_arg_free (&args, nb);
+ fsm->events = head;
+}
+
+fsm_build_event_t *
+fsm_build_get_event (fsm_build_t *fsm, char *event)
+{
+ fsm_build_event_chain_t *curs = fsm->events;
+ while (curs != NULL)
+ {
+ if (strcmp (curs->event.var_name, event) == 0)
+ return &(curs->event);
+ curs = curs->next;
+ }
+ return NULL;
+}
+
+fsm_build_state_t *
+fsm_build_get_state (fsm_build_t *fsm, char *state)
+{
+ fsm_build_state_chain_t *curs = fsm->states;
+ while (curs != NULL)
+ {
+ if (strcmp (curs->state.var_name, state) == 0)
+ return &(curs->state);
+ curs = curs->next;
+ }
+ return NULL;
+}
+
+fsm_build_event_t*
+fsm_build_get_event_by_code (fsm_build_t *fsm, uint event)
+{
+ fsm_build_event_chain_t *curs = fsm->events;
+ while (curs != NULL)
+ {
+ if (curs->event.code == event)
+ return &(curs->event);
+ curs = curs->next;
+ }
+ return NULL;
+}
+
+fsm_build_state_t*
+fsm_build_get_state_by_code (fsm_build_t *fsm, uint state)
+{
+ fsm_build_state_chain_t *curs = fsm->states;
+ while (curs != NULL)
+ {
+ if (curs->state.code == state)
+ return &(curs->state);
+ curs = curs->next;
+ }
+ return NULL;
+}
+
+uint16_t
+fsm_build_get_event_code (fsm_build_t *fsm, char *event)
+{
+ assert (fsm);
+ fsm_build_event_t *e = fsm_build_get_event (fsm, event);
+ assert (e);
+ return e->code;
+}
+
+void
+fsm_build_trans (fsm_build_t *fsm,
+ char *state,
+ char *event,
+ char *output_branches,
+ fsm_build_run_strans_func_t trans_func)
+{
+ fsm_build_trans_t t;
+ fsm_build_branch_chain_t *b;
+ char **args;
+ int i, nb;
+
+ t.output_branches = NULL;
+ /* Check state and event exists. */
+ t.state = fsm_build_get_state (fsm, state);
+ t.event = fsm_build_get_event (fsm, event);
+ assert (t.state);
+ assert (t.event);
+
+ fsm_build_arg_parse (output_branches, &args, &nb);
+ /* Only one output state. */
+ if (nb == 1)
+ {
+ t.output_branches = (fsm_build_branch_chain_t *)
+ malloc (sizeof (fsm_build_branch_chain_t));
+ t.output_branches->name = NULL;
+ t.output_branches->state = fsm_build_get_state (fsm, args[0]);
+ assert (t.output_branches->state);
+ t.output_branches->next = NULL;
+ }
+ else
+ for (i = 0; i < nb; i = i + 2)
+ {
+ b = (fsm_build_branch_chain_t *)
+ malloc (sizeof (fsm_build_branch_chain_t));
+ b->name = strdup (args[i]);
+ b->state = fsm_build_get_state (fsm, args[i+1]);
+ assert (b->state);
+ b->next = t.output_branches;
+ t.output_branches = b;
+ }
+ fsm_build_arg_free (&args, nb);
+
+ /* Add trans to fsm. */
+ fsm_build_trans_chain_t *tc = (fsm_build_trans_chain_t *)
+ malloc (sizeof (fsm_build_trans_chain_t));
+ tc->trans = t;
+ tc->next = fsm->trans;
+ fsm->trans = tc;
+
+ /* Add trans function to run context. */
+ fsm_trans_func_chain_t *fc = (fsm_trans_func_chain_t *)
+ malloc (sizeof (fsm_trans_func_chain_t));
+ fc->func = trans_func;
+ fc->trans = tc->trans;
+ fc->next = fsm->run.func_pool;
+ fsm->run.func_pool = fc;
+}
+
+void
+fsm_build_timeout (fsm_build_t *fsm, char *state, char *event, uint timeout)
+{
+ fsm_build_trans_chain_t *tc = NULL;
+ fsm_build_trans_t *t = NULL;
+ fsm_build_state_t *s = NULL;
+ fsm_build_event_t *e = NULL;
+ fsm_build_timeout_chain_t *to = NULL;
+
+ assert (fsm);
+ assert (state);
+ assert (event);
+
+ /* Find the corresponding transition. */
+ tc = fsm->trans;
+ s = fsm_build_get_state (fsm, state);
+ e = fsm_build_get_event (fsm, event);
+ assert (tc);
+ assert (s);
+ assert (e);
+ while (tc != NULL)
+ {
+ if (tc->trans.state == s && tc->trans.event == e)
+ {
+ t = &tc->trans;
+ break;
+ }
+ tc = tc->next;
+ }
+ assert (t);
+
+ /* Fill a new timeout. */
+ to = (fsm_build_timeout_chain_t *)
+ malloc (sizeof (fsm_build_timeout_chain_t));
+ to->timeout.timeout = timeout;
+ to->timeout.trans = t;
+
+ /* Add timeout to chain. */
+ to->next = fsm->timeouts;
+ fsm->timeouts = to;
+}
+
+void
+fsm_build_start_with (fsm_build_t *fsm, char *starters)
+{
+ char **args;
+ int nb, i;
+ fsm_build_arg_parse (starters, &args, &nb);
+ fsm->max_active_states += nb;
+ for (i=0; i < nb; i++)
+ {
+ fsm_build_state_chain_t *sc = (fsm_build_state_chain_t *)
+ malloc (sizeof (fsm_build_state_chain_t));
+ fsm_build_state_t *s = fsm_build_get_state (fsm, args[i]);
+ assert (s);
+ sc->state = *s;
+ sc->next = fsm->starters;
+ fsm->starters = sc;
+ }
+ fsm_build_arg_free (&args, nb);
+}
+
+int
+fsm_build_handle (fsm_build_t *fsm, fsm_build_event_t *e)
+{
+ fsm_build_state_t *s = NULL;
+ fsm_build_timeout_chain_t *toc = NULL;
+ assert (e);
+ uint i;
+ int handled = 0;
+ for (i = 0; i < fsm->max_active_states; i++)
+ {
+ s = fsm->run.active_states[i];
+ if (s && fsm->run.trans_table[e->code][s->code])
+ {
+ fsm->run.events_before_active_state[i] = e;
+ fsm->run.active_states[i] = fsm->run.trans_table[e->code][s->code]();
+ /* Check the new state has a timeout or not. */
+ toc = fsm->timeouts;
+ fsm->run.timeout_counters[i] = -1;
+ while (toc != NULL)
+ {
+ if (toc->timeout.trans->state->code
+ == fsm->run.active_states[i]->code)
+ {
+ fsm->run.timeout_counters[i] = toc->timeout.timeout;
+ break;
+ }
+ toc = toc->next;
+ }
+
+ handled = 1;
+ }
+ }
+ return handled;
+}
+
+int
+fsm_build_handle_string (fsm_build_t *fsm, char *event)
+{
+ fsm_build_event_t *e = fsm_build_get_event (fsm, event);
+ return fsm_build_handle (fsm, e);
+}
+
+int
+fsm_build_handle_integer (fsm_build_t *fsm, uint16_t event)
+{
+ fsm_build_event_t *e = fsm_build_get_event_by_code (fsm, event);
+ return fsm_build_handle (fsm, e);
+}
+
+int
+fsm_build_can_handle (fsm_build_t *fsm, fsm_build_event_t *e)
+{
+ fsm_build_state_t *s = NULL;
+ assert (e);
+ uint i;
+ for (i = 0; i < fsm->max_active_states; i++)
+ {
+ s = fsm->run.active_states[i];
+ if (s && fsm->run.trans_table[e->code][s->code])
+ return 1;
+ }
+ return 0;
+}
+
+int
+fsm_build_can_handle_string (fsm_build_t *fsm, char *event)
+{
+ fsm_build_event_t *e = fsm_build_get_event (fsm, event);
+ return fsm_build_can_handle (fsm, e);
+}
+
+int
+fsm_build_can_handle_integer (fsm_build_t *fsm, uint16_t event)
+{
+ fsm_build_event_t *e = fsm_build_get_event_by_code (fsm, event);
+ return fsm_build_can_handle (fsm, e);
+}
+
+int
+fsm_build_handle_timeout (fsm_build_t *fsm)
+{
+ int out = 0;
+ int i;
+ char *event = NULL;
+ for (i = 0; i < (int) fsm->max_active_states; i++)
+ {
+ if (fsm->run.timeout_counters[i] > 0)
+ fsm->run.timeout_counters[i]--;
+ /* We have a timeout event. */
+ if (fsm->run.timeout_counters[i] == 0)
+ {
+ fsm->run.timeout_counters[i] = -1;
+ /* build event string */
+ event = (char *) malloc (
+ (strlen (fsm->run.active_states[i]->var_name)
+ + strlen ("_TIMEOUT") + 1) * sizeof (char));
+ sprintf (event, "%s_TIMEOUT", fsm->run.active_states[i]->var_name);
+ fsm_build_handle_string (fsm, event);
+ out = 1;
+ }
+ }
+ return out;
+}
+
+fsm_build_state_t*
+fsm_build_get_next_state (fsm_build_t *fsm,
+ char *state,
+ char *event,
+ char *branch)
+{
+ fsm_build_state_t *s;
+ fsm_build_event_t *e;
+ fsm_build_trans_chain_t *t_curs;
+ fsm_build_branch_chain_t *b_curs;
+
+ /* Convert input data. */
+ s = fsm_build_get_state (fsm, state);
+ e = fsm_build_get_event (fsm, event);
+ assert (s && e);
+
+ /* Get transition. */
+ t_curs = fsm->trans;
+ while (t_curs != NULL)
+ {
+ if (s == t_curs->trans.state && e == t_curs->trans.event)
+ break;
+ t_curs = t_curs->next;
+ }
+ assert (t_curs);
+ assert (t_curs->trans.output_branches);
+
+ /* If we have only one branch. */
+ if (strlen (branch) == 0)
+ {
+ /* Branch has to be given is there are multiple branches. */
+ assert (t_curs->trans.output_branches->next == NULL);
+ fsm_build_print (fsm, &t_curs->trans, t_curs->trans.output_branches);
+ return t_curs->trans.output_branches->state;
+ }
+
+ /* Find correct branch. */
+ b_curs = t_curs->trans.output_branches;
+ while (b_curs != NULL)
+ {
+ if (strcmp (b_curs->name, branch) == 0)
+ break;
+ b_curs = b_curs->next;
+ }
+ assert (b_curs);
+ fsm_build_print (fsm, &t_curs->trans, b_curs);
+ return b_curs->state;
+}
+
+void
+fsm_build_gen_avr_h (fsm_build_t *fsm, uint embedded_strings)
+{
+ fsm_build_state_chain_t *sc;
+ fsm_build_event_chain_t *ec;
+ fsm_build_trans_chain_t *tc;
+ fsm_build_branch_chain_t *bc;
+ fsm_build_state_t *s;
+ fsm_build_event_t *e;
+ fsm_build_chain_t *all_fsm;
+ uint i, j;
+
+ /* Open file. */
+ char *fn = (char *) malloc ((strlen (fsm->name)
+ + strlen ("fsm_gen_.h") + 1) * sizeof (char));
+ sprintf (fn, "fsm_%s_gen.h", fsm->name);
+ FILE *f = fopen (fn, "w");
+
+ /* Introduction. */
+ fprintf (f, "/* This file has been generated, do not edit. */\n\n");
+ fprintf (f, "#ifndef _FSM_%s_\n", fsm->name);
+ fprintf (f, "#define _FSM_%s_\n\n", fsm->name);
+ fprintf (f, "#include <avr/pgmspace.h>\n");
+ fprintf (f, "#include <inttypes.h>\n\n");
+
+ /* Include all other fsm headers. */
+ all_fsm = fsm_build_all_fsm;
+ while (all_fsm != NULL)
+ {
+ fprintf (f, "#include \"fsm_%s_gen.h\"\n",all_fsm->fsm->name);
+ all_fsm = all_fsm->next;
+ }
+
+ /* Gen max active states define */
+ fprintf (f, "#define fsm_%s_max_active_states %u\n",
+ fsm->name,
+ fsm->max_active_states);
+
+ /* Gen state enum. */
+ fprintf (f, "typedef enum\n{\n");
+ sc = fsm->states;
+ while (sc != NULL)
+ {
+ fprintf (f, "\tFSM_STATE_%s_%s = %u,\n", fsm->name, sc->state.var_name, sc->state.code);
+ sc = sc->next;
+ }
+ fprintf (f, "\tFSM_STATE_%s_NB_ = %u\n", fsm->name, fsm->state_nb);
+ fprintf (f, "} fsm_%s_state_t;\n\n", fsm->name);
+
+ /* Gen event enum. */
+ fprintf (f, "typedef enum\n{\n");
+ ec = fsm->events;
+ while (ec != NULL)
+ {
+ fprintf (f, "\tFSM_EVENT_%s_%s = %u,\n", fsm->name, ec->event.var_name, ec->event.code);
+ ec = ec->next;
+ }
+ fprintf (f, "\tFSM_EVENT_%s_NB_ = %u\n", fsm->name, fsm->event_nb);
+ fprintf (f, "} fsm_%s_event_t;\n\n", fsm->name);
+
+ /* Gen state strings. */
+ if (embedded_strings)
+ {
+ sc = fsm->states;
+ while (sc != NULL)
+ {
+ fprintf (f, "extern prog_char fsm_%s_state_str_%s[%u] PROGMEM;\n",
+ fsm->name,
+ sc->state.var_name,
+ strlen (sc->state.var_name) + 1);
+ sc = sc->next;
+ }
+ fprintf (f, "extern const char *fsm_%s_state_str[%u] PROGMEM;\n\n",
+ fsm->name,
+ fsm->state_nb);
+
+ /* Gen event strings. */
+ ec = fsm->events;
+ while (ec != NULL)
+ {
+ fprintf (f, "extern prog_char fsm_%s_event_str_%s[%u] PROGMEM;\n",
+ fsm->name,
+ ec->event.var_name,
+ strlen (ec->event.var_name) + 1);
+ ec = ec->next;
+ }
+ fprintf (f, "extern const char *fsm_%s_event_str[%u] PROGMEM;\n\n",
+ fsm->name,
+ fsm->event_nb);
+
+ /* Create a RAM string able to store event or state string. */
+ j = 0;
+ for (i = 0; i < fsm->event_nb; i++)
+ {
+ e = fsm_build_get_event_by_code (fsm, i);
+ if (strlen (e->var_name) > j)
+ j = strlen (e->var_name);
+ }
+ for (i = 0; i < fsm->state_nb; i++)
+ {
+ s = fsm_build_get_state_by_code (fsm, i);
+ if (strlen (s->var_name) > j)
+ j = strlen (s->var_name);
+ }
+ fprintf (f, "extern char fsm_%s_str_buff[%u];\n", fsm->name, j + 1);
+
+ /* Convert an event enum in string. */
+ fprintf (f, "char *\nfsm_%s_get_event_string_from_enum \
+ (fsm_%s_event_t e);\n", fsm->name, fsm->name);
+
+ /* Convert a event string in enum. */
+ fprintf (f, "fsm_%s_event_t\nfsm_%s_get_event_enum_from_string \
+ (char *str);\n", fsm->name, fsm->name);
+
+ /* Convert an state enum in string. */
+ fprintf (f, "char *\nfsm_%s_get_state_string_from_enum \
+ (fsm_%s_state_t s);\n", fsm->name, fsm->name);
+
+ /* Convert a state string in enum. */
+ fprintf (f, "fsm_%s_state_t\nfsm_%s_get_state_enum_from_string \
+ (char *str);\n", fsm->name, fsm->name);
+ }
+
+ /* Gen transitions branches enum. */
+ fprintf (f, "typedef enum\n{\n");
+ tc = fsm->trans;
+ while (tc != NULL)
+ {
+ bc = tc->trans.output_branches;
+ while (bc != NULL)
+ {
+ if (bc->name != NULL)
+ fprintf (f, "\tFSM_BRANCH_%s_%s_%s_%s = %u,\n",
+ fsm->name,
+ tc->trans.state->var_name,
+ tc->trans.event->var_name,
+ bc->name,
+ bc->state->code);
+ else
+ fprintf (f, "\tFSM_BRANCH_%s_%s_%s_ = %u,\n",
+ fsm->name,
+ tc->trans.state->var_name,
+ tc->trans.event->var_name,
+ bc->state->code);
+ bc = bc->next;
+ }
+ tc = tc->next;
+ }
+ fprintf (f, "} fsm_%s_branch_t;\n\n", fsm->name);
+
+ /* Gen function headers. */
+ tc = fsm->trans;
+ while (tc != NULL)
+ {
+ fprintf (f, "fsm_%s_branch_t fsm_%s_trans_func_%s_%s (void);\n",
+ fsm->name,
+ fsm->name,
+ tc->trans.state->var_name,
+ tc->trans.event->var_name);
+ tc = tc->next;
+ }
+ fprintf (f, "\n");
+
+ /* Gen function table. */
+ fprintf (f, "typedef fsm_%s_branch_t (*fsm_%s_func_t)(void);\n", fsm->name,
+ fsm->name);
+ fprintf (f, "extern const fsm_%s_func_t PROGMEM fsm_%s_trans_table[%u][%u];\n\n",
+ fsm->name,
+ fsm->name,
+ fsm->event_nb,
+ fsm->state_nb);
+
+ /* Gen active states array. */
+ fprintf (f, "extern fsm_%s_state_t fsm_%s_active_states[%u];\n\n",
+ fsm->name,
+ fsm->name,
+ fsm->max_active_states);
+
+ /* Gen initialization function. */
+ sc = fsm->starters;
+ i = 0;
+ fprintf (f, "void\nfsm_%s_init () __attribute__ ((constructor));\n\n",
+ fsm->name);
+
+ /* Gen handle function. */
+ fprintf (f, "int\nfsm_%s_handle (fsm_%s_event_t e);\n",
+ fsm->name,
+ fsm->name);
+
+ /* Gen can handle function. */
+ fprintf (f, "uint16_t\nfsm_%s_can_handle (fsm_%s_event_t e);\n\n",
+ fsm->name,
+ fsm->name);
+
+ if (fsm->timeouts != NULL)
+ {
+ /* Gen handle timeout function. */
+ fprintf (f, "int\nfsm_%s_handle_timeout ();\n",
+ fsm->name);
+
+ /* Gen timeout values. */
+ fprintf (f, "extern int32_t fsm_%s_timeout_values[FSM_STATE_%s_NB_];\n",
+ fsm->name,
+ fsm->name);
+
+ /* Gen timeout corresponding events. */
+ fprintf (f, "extern fsm_%s_event_t fsm_%s_timeout_events[FSM_STATE_%s_NB_];\n",
+ fsm->name,
+ fsm->name,
+ fsm->name);
+
+ /* Gen timeout counters array. */
+ fprintf (f, "extern int32_t fsm_%s_timeout_counters[%u];\n\n",
+ fsm->name,
+ fsm->max_active_states);
+ }
+
+ /* Conclusion. */
+ fprintf (f, "#endif /* #ifndef _FSM_%s_ */", fsm->name),
+
+ /* Close file. */
+ fclose (f);
+
+ /* Free. */
+ free (fn);
+}
+
+void
+fsm_build_gen_avr_c (fsm_build_t *fsm, uint embedded_strings)
+{
+ fsm_build_state_chain_t *sc;
+ fsm_build_event_chain_t *ec;
+ fsm_build_trans_chain_t *tc;
+ fsm_build_timeout_chain_t *toc;
+ fsm_build_state_t *s;
+ fsm_build_event_t *e;
+ uint i, j, found;
+
+ /* Open file. */
+ char *fn = (char*) malloc ((strlen (fsm->name)
+ + strlen ("fsm_gen_.c") + 1) * sizeof (char));
+ sprintf (fn, "fsm_%s_gen.c", fsm->name);
+ FILE *f = fopen (fn, "w");
+
+ /* Introduction. */
+ fprintf (f, "/* This file has been generated, do not edit. */\n\n");
+ fprintf (f, "#include \"fsm_%s_gen.h\"\n\n", fsm->name);
+
+ /* Gen state strings. */
+ if (embedded_strings)
+ {
+ sc = fsm->states;
+ while (sc != NULL)
+ {
+ fprintf (f, "prog_char fsm_%s_state_str_%s[] PROGMEM = \"%s\";\n",
+ fsm->name,
+ sc->state.var_name,
+ sc->state.var_name);
+ sc = sc->next;
+ }
+ fprintf (f, "const char *fsm_%s_state_str[] PROGMEM =\n{\n", fsm->name);
+ for (i = 0; i < fsm->state_nb; i++)
+ {
+ s = fsm_build_get_state_by_code (fsm, i);
+ fprintf (f, "\tfsm_%s_state_str_%s", fsm->name, s->var_name);
+ if (i == fsm->state_nb - 1)
+ fprintf (f, "\n");
+ else
+ fprintf (f, ",\n");
+ }
+ fprintf (f, "};\n\n");
+
+ /* Gen event strings. */
+ ec = fsm->events;
+ while (ec != NULL)
+ {
+ fprintf (f, "prog_char fsm_%s_event_str_%s[] PROGMEM = \"%s\";\n",
+ fsm->name,
+ ec->event.var_name,
+ ec->event.var_name);
+ ec = ec->next;
+ }
+ fprintf (f, "const char *fsm_%s_event_str[] PROGMEM =\n{\n", fsm->name);
+ for (i = 0; i < fsm->event_nb; i++)
+ {
+ e = fsm_build_get_event_by_code (fsm, i);
+ fprintf (f, "\tfsm_%s_event_str_%s", fsm->name, e->var_name);
+ if (i == fsm->event_nb - 1)
+ fprintf (f, "\n");
+ else
+ fprintf (f, ",\n");
+ }
+ fprintf (f, "};\n\n");
+
+ /* Create a RAM string able to store event or state string. */
+ j = 0;
+ for (i = 0; i < fsm->event_nb; i++)
+ {
+ e = fsm_build_get_event_by_code (fsm, i);
+ if (strlen (e->var_name) > j)
+ j = strlen (e->var_name);
+ }
+ for (i = 0; i < fsm->state_nb; i++)
+ {
+ s = fsm_build_get_state_by_code (fsm, i);
+ if (strlen (s->var_name) > j)
+ j = strlen (s->var_name);
+ }
+ fprintf (f, "char fsm_%s_str_buff[%u];\n", fsm->name, j + 1);
+
+ /* Convert an event enum in string. */
+ fprintf (f, "char *\nfsm_%s_get_event_string_from_enum \
+ (fsm_%s_event_t e)\n{\n", fsm->name, fsm->name);
+ fprintf (f, "\treturn strcpy_P (fsm_%s_str_buff, \
+ (char *) pgm_read_word (&(fsm_%s_event_str[e])));\n", fsm->name, fsm->name);
+ fprintf (f, "}\n\n");
+
+ /* Convert a event string in enum. */
+ fprintf (f, "fsm_%s_event_t\nfsm_%s_get_event_enum_from_string \
+ (char *str)\n{\n", fsm->name, fsm->name);
+ fprintf (f, "\tuint16_t i;\n");
+ fprintf (f, "\tfor (i = 0; i < FSM_EVENT_%s_NB_; i++)\n", fsm->name);
+ fprintf (f, "\t\tif (strcpy_P (str, \
+ (char *) pgm_read_word (&(fsm_%s_event_str[i]))) == 0)\n", fsm->name);
+ fprintf (f, "\t\t\treturn i;\n");
+ fprintf (f, "\treturn FSM_EVENT_%s_NB_;\n", fsm->name);
+ fprintf (f, "}\n\n");
+
+ /* Convert an state enum in string. */
+ fprintf (f, "char *\nfsm_%s_get_state_string_from_enum \
+ (fsm_%s_state_t s)\n{\n", fsm->name, fsm->name);
+ fprintf (f, "\treturn strcpy_P (fsm_%s_str_buff, \
+ (char *) pgm_read_word (&(fsm_%s_state_str[s])));\n", fsm->name, fsm->name);
+ fprintf (f, "}\n\n");
+
+ /* Convert a state string in enum. */
+ fprintf (f, "fsm_%s_state_t\nfsm_%s_get_state_enum_from_string \
+ (char *str)\n{\n", fsm->name, fsm->name);
+ fprintf (f, "\tuint16_t i;\n");
+ fprintf (f, "\tfor (i = 0; i < FSM_STATE_%s_NB_; i++)\n", fsm->name);
+ fprintf (f, "\t\tif (strcpy_P (str, \
+ (char *) pgm_read_word (&(fsm_%s_state_str[i]))) == 0)\n", fsm->name);
+ fprintf (f, "\t\t\treturn i;\n");
+ fprintf (f, "\treturn FSM_STATE_%s_NB_;\n", fsm->name);
+ fprintf (f, "}\n\n");
+ }
+
+ /* Gen function table. */
+ fprintf (f, "const fsm_%s_func_t PROGMEM fsm_%s_trans_table[%u][%u] = \n{\n",
+ fsm->name,
+ fsm->name,
+ fsm->event_nb,
+ fsm->state_nb);
+ /* for each events and state, see if it exists an associated transition. */
+ for (i = 0; i < fsm->event_nb; i++)
+ {
+ e = fsm_build_get_event_by_code (fsm, i);
+ fprintf (f, "\t{");
+ for (j = 0; j < fsm->state_nb; j++)
+ {
+ s = fsm_build_get_state_by_code (fsm, j);
+ tc = fsm->trans;
+ found = 0;
+ while (tc != NULL)
+ {
+ if (tc->trans.state == s && tc->trans.event == e)
+ {
+ found = 1;
+ fprintf (f, "&fsm_%s_trans_func_%s_%s",
+ fsm->name,
+ tc->trans.state->var_name,
+ tc->trans.event->var_name);
+ tc = tc->next;
+ break;
+ }
+ tc = tc->next;
+ }
+ if (!found)
+ fprintf (f, "(fsm_%s_func_t) 0", fsm->name);
+ if (j == fsm->state_nb - 1)
+ fprintf (f, "}");
+ else
+ fprintf (f, ", ");
+ }
+ if (i != fsm->event_nb - 1)
+ fprintf (f, ",");
+ fprintf (f, "\n");
+ }
+ fprintf (f, "};\n\n");
+
+ /* Gen active states array. */
+ fprintf (f, "fsm_%s_state_t fsm_%s_active_states[%u];\n\n",
+ fsm->name,
+ fsm->name,
+ fsm->max_active_states);
+
+ /* Gen initialization function. */
+ sc = fsm->starters;
+ i = 0;
+ fprintf (f, "void\nfsm_%s_init ()\n{\n", fsm->name);
+ while (sc != NULL)
+ {
+ fprintf (f, "\tfsm_%s_active_states[%u] = %u;\n",
+ fsm->name,
+ i,
+ sc->state.code);
+ if (fsm->timeouts != NULL)
+ {
+ toc = fsm->timeouts;
+ while (toc != NULL)
+ {
+ if (toc->timeout.trans->state->code == sc->state.code)
+ {
+ fprintf (f, "\tfsm_%s_timeout_counters[%u] = %u;\n",
+ fsm->name,
+ i,
+ toc->timeout.timeout);
+ }
+ toc = toc->next;
+ }
+ }
+ i++;
+ sc = sc->next;
+ }
+ fprintf (f, "}\n\n");
+
+ /* Gen handle function. */
+ fprintf (f, "int\nfsm_%s_handle (fsm_%s_event_t e)\n{\n",
+ fsm->name,
+ fsm->name);
+ fprintf (f, "\tuint16_t i;\n");
+ fprintf (f, "\tint handled = 0;\n");
+ fprintf (f, "\tfor (i = 0; i < fsm_%s_max_active_states; i++)\n\t{\n",
+ fsm->name);
+ fprintf (f, "\t\tif (fsm_%s_trans_table[e][fsm_%s_active_states[i]])\n",
+ fsm->name,
+ fsm->name);
+ fprintf (f, "\t\t{\n");
+ fprintf (f, "\t\t\tfsm_%s_active_states[i] = \
+ fsm_%s_trans_table[e][fsm_%s_active_states[i]]();\n",
+ fsm->name,
+ fsm->name,
+ fsm->name);
+ fprintf (f, "\t\t\thandled = 1;\n");
+ if (fsm->timeouts != NULL)
+ {
+ fprintf (f, "\t\t\tfsm_%s_timeout_counters[i] = fsm_%s_timeout_values[e];\n",
+ fsm->name,
+ fsm->name);
+ }
+ fprintf (f, "\t\t}\n");
+ fprintf (f, "\t}\n");
+ fprintf (f, "\treturn handled;\n");
+ fprintf (f, "}\n\n");
+
+ /* Gen can handle function. */
+ fprintf (f, "uint16_t\nfsm_%s_can_handle (fsm_%s_event_t e)\n{\n",
+ fsm->name,
+ fsm->name);
+ fprintf (f, "\tuint16_t i;\n");
+ fprintf (f, "\tfor (i = 0; i < fsm_%s_max_active_states; i++)\n",
+ fsm->name);
+ fprintf (f, "\t\tif (fsm_%s_trans_table[e][fsm_%s_active_states[i]])\n",
+ fsm->name,
+ fsm->name);
+ fprintf (f, "\t\t\treturn 1;\n");
+ fprintf (f, "\treturn 0;\n");
+ fprintf (f, "}\n\n");
+
+ if (fsm->timeouts != NULL)
+ {
+ /* Gen timeout counters array. */
+ fprintf (f, "int32_t fsm_%s_timeout_counters[%u];\n",
+ fsm->name,
+ fsm->max_active_states);
+
+ /* Gen timeout values array. */
+ fprintf (f, "int32_t fsm_%s_timeout_values[FSM_STATE_%s_NB_] =\n{\n",
+ fsm->name,
+ fsm->name);
+ int value;
+ for (i = 0; i < fsm->state_nb; i++)
+ {
+ value = -1;
+ s = fsm_build_get_state_by_code (fsm, i);
+ assert (s);
+ toc = fsm->timeouts;
+ while (toc != NULL)
+ {
+ if (s->code == toc->timeout.trans->state->code)
+ {
+ value = toc->timeout.timeout;
+ break;
+ }
+ toc = toc->next;
+ }
+ fprintf (f, "\t%i", value);
+ if (i != fsm->state_nb - 1)
+ fprintf (f, ",");
+ fprintf (f, "\n");
+ }
+ fprintf (f, "};\n\n");
+
+ /* Gen timeout corresponding events array. */
+ fprintf (f, "fsm_%s_event_t fsm_%s_timeout_events[FSM_STATE_%s_NB_] =\n{\n",
+ fsm->name,
+ fsm->name,
+ fsm->name);
+ for (i = 0; i < fsm->state_nb; i++)
+ {
+ value = -1;
+ s = fsm_build_get_state_by_code (fsm, i);
+ assert (s);
+ toc = fsm->timeouts;
+ while (toc != NULL)
+ {
+ if (s->code == toc->timeout.trans->state->code)
+ {
+ value = toc->timeout.trans->event->code;
+ break;
+ }
+ toc = toc->next;
+ }
+ if (value == -1)
+ fprintf (f, "\tFSM_STATE_%s_NB_", fsm->name);
+ else
+ fprintf (f, "\t%u", value);
+
+ if (i != fsm->state_nb - 1)
+ fprintf (f, ",");
+ fprintf (f, "\n");
+ }
+ fprintf (f, "};\n\n");
+
+ /* Gen handle timeout function. */
+ fprintf (f, "int\nfsm_%s_handle_timeout ()\n{\n",
+ fsm->name);
+ fprintf (f, "\tuint16_t i;\n");
+ fprintf (f, "\tint out = 0;\n");
+ fprintf (f, "\tfor (i = 0; i < fsm_%s_max_active_states; i++)\n\t{\n",
+ fsm->name);
+ fprintf (f, "\t\tif (fsm_%s_timeout_counters[i] > 0)\n",
+ fsm->name);
+ fprintf (f, "\t\t\tfsm_%s_timeout_counters[i]--;\n",
+ fsm->name);
+ fprintf (f, "\t\tif (fsm_%s_timeout_counters[i] == 0)\n\t\t{\n",
+ fsm->name);
+ fprintf (f, "\t\t\tfsm_%s_handle (fsm_%s_timeout_events[fsm_%s_active_states[i]]);\n",
+ fsm->name,
+ fsm->name,
+ fsm->name);
+ fprintf (f, "\t\t\tout = 1;\n");
+ fprintf (f, "\t\t}\n\n");
+ fprintf (f, "\t}\n");
+ fprintf (f, "\treturn out;\n");
+ fprintf (f, "}\n\n");
+ }
+
+ /* Close file. */
+ fclose (f);
+
+ /* Free. */
+ free (fn);
+}
+
+void
+fsm_build_gen (char *arch, uint embedded_strings)
+{
+ fsm_build_chain_t *curs = fsm_build_all_fsm;
+ while (curs != NULL)
+ {
+ if (strcmp (arch, "AVR") == 0)
+ {
+ fsm_build_gen_avr_h (curs->fsm, embedded_strings);
+ fsm_build_gen_avr_c (curs->fsm, embedded_strings);
+ }
+ curs = curs->next;
+ }
+}
+
+void
+fsm_build_free (fsm_build_t *fsm)
+{
+ fsm_build_state_chain_t *sc;
+ fsm_build_state_chain_t *sc_tmp;
+ fsm_build_event_chain_t *ec;
+ fsm_build_event_chain_t *ec_tmp;
+ fsm_build_trans_chain_t *tc;
+ fsm_build_trans_chain_t *tc_tmp;
+ fsm_build_branch_chain_t *bc;
+ fsm_build_branch_chain_t *bc_tmp;
+ fsm_trans_func_chain_t *fc;
+ fsm_trans_func_chain_t *fc_tmp;
+ fsm_build_timeout_chain_t *toc;
+ fsm_build_timeout_chain_t *toc_tmp;
+ uint i;
+
+ /* Free states. */
+ sc = fsm->states;
+ while (sc != NULL)
+ {
+ sc_tmp = sc;
+ free (sc->state.var_name);
+ sc = sc->next;
+ free (sc_tmp);
+ }
+
+ /* Free events. */
+ ec = fsm->events;
+ while (sc != NULL)
+ {
+ ec_tmp = ec;
+ free (ec->event.var_name);
+ ec = ec->next;
+ free (ec_tmp);
+ }
+
+ /* Free trans */
+ tc = fsm->trans;
+ while (tc != NULL)
+ {
+ tc_tmp = tc;
+
+ /* Free each branches. */
+ bc = tc->trans.output_branches;
+ while (bc != NULL)
+ {
+ bc_tmp = bc;
+ free (bc->name);
+ bc = bc->next;
+ free (bc_tmp);
+ }
+
+ tc = tc->next;
+ free (tc_tmp);
+ }
+
+ /* Free start chain. */
+ sc = fsm->starters;
+ while (sc != NULL)
+ {
+ sc_tmp = sc;
+ sc = sc->next;
+ free (sc_tmp);
+ }
+
+ /* Free timeout chain. */
+ toc = fsm->timeouts;
+ while (toc != NULL)
+ {
+ toc_tmp = toc;
+ toc = toc->next;
+ free (toc_tmp);
+ }
+
+ /* Free run data (trans_table). */
+ for (i = 0; i < fsm->event_nb; i++)
+ free (fsm->run.trans_table[i]);
+ free (fsm->run.trans_table);
+
+ /* Free run data (active states). */
+ free (fsm->run.active_states);
+
+ /* Free last-seen event array*/
+ free (fsm->run.events_before_active_state);
+
+ /* Free run data (function pool). */
+ fc = fsm->run.func_pool;
+ while (fc != NULL)
+ {
+ fc_tmp = fc;
+ fc = fc->next;
+ free (fc_tmp);
+ }
+
+ /*Free run data (timeout counters). */
+ free (fsm->run.timeout_counters);
+}
+
diff --git a/digital/ai/src/fsm/fsm_queue.c b/digital/ai/src/fsm/fsm_queue.c
new file mode 100644
index 00000000..357fc39e
--- /dev/null
+++ b/digital/ai/src/fsm/fsm_queue.c
@@ -0,0 +1,71 @@
+/* fsm_queue.c */
+/* ai - Robot Artificial Intelligence. {{{
+ *
+ * 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 "fsm_queue.h"
+
+/** Maximum number of events in post queue. */
+#define FSM_QUEUE_SIZE 8
+
+/** Module context. */
+struct fsm_queue_t
+{
+ /** Events to post to the FSM in next iteration. */
+ uint8_t events[FSM_QUEUE_SIZE];
+ /** First event in the queue. */
+ uint8_t head;
+ /** Number of events in the queue. */
+ uint8_t nb;
+};
+
+/** Global context. */
+static struct fsm_queue_t fsm_queue_global;
+# define ctx fsm_queue_global
+
+void
+fsm_queue_post_event (uint8_t event)
+{
+ assert (ctx.nb < FSM_QUEUE_SIZE);
+ uint8_t tail = (ctx.head + ctx.nb) % FSM_QUEUE_SIZE;
+ ctx.events[tail] = event;
+ ctx.nb++;
+}
+
+uint8_t
+fsm_queue_poll (void)
+{
+ return ctx.nb;
+}
+
+uint8_t
+fsm_queue_pop_event (void)
+{
+ uint8_t e;
+ assert (ctx.nb > 0);
+ e = ctx.events[ctx.head];
+ ctx.nb--;
+ ctx.head = (ctx.head + 1) % FSM_QUEUE_SIZE;
+ return e;
+}
+
diff --git a/digital/ai/src/fsm/fsm_queue.h b/digital/ai/src/fsm/fsm_queue.h
new file mode 100644
index 00000000..ce230bec
--- /dev/null
+++ b/digital/ai/src/fsm/fsm_queue.h
@@ -0,0 +1,41 @@
+#ifndef fsm_queue_h
+#define fsm_queue_h
+/* fsm_queue.h */
+/* ai - Robot Artificial Intelligence. {{{
+ *
+ * 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.
+ *
+ * }}} */
+
+/** Post an event to be processed in the next main loop. */
+void
+fsm_queue_post_event (uint8_t event);
+
+/** Poll for event in the event queue (return non zero if there is an event
+ * pending). */
+uint8_t
+fsm_queue_poll (void);
+
+/** Pop one event from the event queue. */
+uint8_t
+fsm_queue_pop_event (void);
+
+#endif /* fsm_queue_h */
diff --git a/digital/io/src/asserv.c b/digital/ai/src/twi_master/asserv.c
index e9adcbbc..d066737b 100644
--- a/digital/io/src/asserv.c
+++ b/digital/ai/src/twi_master/asserv.c
@@ -1,5 +1,5 @@
/* asserv.c */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
+/* ai - Robot Artificial Intelligence. {{{
*
* Copyright (C) 2008 Dufour Jérémy
*
@@ -27,18 +27,12 @@
#include "twi_master.h"
-#include "modules/utils/byte.h" /* v*_to_v* */
+#include "modules/utils/byte.h"
#include "modules/math/fixed/fixed.h"
#include "bot.h"
#include "io.h"
/**
- * @defgroup AsservPrivate Asserv module private variables and functions
- * declarations and definitions
- * @{
- */
-
-/**
* Flag bit position value for the status byte of the asserv.
*/
enum asserv_status_flag_e
@@ -106,6 +100,16 @@ typedef struct asserv_struct_s
*/
asserv_struct_s asserv_status;
+/** Set scale.
+ * @param scale number of millimeter per step (f8.24).
+ */
+static void
+asserv_set_scale (uint32_t scale)
+{
+ asserv_scale = scale;
+ asserv_scale_inv = fixed_div_f824 (1L << 24, scale);
+}
+
void
asserv_init (void)
{
@@ -408,13 +412,6 @@ asserv_goto (uint32_t x, uint32_t y, uint8_t backward)
}
void
-asserv_set_scale (uint32_t scale)
-{
- asserv_scale = scale;
- asserv_scale_inv = fixed_div_f824 (1L << 24, scale);
-}
-
-void
asserv_motor0_zero_position (int8_t speed)
{
uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
diff --git a/digital/io/src/asserv.h b/digital/ai/src/twi_master/asserv.h
index a92d95f7..0d29ba96 100644
--- a/digital/io/src/asserv.h
+++ b/digital/ai/src/twi_master/asserv.h
@@ -1,7 +1,7 @@
#ifndef asserv_h
#define asserv_h
/* asserv.h */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
+/* ai - Robot Artificial Intelligence. {{{
*
* Copyright (C) 2008 Dufour Jérémy
*
@@ -27,10 +27,9 @@
#include "defs.h"
/**
- * @file Control the asserv board from io using the TWI protocol.
+ * @file Control the asserv board using the TWI protocol.
* This files contains the 'public' functions to send commands to the asserv
* board using a protocol over TWI communication.
- * @see trunk/digital/io/doc/proto_asserv.txt
*/
/** Slave number in twi_master list. */
@@ -67,7 +66,7 @@ typedef enum asserv_status_e
none,
/** The command has succeed. */
success,
- /** The command has failed. The bot or the motor0 is blocked */
+ /** The command has failed. The bot or the motor is blocked */
failure
} asserv_status_e;
@@ -176,7 +175,7 @@ asserv_move_angularly (int16_t angle);
/**
* Make the bot turn of an absolute angle.
* The angle is absolute and not a difference with the current one.
- * @param a the absolute angle in degrees
+ * @param a the absolute angle
*/
void
asserv_goto_angle (int16_t angle);
@@ -279,12 +278,6 @@ asserv_set_position (int32_t x, int32_t y, int16_t angle);
void
asserv_goto (uint32_t x, uint32_t y, uint8_t backward);
-/** Set scale.
- * @param scale number of millimeter per step (f8.24).
- */
-void
-asserv_set_scale (uint32_t scale);
-
/** Reset the motor0 to the zero position. */
void
asserv_motor0_zero_position (int8_t speed);
diff --git a/digital/io/src/mimot.c b/digital/ai/src/twi_master/mimot.c
index ba09a3f5..af6ff5f9 100644
--- a/digital/io/src/mimot.c
+++ b/digital/ai/src/twi_master/mimot.c
@@ -1,5 +1,5 @@
/* mimot.c */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
+/* ai - Robot Artificial Intelligence. {{{
*
* Copyright (C) 2010 Nicolas Schodet
*
diff --git a/digital/io/src/mimot.h b/digital/ai/src/twi_master/mimot.h
index 98709162..60811c10 100644
--- a/digital/io/src/mimot.h
+++ b/digital/ai/src/twi_master/mimot.h
@@ -1,7 +1,7 @@
#ifndef mimot_h
#define mimot_h
/* mimot.h */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
+/* ai - Robot Artificial Intelligence. {{{
*
* Copyright (C) 2010 Nicolas Schodet
*
diff --git a/digital/io/src/twi_master.c b/digital/ai/src/twi_master/twi_master.c
index cb600da8..90977fd3 100644
--- a/digital/io/src/twi_master.c
+++ b/digital/ai/src/twi_master/twi_master.c
@@ -1,5 +1,5 @@
/* twi_master.c */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
+/* ai - Robot Artificial Intelligence. {{{
*
* Copyright (C) 2010 Nicolas Schodet
*
diff --git a/digital/io/src/twi_master.h b/digital/ai/src/twi_master/twi_master.h
index e7cb9d86..e75dbfab 100644
--- a/digital/io/src/twi_master.h
+++ b/digital/ai/src/twi_master/twi_master.h
@@ -1,7 +1,7 @@
#ifndef twi_master_h
#define twi_master_h
/* twi_master.h */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
+/* ai - Robot Artificial Intelligence. {{{
*
* Copyright (C) 2010 Nicolas Schodet
*
diff --git a/digital/io/src/chrono.c b/digital/ai/src/utils/chrono.c
index b73b627c..a1212e19 100644
--- a/digital/io/src/chrono.c
+++ b/digital/ai/src/utils/chrono.c
@@ -1,5 +1,5 @@
/* chrono.c */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
+/* ai - Robot Artificial Intelligence. {{{
*
* Copyright (C) 2008 Dufour Jérémy
*
@@ -22,11 +22,9 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
-
#include "common.h"
-#include "bot.h"
-#include "main_timer.h"
+#include "timer.h"
#include "asserv.h"
#include "twi_master.h"
@@ -43,13 +41,9 @@
* match is over (chrono_is_match_over, chrono_end_match).
*/
-/**
- * Number of overflows of the timer/counter 0 to wait before the match is
- * over.
- * Basically, it is match_duration / timer_counter_0_overflow_duration.
- * Minus one is here for safety reason (because rounding is done at plus one).
- */
-#define CHRONO_MATCH_OVERFLOW_COUNT (MATCH_DURATION_MS / MT_TC0_PERIOD - 1)
+/** Number of overflows of the timer to wait before the match is over. */
+#define CHRONO_MATCH_OVERFLOW_COUNT \
+ (CHRONO_MATCH_DURATION_MS / TIMER_PERIOD_MS)
/**
* Duration of a loop to emulate from the original behaviour, in ms.
@@ -121,7 +115,7 @@ chrono_enabled (void)
uint32_t
chrono_remaining_time (void)
{
- return chrono_ov_count_ * MT_TC0_PERIOD;
+ return chrono_ov_count_ * TIMER_PERIOD_MS;
}
void
diff --git a/digital/io/src/chrono.h b/digital/ai/src/utils/chrono.h
index e2b09ac8..38bf3d61 100644
--- a/digital/io/src/chrono.h
+++ b/digital/ai/src/utils/chrono.h
@@ -1,7 +1,7 @@
#ifndef chrono_h
#define chrono_h
/* chrono.h */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
+/* ai - Robot Artificial Intelligence. {{{
*
* Copyright (C) 2008 Dufour Jérémy
*
@@ -28,16 +28,18 @@
/**
* @file Module to manage the chrono responsible to stop the bot after 90s.
*
- * It is based on the main timer (time/counter 0) to know when to stop the
- * bot.
+ * It is based on the main timer to know when to stop the bot.
*
* The main loop should never last more than the 4.44ms defined, otherwise,
* this module will not be precise at all!
*/
+/** Duration of a match in milliseconds, with margin. */
+#define CHRONO_MATCH_DURATION_MS (90000 - 2500)
+
/**
* Initialize the chrono module.
- * It setups it for a duration of MATCH_DURATION_MS.
+ * It setups it for a duration of CHRONO_MATCH_DURATION_MS.
*/
void
chrono_init (void);
diff --git a/digital/io/src/main_timer.avr.c b/digital/ai/src/utils/timer.avr.c
index bcb185b6..63aa7bdc 100644
--- a/digital/io/src/main_timer.avr.c
+++ b/digital/ai/src/utils/timer.avr.c
@@ -1,5 +1,5 @@
-/* main_timer.avr.c */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
+/* timer.avr.c */
+/* ai - Robot Artificial Intelligence. {{{
*
* Copyright (C) 2009 Dufour Jérémy
*
@@ -22,39 +22,47 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
-
#include "common.h"
-#include "main_timer.h"
+#include "timer.h"
#include "modules/utils/utils.h"
#include "io.h"
void
-main_timer_init (void)
+timer_init (void)
{
/* Configuration of the timer/counter 0:
* - top = 0xff,
* - prescaler = 256,
* -> Fov = F_io / (prescaler * (TOP + 1))
- * -> Tov = 1 / Fov = 4.444 ms.
+ * -> Tov = 1 / Fov
* Note: if you change the TCCR0 register value, please also update
- * MT_TC0_PRESCALER and MT_TC0_TOP. */
+ * TIMER_TC0_PRESCALER and TIMER_TC0_TOP. */
+#ifdef TCCR0
+# define TIFR_reg TIFR
TCCR0 = regv (FOC0, WGM00, COM01, COM0, WGM01, CS02, CS01, CS00,
0, 0, 0, 0, 0, 1, 1, 0);
+#else
+# define TIFR_reg TIFR0
+ TCCR0A = regv (COM0A1, COM0A0, COM0B1, COM0B0, 3, 2, WGM01, WGM00,
+ 0, 0, 0, 0, 0, 0, 0, 0);
+ TCCR0B = regv (FOC0A, FOC0B, 5, 4, WGM02, CS02, CS01, CS00,
+ 0, 0, 0, 0, 0, 1, 0, 0);
+#endif
}
uint8_t
-main_timer_wait (void)
+timer_wait (void)
{
/* Let's pretend we have reached overflow before calling this function. */
uint8_t count_before_ov = 1;
/* Loop until an overflow of the timer occurs. */
- while (!(TIFR & _BV (TOV0)))
+ while (!(TIFR_reg & _BV (TOV0)))
/* We have not reached overflow. */
count_before_ov = 0;
/* Write 1 to clear overflow. */
- TIFR = _BV (TOV0);
-
+ TIFR_reg = _BV (TOV0);
return count_before_ov;
}
+
diff --git a/digital/io/src/main_timer.h b/digital/ai/src/utils/timer.h
index b04c1775..e085b86e 100644
--- a/digital/io/src/main_timer.h
+++ b/digital/ai/src/utils/timer.h
@@ -1,7 +1,7 @@
-#ifndef main_timer_h
-#define main_timer_h
-/* main_timer.h */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
+#ifndef timer_h
+#define timer_h
+/* timer.h */
+/* ai - Robot Artificial Intelligence. {{{
*
* Copyright (C) 2009 Dufour Jérémy
*
@@ -26,14 +26,7 @@
* }}} */
/**
- * @file Main timer.
- * The main timer is responsible for making the main loop executed regularly
- * to a specific time (at least).
- *
- * Sometimes, the main loop can be executed less frequently that we want
- * because it takes too much time! This is bad and should be avoided.
- *
- * The main timer used the timer/counter 0 of the AVR.
+ * Tick timer, used to slow down the main loop to a given rate.
*/
/**
@@ -41,28 +34,25 @@
* If you want to change this value, you also need to change the TCCR0
* register.
*/
-#define MT_TC0_PRESCALER 256L
+#define TIMER_TC0_PRESCALER 256L
+
/**
* Top configured for timer/counter 0.
* If you want to change this value, you also need to change the TCCR0
* register.
*/
-#define MT_TC0_TOP 255
-/**
- * Period of timer/counter 0 (in millisecond).
- */
-#define MT_TC0_PERIOD \
- (1000.0 / (AC_FREQ / (MT_TC0_PRESCALER * (MT_TC0_TOP + 1))))
+#define TIMER_TC0_TOP 255
-/**
- * Initialize the main timer to 4.444 ms.
- * This function setup the timer/counter 0 configuration register.
- */
+/** Period of timer, ms. */
+#define TIMER_PERIOD_MS \
+ (1000.0 / (AC_FREQ / (TIMER_TC0_PRESCALER * (TIMER_TC0_TOP + 1))))
+
+/** Initialise timer. */
void
-main_timer_init (void);
+timer_init (void);
/**
- * Wait until the main timer overflows.
+ * Wait until the timer overflows.
* @return
* - 0 if we are on time (we have not reached overflow before calling this
* function).
@@ -73,6 +63,6 @@ main_timer_init (void);
* correctly!
*/
uint8_t
-main_timer_wait (void);
+timer_wait (void);
-#endif /* main_timer_h */
+#endif /* timer_h */
diff --git a/digital/ai/tools/marcel.py b/digital/ai/tools/marcel.py
new file mode 100644
index 00000000..08314ca3
--- /dev/null
+++ b/digital/ai/tools/marcel.py
@@ -0,0 +1,39 @@
+import simu.model.table_eurobot2010
+import simu.view.table_eurobot2010
+
+import simu.robots.marcel.link.bag
+import simu.robots.marcel.model.bag
+import simu.robots.marcel.view.bag
+
+import asserv
+import asserv.init
+import mimot
+import mimot.init
+import io
+import io.init
+
+from proto.popen_io import PopenIO
+import math
+
+class Robot:
+
+ def __init__ (self, proto_time):
+ self.table_model = simu.model.table_eurobot2010
+ self.table_view = simu.view.table_eurobot2010
+ self.robot_link = simu.robots.marcel.link.bag
+ self.robot_model = simu.robots.marcel.model.bag
+ self.robot_view = simu.robots.marcel.view.bag
+ asserv_cmd = ('../../asserv/src/asserv/asserv.host', '-m9', 'marcel')
+ mimot_cmd = ('../../mimot/src/dirty/dirty.host', '-m9', 'marcel')
+ io_cmd = ('../../io/src/io.host')
+ self.asserv = asserv.Proto (PopenIO (asserv_cmd), proto_time,
+ **asserv.init.host['marcel'])
+ self.mimot = mimot.Proto (PopenIO (mimot_cmd), proto_time,
+ **mimot.init.host['marcel'])
+ self.io = io.Proto (PopenIO (io_cmd), proto_time,
+ **io.init.host['marcel'])
+ self.robot_start_pos = {
+ False: (300, 2100 - 305, math.radians (-270)),
+ True: (3000 - 300, 2100 - 305, math.radians (-270))
+ }
+
diff --git a/digital/ai/tools/robospierre.py b/digital/ai/tools/robospierre.py
new file mode 100644
index 00000000..09be4bea
--- /dev/null
+++ b/digital/ai/tools/robospierre.py
@@ -0,0 +1,39 @@
+import simu.model.table_eurobot2011
+import simu.view.table_eurobot2011
+
+import simu.robots.robospierre.link.bag
+import simu.robots.robospierre.model.bag
+import simu.robots.robospierre.view.bag
+
+import asserv
+import asserv.init
+import mimot
+import mimot.init
+import io_hub
+import io_hub.init
+
+from proto.popen_io import PopenIO
+import math
+
+class Robot:
+
+ def __init__ (self, proto_time):
+ self.table_model = simu.model.table_eurobot2011
+ self.table_view = simu.view.table_eurobot2011
+ self.robot_link = simu.robots.robospierre.link.bag
+ self.robot_model = simu.robots.robospierre.model.bag
+ self.robot_view = simu.robots.robospierre.view.bag
+ asserv_cmd = ('../../asserv/src/asserv/asserv.host', '-m9', 'marcel')
+ mimot_cmd = ('../../mimot/src/dirty/dirty.host', '-m9', 'marcel')
+ io_hub_cmd = ('../../io-hub/src/robospierre/io_hub.host')
+ self.asserv = asserv.Proto (PopenIO (asserv_cmd), proto_time,
+ **asserv.init.host['robospierre'])
+ self.mimot = mimot.Proto (PopenIO (mimot_cmd), proto_time,
+ **mimot.init.host['robospierre'])
+ self.io = io_hub.Proto (PopenIO (io_hub_cmd), proto_time,
+ **io_hub.init.host['robospierre'])
+ self.robot_start_pos = {
+ False: (700, 2100 - 250, math.radians (-270)),
+ True: (3000 - 700, 2100 - 250, math.radians (-270))
+ }
+
diff --git a/digital/io/tools/test_simu.py b/digital/ai/tools/test_simu.py
index c6fc24f3..80008bb9 100644
--- a/digital/io/tools/test_simu.py
+++ b/digital/ai/tools/test_simu.py
@@ -21,29 +21,12 @@
# Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
#
# }}}
-import math
-
import mex.hub
import utils.forked
-import asserv
-import asserv.init
-import mimot
-import mimot.init
-import io
-import io.init
-from proto.popen_io import PopenIO
-
-import simu.model.table_eurobot2010 as table_model
-import simu.view.table_eurobot2010 as table
-
import simu.model.round_obstacle as obstacle_model
import simu.view.round_obstacle as obstacle_view
-import simu.robots.marcel.link.bag as robot_link
-import simu.robots.marcel.model.bag as robot_model
-import simu.robots.marcel.view.bag as robot_view
-
from simu.inter.inter_node import InterNode
from Tkinter import *
@@ -66,36 +49,32 @@ class ObstacleWithBeacon (obstacle_view.RoundObstacle):
class TestSimu (InterNode):
"""Interface, with simulated programs."""
- robot_start_pos = {
- False: (300, 2100 - 305, math.radians (-270)),
- True: (3000 - 300, 2100 - 305, math.radians (-270))
- }
-
- def __init__ (self, asserv_cmd, mimot_cmd, io_cmd):
+ def __init__ (self, robot_class):
# Hub.
- self.hub = mex.hub.Hub (min_clients = 2)
+ self.hub = mex.hub.Hub (min_clients = 4)
self.forked_hub = utils.forked.Forked (self.hub.wait)
# InterNode.
InterNode.__init__ (self)
- def time ():
+ def proto_time ():
return self.node.date / self.node.tick
+ # Robot parameters.
+ robot = robot_class (proto_time)
+ self.robot = robot
# Asserv.
- self.asserv = asserv.Proto (PopenIO (asserv_cmd), time,
- **asserv.init.host)
+ self.asserv = robot.asserv
self.asserv.async = True
self.tk.createfilehandler (self.asserv, READABLE, self.asserv_read)
# Mimot.
- self.mimot = mimot.Proto (PopenIO (mimot_cmd), time,
- **mimot.init.host)
+ self.mimot = robot.mimot
self.mimot.async = True
self.tk.createfilehandler (self.mimot, READABLE, self.mimot_read)
# Io.
- self.io = io.Proto (PopenIO (io_cmd), time, **io.init.host)
+ self.io = robot.io
self.io.async = True
self.tk.createfilehandler (self.io, READABLE, self.io_read)
# Add table.
- self.table_model = table_model.Table ()
- self.table = table.Table (self.table_view, self.table_model)
+ self.table_model = robot.table_model.Table ()
+ self.table = robot.table_view.Table (self.table_view, self.table_model)
self.obstacle = obstacle_model.RoundObstacle (150)
self.table_model.obstacles.append (self.obstacle)
self.obstacle_beacon = obstacle_model.RoundObstacle (40, 2)
@@ -104,11 +83,11 @@ class TestSimu (InterNode):
self.obstacle_beacon)
self.table_view.bind ('<2>', self.place_obstacle)
# Add robot.
- self.robot_link = robot_link.Bag (self.node)
- self.robot_model = robot_model.Bag (self.node, self.table_model,
+ self.robot_link = robot.robot_link.Bag (self.node)
+ self.robot_model = robot.robot_model.Bag (self.node, self.table_model,
self.robot_link)
- self.robot_view = robot_view.Bag (self.table, self.actuator_view,
- self.sensor_frame, self.robot_model)
+ self.robot_view = robot.robot_view.Bag (self.table,
+ self.actuator_view, self.sensor_frame, self.robot_model)
# Color switch.
self.robot_model.color_switch.register (self.change_color)
@@ -139,7 +118,7 @@ class TestSimu (InterNode):
def change_color (self, *dummy):
i = self.robot_model.color_switch.state
- self.asserv.set_simu_pos (*self.robot_start_pos[i]);
+ self.asserv.set_simu_pos (*self.robot.robot_start_pos[i]);
def place_obstacle (self, ev):
pos = self.table_view.screen_coord ((ev.x, ev.y))
@@ -148,9 +127,25 @@ class TestSimu (InterNode):
self.obstacle.notify ()
self.obstacle_beacon.notify ()
-if __name__ == '__main__':
- app = TestSimu (('../../asserv/src/asserv/asserv.host', '-m9', 'marcel'),
- ('../../mimot/src/dirty/dirty.host', '-m9', 'marcel'),
- ('../src/io.host'))
+def run (default_robot, test_class = TestSimu):
+ import optparse
+ parser = optparse.OptionParser ()
+ parser.add_option ('-r', '--robot', help = "use specified robot",
+ metavar = 'NAME', default = default_robot)
+ (options, args) = parser.parse_args ()
+ if args:
+ parser.error ("too many arguments")
+ if options.robot == 'marcel':
+ import marcel
+ robot = marcel.Robot
+ elif options.robot == 'robospierre':
+ import robospierre
+ robot = robospierre.Robot
+ else:
+ parser.error ("unknown robot")
+ app = test_class (robot)
app.mainloop ()
app.close ()
+
+if __name__ == '__main__':
+ run ('marcel')
diff --git a/digital/io/tools/test_simu_control.py b/digital/ai/tools/test_simu_control_marcel.py
index 9365ab2e..2a43b7bb 100644
--- a/digital/io/tools/test_simu_control.py
+++ b/digital/ai/tools/test_simu_control_marcel.py
@@ -21,15 +21,15 @@
# Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
#
# }}}
-from test_simu import TestSimu
+from test_simu import TestSimu, run
from Tkinter import *
import math
class TestSimuControl (TestSimu):
"""Interface with extra control."""
- def __init__ (self, asserv_cmd, mimot_cmd, io_cmd):
- TestSimu.__init__ (self, asserv_cmd, mimot_cmd, io_cmd)
+ def __init__ (self, robot_class):
+ TestSimu.__init__ (self, robot_class)
def create_widgets (self):
TestSimu.create_widgets (self)
@@ -105,9 +105,4 @@ class TestSimuControl (TestSimu):
pass
if __name__ == '__main__':
- app = TestSimuControl (('../../asserv/src/asserv/asserv.host', '-m9',
- 'marcel'),
- ('../../mimot/src/dirty/dirty.host', '-m9', 'marcel'),
- ('../src/io.host'))
- app.mainloop ()
- app.close ()
+ run ('marcel', TestSimuControl)
diff --git a/digital/ai/tools/test_simu_control_robospierre.py b/digital/ai/tools/test_simu_control_robospierre.py
new file mode 100644
index 00000000..b1cccf4b
--- /dev/null
+++ b/digital/ai/tools/test_simu_control_robospierre.py
@@ -0,0 +1,111 @@
+# io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
+#
+# 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.
+#
+# }}}
+from test_simu import TestSimu, run
+from Tkinter import *
+import math
+
+class TestSimuControl (TestSimu):
+ """Interface with extra control."""
+
+ ELEVATION_STROKE = 0x3b0b
+
+ ROTATION_STROKE = 0x11c6
+
+ def __init__ (self, robot_class):
+ TestSimu.__init__ (self, robot_class)
+
+ def create_widgets (self):
+ TestSimu.create_widgets (self)
+ self.control_frame = Frame (self)
+ self.control_frame.pack (side = 'left', before = self.table_view,
+ fill = 'y')
+ self.clamp_var = IntVar ()
+ self.clamp_var.set (1)
+ self.clamp_button = Checkbutton (self.control_frame, text = 'Clamp',
+ indicatoron = False,
+ variable = self.clamp_var, command = self.clamp_command)
+ self.clamp_button.pack ()
+ self.elevation_up_button = Button (self.control_frame,
+ text = 'Elevation up', padx = 0, pady = 0,
+ command = self.elevation_up_command)
+ self.elevation_up_button.pack ()
+ self.elevation_down_button = Button (self.control_frame,
+ text = 'Elevation down', padx = 0, pady = 0,
+ command = self.elevation_down_command)
+ self.elevation_down_button.pack ()
+ self.rotation_cw_button = Button (self.control_frame,
+ text = 'Rotation cw', padx = 0, pady = 0,
+ command = self.rotation_cw_command)
+ self.rotation_cw_button.pack ()
+ self.rotation_ccw_button = Button (self.control_frame,
+ 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)
+
+ def move (self, ev):
+ pos = self.table_view.screen_coord ((ev.x, ev.y))
+ self.asserv.goto (pos[0], pos[1])
+
+ def orient (self, ev):
+ x, y = self.table_view.screen_coord ((ev.x, ev.y))
+ robot_pos = self.robot_model.position.pos
+ if robot_pos is not None:
+ a = math.atan2 (y - robot_pos[1], x - robot_pos[0])
+ self.asserv.goto_angle (a)
+
+ def clamp_command (self):
+ if self.clamp_var.get ():
+ self.io.pwm_set_timed (0, -0x3ff, 255, 0)
+ else:
+ self.io.pwm_set_timed (0, 0x3ff, 255, 0)
+
+ def elevation_up_command (self):
+ self.mimot.speed_pos ('a0', self.ELEVATION_STROKE / 2)
+
+ def elevation_down_command (self):
+ self.mimot.speed_pos ('a0', -self.ELEVATION_STROKE / 2)
+
+ def rotation_cw_command (self):
+ self.mimot.speed_pos ('a1', self.ROTATION_STROKE / 2)
+
+ 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
+
+if __name__ == '__main__':
+ run ('robospierre', TestSimuControl)
diff --git a/digital/asserv/src/asserv/counter_ext.avr.c b/digital/asserv/src/asserv/counter_ext.avr.c
index 6a883caa..29c51b87 100644
--- a/digital/asserv/src/asserv/counter_ext.avr.c
+++ b/digital/asserv/src/asserv/counter_ext.avr.c
@@ -178,6 +178,7 @@ counter_update (void)
counter_update_step ();
/* Left counter. */
uint16_t left = counter_left_new_step;
+ left &= 0xffff << COUNTER_LEFT_SHIFT; /* Reset unused bits. */
#if !COUNTER_LEFT_REVERSE
counter_left_diff = (int16_t) (left - counter_left_old);
#else
@@ -188,6 +189,7 @@ counter_update (void)
counter_left += counter_left_diff;
/* Right counter. */
uint16_t right = counter_right_new_step;
+ right &= 0xffff << COUNTER_RIGHT_SHIFT; /* Reset unused bits. */
#if !COUNTER_RIGHT_REVERSE
counter_right_diff = (int16_t) (right - counter_right_old);
#else
@@ -203,6 +205,7 @@ counter_update (void)
counter_right = right_new;
/* First auxiliary counter. */
uint16_t aux0 = counter_aux_new_step[0];
+ aux0 &= 0xffff << COUNTER_AUX0_SHIFT; /* Reset unused bits. */
#if !COUNTER_AUX0_REVERSE
counter_aux_diff[0] = (int16_t) (aux0 - counter_aux_old[0]);
#else
@@ -213,6 +216,7 @@ counter_update (void)
counter_aux[0] += counter_aux_diff[0];
/* Second auxiliary counter. */
uint16_t aux1 = counter_aux_new_step[1];
+ aux1 &= 0xffff << COUNTER_AUX1_SHIFT; /* Reset unused bits. */
#if !COUNTER_AUX1_REVERSE
counter_aux_diff[1] = (int16_t) (aux1 - counter_aux_old[1]);
#else
diff --git a/digital/asserv/tools/asserv/init.py b/digital/asserv/tools/asserv/init.py
index 29596a77..3f0afa36 100644
--- a/digital/asserv/tools/asserv/init.py
+++ b/digital/asserv/tools/asserv/init.py
@@ -1,5 +1,5 @@
"""Default parameters for asserv."""
-host = dict (
+host_marcel = dict (
scale = 0.0395840674352314, f = 0xdd1,
tkp = 1, tkd = 16,
ta = 0.75, tsm = 0x20, tss = 0x10,
@@ -12,7 +12,12 @@ host = dict (
E = 0x3ff, D = 0x1ff,
l = 0x1000,
)
-target = dict (
+host = {
+ 'giboulee': host_marcel,
+ 'marcel': host_marcel,
+ 'robospierre': host_marcel,
+ }
+target_marcel = dict (
scale = 0.0415178942124, f = 0xcef,
c = float (0x00ffbabf) / (1 << 24),
tkp = 1, tkd = 16,
@@ -29,3 +34,8 @@ target = dict (
l = 0x1000,
w = 0x09,
)
+target = {
+ 'giboulee': target_marcel,
+ 'marcel': target_marcel,
+ 'robospierre': target_marcel,
+ }
diff --git a/digital/asserv/tools/inter_asserv.py b/digital/asserv/tools/inter_asserv.py
index f2c74a5c..82a92c32 100644
--- a/digital/asserv/tools/inter_asserv.py
+++ b/digital/asserv/tools/inter_asserv.py
@@ -26,8 +26,7 @@ import math
import asserv
import asserv.init
-import proto.popen_io
-import serial
+from utils.init_proto import init_proto
from inter.inter import Inter
from Tkinter import *
@@ -35,15 +34,9 @@ from Tkinter import *
class InterAsserv (Inter):
"""Inter, communicating with the asserv board."""
- def __init__ (self, argv):
+ def __init__ (self):
# Asserv.
- if argv[0] == '!':
- io = proto.popen_io.PopenIO (argv[1:])
- i = asserv.init.host
- else:
- io = serial.Serial (argv[0])
- i = asserv.init.target
- self.a = asserv.Proto (io, **i)
+ self.a = init_proto ('giboulee', asserv.Proto, asserv.init)
self.a.async = True
# Inter.
Inter.__init__ (self)
@@ -128,8 +121,7 @@ class InterAsserv (Inter):
self.a.ftw (self.backwardVar.get ())
if __name__ == '__main__':
- import sys
- app = InterAsserv (sys.argv[1:])
+ app = InterAsserv ()
try:
app.mainloop ()
finally:
diff --git a/digital/asserv/tools/step.py b/digital/asserv/tools/step.py
index 5b5040b1..b9f798ef 100644
--- a/digital/asserv/tools/step.py
+++ b/digital/asserv/tools/step.py
@@ -1,19 +1,12 @@
-import sys
-
import Gnuplot
import asserv
-import proto.popen_io
-import serial
+from utils.init_proto import init_proto
def step (name, offset, kp, ki, kd, plots, **param):
- if sys.argv[1] == '!':
- io = proto.popen_io.PopenIO (sys.argv[2:])
- else:
- io = serial.Serial (sys.argv[1])
p = { name + 'kp': kp, name + 'ki': ki, name + 'kd': kd}
p.update (param)
- a = asserv.Proto (io, **p)
+ a = init_proto (None, asserv.Proto, init = p)
a.stats (*plots)
a.consign (name, offset)
#a.speed (name, 16)
diff --git a/digital/asserv/tools/test_goto.py b/digital/asserv/tools/test_goto.py
index da9ee41c..2cbbb350 100644
--- a/digital/asserv/tools/test_goto.py
+++ b/digital/asserv/tools/test_goto.py
@@ -1,19 +1,11 @@
-import sys
import random
import math
import asserv
import asserv.init
-import proto.popen_io
-import serial
+from utils.init_proto import init_proto
-if sys.argv[1] == '!':
- io = proto.popen_io.PopenIO (sys.argv[2:])
- init = asserv.init.host
-else:
- io = serial.Serial (sys.argv[1])
- init = asserv.init.target
-a = asserv.Proto (io, **init)
+a = init_proto (None, asserv.Proto, asserv.init)
for i in xrange (10):
x = random.randrange (2000)
y = random.randrange (1100)
diff --git a/digital/asserv/tools/write_eeprom.py b/digital/asserv/tools/write_eeprom.py
index b6b9abd9..09a727ea 100644
--- a/digital/asserv/tools/write_eeprom.py
+++ b/digital/asserv/tools/write_eeprom.py
@@ -2,15 +2,8 @@ import sys
import asserv
import asserv.init
-import proto.popen_io
-import serial
+from utils.init_proto import init_proto
-if sys.argv[1] == '!':
- io = proto.popen_io.PopenIO (sys.argv[2:])
- init = asserv.init.host
-else:
- io = serial.Serial (sys.argv[1])
- init = asserv.init.target
-a = asserv.Proto (io, **init)
+a = init_proto (None, asserv.Proto, asserv.init)
a.write_eeprom ()
a.close ()
diff --git a/digital/avr/common/io.h b/digital/avr/common/io.h
index 7f94f4e6..9690a55b 100644
--- a/digital/avr/common/io.h
+++ b/digital/avr/common/io.h
@@ -36,6 +36,10 @@
/* SIGNAL is now ISR. */
# define ISR SIGNAL
#endif
+/* Some AVR includes define HOST! */
+#ifdef HOST
+# undef HOST
+#endif
/** Saved interrupts state. */
typedef uint8_t intr_flags_t;
@@ -106,10 +110,15 @@ intr_restore (intr_flags_t flags) { }
#define IO_N(io) IO_N_ (io)
#define IO_N_(p, n) (n)
-#define IO_GET(io) (IO_PIN_ (io) & IO_BV_ (io))
-#define IO_SET(io) IO_PORT_ (io) |= IO_BV_ (io)
-#define IO_CLR(io) IO_PORT_ (io) &= ~IO_BV_ (io)
-#define IO_OUTPUT(io) IO_DDR_ (io) |= IO_BV_ (io)
-#define IO_INPUT(io) IO_DDR_ (io) &= ~IO_BV_ (io)
+#define IO_GET(io) IO_GET_ (io)
+#define IO_GET_(p, n) (IO_PIN_ (p, n) & IO_BV_ (p, n))
+#define IO_SET(io) IO_SET_ (io)
+#define IO_SET_(p, n) IO_PORT_ (p, n) |= IO_BV_ (p, n)
+#define IO_CLR(io) IO_CLR_ (io)
+#define IO_CLR_(p, n) IO_PORT_ (p, n) &= ~IO_BV_ (p, n)
+#define IO_OUTPUT(io) IO_OUTPUT_ (io)
+#define IO_OUTPUT_(p, n) IO_DDR_ (p, n) |= IO_BV_ (p, n)
+#define IO_INPUT(io) IO_INPUT_ (io)
+#define IO_INPUT_(p, n) IO_DDR_ (p, n) &= ~IO_BV_ (p, n)
#endif /* io_h */
diff --git a/digital/avr/modules/math/geometry/distance.c b/digital/avr/modules/math/geometry/distance.c
index f8bb7c1f..38a5350c 100644
--- a/digital/avr/modules/math/geometry/distance.c
+++ b/digital/avr/modules/math/geometry/distance.c
@@ -29,7 +29,7 @@
#include "modules/math/fixed/fixed.h"
int16_t
-distance_point_point (vect_t *a, vect_t *b)
+distance_point_point (const vect_t *a, const vect_t *b)
{
int32_t dx = b->x - a->x;
int32_t dy = b->y - a->y;
@@ -37,7 +37,7 @@ distance_point_point (vect_t *a, vect_t *b)
}
int16_t
-distance_segment_point (vect_t *a, vect_t *b, vect_t *p)
+distance_segment_point (const vect_t *a, const vect_t *b, const vect_t *p)
{
int16_t proj;
/* The space is split in three regions delimited by the two parallel lines
diff --git a/digital/avr/modules/math/geometry/distance.h b/digital/avr/modules/math/geometry/distance.h
index c48c2226..049b84ce 100644
--- a/digital/avr/modules/math/geometry/distance.h
+++ b/digital/avr/modules/math/geometry/distance.h
@@ -29,13 +29,13 @@
/** Compute distance between two points.
* - a, b: points. */
int16_t
-distance_point_point (vect_t *a, vect_t *b);
+distance_point_point (const vect_t *a, const vect_t *b);
/** Compute distance between a segment and a point.
* - a: segment first vertice.
* - b: segment second vertice.
* - p: point. */
int16_t
-distance_segment_point (vect_t *a, vect_t *b, vect_t *p);
+distance_segment_point (const vect_t *a, const vect_t *b, const vect_t *p);
#endif /* distance_h */
diff --git a/digital/avr/modules/twi/twi_hard.avr.c b/digital/avr/modules/twi/twi_hard.avr.c
index ff65d31e..8bd18d34 100644
--- a/digital/avr/modules/twi/twi_hard.avr.c
+++ b/digital/avr/modules/twi/twi_hard.avr.c
@@ -56,6 +56,10 @@
# if defined (__AVR_ATmega32__)
# elif defined (__AVR_ATmega64__)
# elif defined (__AVR_ATmega128__)
+# elif defined (__AVR_AT90USB646__)
+# elif defined (__AVR_AT90USB647__)
+# elif defined (__AVR_AT90USB1286__)
+# elif defined (__AVR_AT90USB1287__)
# else
# error "twi: not tested on this chip"
# endif
@@ -201,7 +205,7 @@ twi_master_wait (void)
# endif /* AC_TWI_MASTER_ENABLE */
-SIGNAL (SIG_2WIRE_SERIAL)
+SIGNAL (TWI_vect)
{
# if AC_TWI_SLAVE_ENABLE
/** Slave work buffer. */
diff --git a/digital/avr/modules/uart/uart.avr.c b/digital/avr/modules/uart/uart.avr.c
index e3ffb18c..689866ae 100644
--- a/digital/avr/modules/uart/uart.avr.c
+++ b/digital/avr/modules/uart/uart.avr.c
@@ -39,6 +39,10 @@
#elif defined (__AVR_ATmega64__)
#elif defined (__AVR_ATmega128__)
#elif defined (__AVR_AT90USB162__)
+#elif defined (__AVR_AT90USB646__)
+#elif defined (__AVR_AT90USB647__)
+#elif defined (__AVR_AT90USB1286__)
+#elif defined (__AVR_AT90USB1287__)
#else
# warning "uart: not tested on this chip"
#endif
@@ -149,12 +153,18 @@
/* Baud rate error check. */
#define UBRR_VAL (AC_FREQ / 16 / AC_UART (BAUDRATE) - 1)
#define BAUDRATE_VAL (AC_FREQ / 16 / (UBRR_VAL + 1))
+#define BAUDRATE_ERROR (BAUDRATE_VAL - AC_UART (BAUDRATE))
+#define BAUDRATE_ERROR_PCT (BAUDRATE_ERROR * 100 / AC_UART (BAUDRATE))
-#if BAUDRATE_VAL - AC_UART (BAUDRATE) > 2
+#if BAUDRATE_ERROR_PCT >= 2
+#warning "uart: baud rate error > 2 %"
+#elif BAUDRATE_ERROR_PCT >= 1
+#warning "uart: baud rate error > 1 %"
+#elif BAUDRATE_ERROR > 2
#warning "uart: baud rate error > 2"
-#elif BAUDRATE_VAL - AC_UART (BAUDRATE) > 1
+#elif BAUDRATE_ERROR > 1
#warning "uart: baud rate error > 1"
-#elif BAUDRATE_VAL - AC_UART (BAUDRATE) > 0
+#elif BAUDRATE_ERROR > 0
#warning "uart: baud rate error > 0"
#endif
diff --git a/digital/avr/modules/utils/utils.avr.h b/digital/avr/modules/utils/utils.avr.h
index efe9eaa1..aa7421d2 100644
--- a/digital/avr/modules/utils/utils.avr.h
+++ b/digital/avr/modules/utils/utils.avr.h
@@ -25,6 +25,7 @@
*
* }}} */
+#include "io.h"
#include <avr/wdt.h>
/** Helper macro to build register value. Call it like this::
diff --git a/digital/io-hub/src/common/contact.avr.c b/digital/io-hub/src/common/contact.avr.c
new file mode 100644
index 00000000..9f6869b4
--- /dev/null
+++ b/digital/io-hub/src/common/contact.avr.c
@@ -0,0 +1,98 @@
+/* contact.avr.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 "contact.h"
+
+/** Filter depth. */
+#define CONTACT_JACK_FILTER_DEPTH 50
+
+/** Contact context. */
+struct contact_t
+{
+ /** Jack current state. */
+ uint8_t jack_state;
+ /** Jack filter, number of updates before state can change. */
+ uint8_t jack_filter;
+};
+
+/** Global context. */
+struct contact_t contact_global;
+#define ctx contact_global
+
+void
+contact_init (void)
+{
+ /* Pull-up. */
+#define CONTACT(io) IO_SET_ (io);
+ CONTACT (CONTACT_COLOR);
+ CONTACT (CONTACT_JACK);
+ CONTACT_LIST
+#undef CONTACT
+ /* Default to jack out. */
+ ctx.jack_state = 1;
+}
+
+void
+contact_update (void)
+{
+ uint8_t jack_now = IO_GET (CONTACT_JACK);
+ /* If state is unchanged, reset filter. */
+ if (jack_now == ctx.jack_state)
+ ctx.jack_filter = 0;
+ /* If state changed, should be stable for CONTACT_JACK_FILTER_DEPTH before
+ * it goes through the filter. */
+ else if (ctx.jack_filter++ == CONTACT_JACK_FILTER_DEPTH)
+ {
+ ctx.jack_state = jack_now;
+ ctx.jack_filter = 0;
+ }
+}
+
+enum team_color_e
+contact_get_color (void)
+{
+ return !IO_GET (CONTACT_COLOR) ? TEAM_COLOR_LEFT : TEAM_COLOR_RIGHT;
+}
+
+uint8_t
+contact_get_jack (void)
+{
+ return ctx.jack_state;
+}
+
+uint32_t
+contact_all (void)
+{
+ uint32_t contacts = 0, bit = 1;
+#define CONTACT(io) do { \
+ contacts |= (IO_GET_ (io) ? bit : 0); \
+ bit <<= 1; \
+} while (0);
+ CONTACT (CONTACT_COLOR);
+ CONTACT (CONTACT_JACK);
+ CONTACT_LIST
+#undef CONTACT
+ return contacts;
+}
diff --git a/digital/io-hub/src/common/contact.h b/digital/io-hub/src/common/contact.h
new file mode 100644
index 00000000..fce06f84
--- /dev/null
+++ b/digital/io-hub/src/common/contact.h
@@ -0,0 +1,55 @@
+#ifndef contact_h
+#define contact_h
+/* contact.h */
+/* robospierre - Eurobot 2011 AI. {{{
+ *
+ * Copyright (C) 2011 Nicolas Schodet
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+#include "simu.host.h"
+#include "defs.h"
+#include "io.h"
+
+#include "contact_defs.h"
+
+/** To get a state of a generic contact, use: IO_GET (CONTACT_EXAMPLE). */
+
+/** Initialize contact module. */
+void
+contact_init (void);
+
+/** Update contact module. This handle debouncing. */
+void
+contact_update (void);
+
+/** Get color switch state, unfiltered. */
+enum team_color_e
+contact_get_color (void);
+
+/** Get filtered jack state. */
+uint8_t
+contact_get_jack (void);
+
+/** Return state of all contact, unfiltered. */
+uint32_t
+contact_all (void);
+
+#endif /* contact_h */
diff --git a/digital/io-hub/src/common/contact.host.c b/digital/io-hub/src/common/contact.host.c
new file mode 100644
index 00000000..5e2309c4
--- /dev/null
+++ b/digital/io-hub/src/common/contact.host.c
@@ -0,0 +1,98 @@
+/* contact.host.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 "contact.h"
+
+#include "modules/host/host.h"
+#include "modules/host/mex.h"
+
+/** Contact context. */
+struct contact_t
+{
+ /** All contact state. */
+ uint32_t all;
+ /** Jack current state. */
+ uint8_t jack_state;
+ /** Color current state. */
+ enum team_color_e color_state;
+};
+
+/** Global context. */
+struct contact_t contact_global;
+#define ctx contact_global
+
+static void
+contact_handle (void *user, mex_msg_t *msg)
+{
+ uint32_t contacts;
+ mex_msg_pop (msg, "L", &contacts);
+ ctx.all = contacts;
+ ctx.color_state = !(contacts & 1) ? TEAM_COLOR_LEFT : TEAM_COLOR_RIGHT;
+ ctx.jack_state = (contacts & 2) ? 1 : 0;
+ contacts >>= 2;
+#define CONTACT(io) do { \
+ if (contacts & 1) \
+ IO_PIN_ (io) |= IO_BV_ (io); \
+ else \
+ IO_PIN_ (io) &= ~IO_BV_ (io); \
+ contacts >>= 1; \
+} while (0);
+ CONTACT_LIST
+#undef CONTACT
+}
+
+void
+contact_init (void)
+{
+ const char *mex_instance = host_get_instance ("io-hub0", 0);
+ uint8_t mtype = mex_node_reservef ("%s:contact", mex_instance);
+ mex_node_register (mtype, contact_handle, 0);
+ /* Default to jack out. */
+ ctx.jack_state = 1;
+}
+
+void
+contact_update (void)
+{
+ /* Nothing to do. */
+}
+
+enum team_color_e
+contact_get_color (void)
+{
+ return ctx.color_state;
+}
+
+uint8_t
+contact_get_jack (void)
+{
+ return ctx.jack_state;
+}
+
+uint32_t
+contact_all (void)
+{
+ return ctx.all;
+}
diff --git a/digital/io-hub/src/common/pwm.avr.c b/digital/io-hub/src/common/pwm.avr.c
new file mode 100644
index 00000000..4b6ad896
--- /dev/null
+++ b/digital/io-hub/src/common/pwm.avr.c
@@ -0,0 +1,133 @@
+/* pwm.avr.c */
+/* io-hub - Modular Input/Output. {{{
+ *
+ * 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 "pwm.h"
+
+#include "modules/utils/utils.h"
+#include "preproc.h"
+
+/** PWM context. */
+struct pwm_t
+{
+ /** Output compare register. */
+ volatile uint16_t *ocr;
+ /** Direction port. */
+ volatile uint8_t *dir_port;
+ /** Direction bit value. */
+ uint8_t dir_bv;
+ /** Remaining time before timer elapse, 0 for disabled. */
+ uint16_t time;
+ /** Value to be used after timer elapse. */
+ int16_t value_rest;
+};
+
+/** Array of PWM contexts. */
+#define PWM(timer, pwm, pwm_io_port, pwm_io_n, dir_io_port, dir_io_n) \
+{ &PREPROC_PASTE (OCR, timer, pwm), \
+ &IO_PORT_ (dir_io_port, dir_io_n), IO_BV_ (dir_io_port, dir_io_n), \
+ 0, 0 },
+struct pwm_t pwm[] = {
+ AC_IOHUB_PWM
+};
+#undef PWM
+
+void
+pwm_init (void)
+{
+ /* Fast PWM, TOP = 0x3ff, with positive logic.
+ f_IO without prescaler.
+ Fpwm = f_IO / (prescaler * (1 + TOP)). */
+ uint8_t timer_com_1 = 0, timer_com_3 = 0;
+#define PWM(timer, pwm, pwm_io_port, pwm_io_n, dir_io_port, dir_io_n) \
+ do { \
+ PREPROC_PASTE (timer_com_, timer) |= \
+ _BV (PREPROC_PASTE (COM, timer, pwm, 1)); \
+ IO_DDR_ (pwm_io_port, pwm_io_n) |= IO_BV_ (pwm_io_port, pwm_io_n); \
+ IO_DDR_ (dir_io_port, dir_io_n) |= IO_BV_ (dir_io_port, dir_io_n); \
+ } while (0);
+ AC_IOHUB_PWM
+#undef PWM
+ if (timer_com_1)
+ {
+ TCCR1A = timer_com_1 |
+ regv (COM1A1, COM1A0, COM1B1, COM1B0, COM1C1, COM1C0, WGM11, WGM10,
+ 0, 0, 0, 0, 0, 0, 1, 1);
+ TCCR1B = regv (ICNC1, ICES1, 5, WGM13, WGM12, CS12, CS11, CS10,
+ 0, 0, 0, 0, 1, 0, 0, 1);
+ }
+ if (timer_com_3)
+ {
+ TCCR3A = timer_com_3 |
+ regv (COM3A1, COM3A0, COM3B1, COM3B0, COM3C1, COM3C0, WGM31, WGM30,
+ 0, 0, 0, 0, 0, 0, 1, 1);
+ TCCR3B = regv (ICNC3, ICES3, 5, WGM33, WGM32, CS32, CS31, CS30,
+ 0, 0, 0, 0, 1, 0, 0, 1);
+ }
+}
+
+void
+pwm_update (void)
+{
+ uint8_t i;
+ for (i = 0; i < UTILS_COUNT (pwm); i++)
+ {
+ if (pwm[i].time)
+ {
+ pwm[i].time--;
+ if (pwm[i].time == 0)
+ pwm_set (i, pwm[i].value_rest);
+ }
+ }
+}
+
+void
+pwm_set (uint8_t index, int16_t value)
+{
+ assert (index < UTILS_COUNT (pwm));
+ assert (value <= PWM_MAX && value >= -PWM_MAX);
+ pwm[index].time = 0;
+ if (value >= 0)
+ {
+ *pwm[index].dir_port &= ~pwm[index].dir_bv;
+ *pwm[index].ocr = value;
+ }
+ else
+ {
+ *pwm[index].dir_port |= pwm[index].dir_bv;
+ *pwm[index].ocr = -value;
+ }
+}
+
+void
+pwm_set_timed (uint8_t index, int16_t value, uint16_t time, int16_t value_rest)
+{
+ assert (index < UTILS_COUNT (pwm));
+ assert (value <= PWM_MAX && value >= -PWM_MAX);
+ assert (value_rest <= PWM_MAX && value_rest >= -PWM_MAX);
+ pwm_set (index, value);
+ pwm[index].time = time;
+ pwm[index].value_rest = value_rest;
+}
+
diff --git a/digital/io-hub/src/common/pwm.h b/digital/io-hub/src/common/pwm.h
new file mode 100644
index 00000000..aff84b39
--- /dev/null
+++ b/digital/io-hub/src/common/pwm.h
@@ -0,0 +1,60 @@
+#ifndef pwm_h
+#define pwm_h
+/* pwm.h */
+/* io-hub - Modular Input/Output. {{{
+ *
+ * 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.
+ *
+ * }}} */
+
+/** Handle PWM output with direction. Also handle timed PWM setting to apply
+ * a PWM value for a given number of update, then switch to a resting value.
+ *
+ * AC_IOHUB_PWM should be set to a list of space separated PWM macro call:
+ *
+ * PWM (timer, pwm, pwm_io_port, pwm_io_n, dir_io_port, dir_io_n)
+ *
+ * For example:
+ *
+ * PWM (1, A, B, 1, B, 2) => use output compare A on timer 1, PWM output on
+ * B1, direction on B2. */
+
+/** Maximum positive value. */
+#define PWM_MAX 0x3ff
+
+/** Initialise PWM module. */
+void
+pwm_init (void);
+
+/** Update PWM module (handle timed PWM). */
+void
+pwm_update (void);
+
+/** Set PWM value for given PWM index. */
+void
+pwm_set (uint8_t index, int16_t value);
+
+/** Set PWM value for given PWM index for a given time. After time elapsed,
+ * use a rest value. */
+void
+pwm_set_timed (uint8_t index, int16_t value, uint16_t time, int16_t value_rest);
+
+#endif /* pwm_h */
diff --git a/digital/io-hub/src/common/pwm.host.c b/digital/io-hub/src/common/pwm.host.c
new file mode 100644
index 00000000..f528522a
--- /dev/null
+++ b/digital/io-hub/src/common/pwm.host.c
@@ -0,0 +1,109 @@
+/* pwm.host.c */
+/* io-hub - Modular Input/Output. {{{
+ *
+ * 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 "pwm.h"
+
+#include "modules/utils/utils.h"
+#include "modules/host/host.h"
+#include "modules/host/mex.h"
+
+/** PWM context. */
+struct pwm_t
+{
+ /** Current value. */
+ int16_t value;
+ /** Remaining time before timer elapse, 0 for disabled. */
+ uint16_t time;
+ /** Value to be used after timer elapse. */
+ int16_t value_rest;
+};
+
+/** Array of PWM contexts. */
+#define PWM(timer, pwm, pwm_io_port, pwm_io_n, dir_io_port, dir_io_n) \
+ { 0, 0, 0 },
+struct pwm_t pwm[] = {
+ AC_IOHUB_PWM
+};
+#undef PWM
+
+/** Message type. */
+uint8_t pwm_mtype;
+
+/** Message has to be sent. */
+uint8_t pwm_changed;
+
+void
+pwm_init (void)
+{
+ const char *mex_instance = host_get_instance ("io-hub0", 0);
+ pwm_mtype = mex_node_reservef ("%s:pwm", mex_instance);
+}
+
+void
+pwm_update (void)
+{
+ uint8_t i;
+ /* Update timed PWM. */
+ for (i = 0; i < UTILS_COUNT (pwm); i++)
+ {
+ if (pwm[i].time)
+ {
+ pwm[i].time--;
+ if (pwm[i].time == 0)
+ pwm_set (i, pwm[i].value_rest);
+ }
+ }
+ /* Send change if needed. */
+ if (pwm_changed)
+ {
+ mex_msg_t *m = mex_msg_new (pwm_mtype);
+ for (i = 0; i < UTILS_COUNT (pwm); i++)
+ mex_msg_push (m, "h", pwm[i].value);
+ mex_node_send (m);
+ pwm_changed = 0;
+ }
+}
+
+void
+pwm_set (uint8_t index, int16_t value)
+{
+ assert (index < UTILS_COUNT (pwm));
+ assert (value <= PWM_MAX && value >= -PWM_MAX);
+ pwm[index].time = 0;
+ pwm[index].value = value;
+ pwm_changed = 1;
+}
+
+void
+pwm_set_timed (uint8_t index, int16_t value, uint16_t time, int16_t value_rest)
+{
+ assert (index < UTILS_COUNT (pwm));
+ assert (value <= PWM_MAX && value >= -PWM_MAX);
+ assert (value_rest <= PWM_MAX && value_rest >= -PWM_MAX);
+ pwm_set (index, value);
+ pwm[index].time = time;
+ pwm[index].value_rest = value_rest;
+}
+
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 */
diff --git a/digital/io-hub/tools/io_hub/__init__.py b/digital/io-hub/tools/io_hub/__init__.py
new file mode 100644
index 00000000..e84cb000
--- /dev/null
+++ b/digital/io-hub/tools/io_hub/__init__.py
@@ -0,0 +1,2 @@
+from io_hub import Proto
+from mex import Mex
diff --git a/digital/io-hub/tools/io_hub/init.py b/digital/io-hub/tools/io_hub/init.py
new file mode 100644
index 00000000..2adaa85c
--- /dev/null
+++ b/digital/io-hub/tools/io_hub/init.py
@@ -0,0 +1,6 @@
+"""Default parameters for io-hub."""
+host = {
+ 'robospierre': dict (
+ ),
+ }
+target = host
diff --git a/digital/io-hub/tools/io_hub/io_hub.py b/digital/io-hub/tools/io_hub/io_hub.py
new file mode 100644
index 00000000..a363e5f6
--- /dev/null
+++ b/digital/io-hub/tools/io_hub/io_hub.py
@@ -0,0 +1,57 @@
+# io-hub - Modular Input/Output. {{{
+#
+# 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.
+#
+# }}}
+import proto, time
+
+class Proto:
+
+ def __init__ (self, file, time = time.time, **param):
+ self.proto = proto.Proto (file, time, 0.1)
+ self.async = False
+ self.param = param
+ self.send_param ()
+
+ def send_param (self):
+ pass
+
+ def reset (self):
+ self.proto.send ('z')
+ self.proto.send ('z')
+
+ def pwm_set (self, index, value):
+ self.proto.send ('w', 'Bh', index, value)
+
+ 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)
+ self.proto.file.close ()
+
+ def fileno (self):
+ return self.proto.fileno ()
+
diff --git a/digital/io-hub/tools/io_hub/mex.py b/digital/io-hub/tools/io_hub/mex.py
new file mode 100644
index 00000000..7c8c0012
--- /dev/null
+++ b/digital/io-hub/tools/io_hub/mex.py
@@ -0,0 +1,119 @@
+# io-hub - Modular Input/Output. {{{
+#
+# 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.
+#
+# }}}
+"""Mex interface to io-hub."""
+
+from utils.observable import Observable
+import simu.mex.msg
+
+ADC_NB = 8
+
+PWM_NB = 6
+PWM_VALUE_MAX = 1024
+
+CONTACT_NB = 4
+CONTACT_INIT = 0xffffffff
+
+class Mex:
+ """Handle communications with simulated io-hub."""
+
+ class ADC (Observable):
+ """Analog to digital input."
+
+ - value: volts.
+
+ """
+
+ def __init__ (self):
+ Observable.__init__ (self)
+ self.value = None
+
+ class PWM (Observable):
+ """PWM output.
+
+ - value: current PWM value (-1 ... +1).
+
+ """
+
+ def __init__ (self):
+ Observable.__init__ (self)
+ self.value = None
+
+ class Pack:
+ """Handle reception of several PWM for one message."""
+
+ def __init__ (self, node, instance, list):
+ self.__list = list
+ node.register (instance + ':pwm', self.__handle)
+
+ def __handle (self, msg):
+ values = msg.pop ('%dh' % len (self.__list))
+ for pwm, value in zip (self.__list, values):
+ pwm.value = float (value) / PWM_VALUE_MAX
+ pwm.notify ()
+
+ class Contact (Observable):
+ """Contact input.
+
+ - state: True (open) or False (closed).
+
+ """
+
+ def __init__ (self, pack, index):
+ Observable.__init__ (self)
+ self.pack = pack
+ self.index = index
+ self.state = None
+ self.register (self.__notified)
+
+ def __notified (self):
+ self.pack.set (self.index, self.state)
+
+ class Pack:
+ """Handle emission of several contacts for one message."""
+
+ def __init__ (self, node, instance):
+ self.node = node
+ self.contacts = CONTACT_INIT
+ self.mtype = node.reserve (instance + ':contact')
+
+ def set (self, index, state):
+ if state is None or state:
+ self.contacts |= 1 << index
+ else:
+ self.contacts &= ~(1 << index)
+ self.__send ()
+
+ def __send (self):
+ m = simu.mex.msg.Msg (self.mtype)
+ m.push ('L', self.contacts)
+ self.node.send (m)
+
+ def __init__ (self, node, instance = 'io-hub0'):
+ self.adc = tuple (self.ADC () for i in range (0, ADC_NB))
+ self.pwm = tuple (self.PWM () for i in range (0, PWM_NB))
+ self.__adc_pwm = self.PWM.Pack (node, instance, self.pwm)
+ self.__contact_pack = self.Contact.Pack (node, instance)
+ self.contact = tuple (self.Contact (self.__contact_pack, i)
+ for i in range (CONTACT_NB))
+
diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile
index 5dfea34b..5cbea0ba 100644
--- a/digital/io/src/Makefile
+++ b/digital/io/src/Makefile
@@ -3,14 +3,16 @@ BASE = ../../avr
# Name of the program to build.
PROGS = io
# Sources to compile.
-io_SOURCES = main.c servo.avr.c eeprom.avr.c pwm.c \
- switch.avr.c chrono.c main_timer.avr.c servo_pos.c \
+io_SOURCES = main.c fsm_queue.c servo.avr.c eeprom.avr.c pwm.c \
+ switch.avr.c chrono.c timer.avr.c servo_pos.c \
twi_master.c asserv.c mimot.c \
simu.host.c contact.c usdist.c radar.c \
- path.c food.c events.host.c
+ path.c food.c events.host.c \
+ fsm.host.c init.c move.c top.c hola.c loader.c fsm_AI_gen.avr.c
# Modules needed for IO.
MODULES = proto uart twi utils adc math/fixed math/geometry path/astar \
trace flash spi
+AI_MODULES = twi_master common utils fsm
# Configuration file.
CONFIGFILE = avrconfig.h
# IO board use an ATMega128.
@@ -19,11 +21,20 @@ AVR_MCU = atmega128
OPTIMIZE = -O2
HOST_LIBS = -lm
-# FSM.
-FSM = ai
-FSM_SOURCES := top init move loader hola
-# Include FSM makefile.
-include $(BASE)/make/Makefile.fsm
+vpath %.c $(AI_MODULES:%=../../ai/src/%)
+vpath %.h $(AI_MODULES:%=../../ai/src/%)
+INCLUDES += -I. $(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.host
+ ./$< --gen
+ mv fsm_AI_gen.c fsm_AI_gen.avr.c
events.h events.host.c: trace.trc
PYTHONPATH=$(BASE)/../../host python $(BASE)/../../tools/trace/trace.py -t create -i $< -o
diff --git a/digital/io/src/ai.conf b/digital/io/src/ai.conf
deleted file mode 100644
index ce7f6374..00000000
--- a/digital/io/src/ai.conf
+++ /dev/null
@@ -1,12 +0,0 @@
-[user]
-template-dir = fsm_templates
-
-[templates]
-template_fsm.h = %_fsm.h
-template_fsm.c = %_fsm.c
-template_cb.h = %_cb.h
-template_top_cb_skel.c = %_top_cb_skel.c
-template_init_cb_skel.c = %_init_cb_skel.c
-template_move_cb_skel.c = %_move_cb_skel.c
-template_loader_cb_skel.c = %_loader_cb_skel.c
-template_hola_cb_skel.c = %_hola_cb_skel.c
diff --git a/digital/io/src/ai_hola_cb.c b/digital/io/src/ai_hola_cb.c
deleted file mode 100644
index 6b76b058..00000000
--- a/digital/io/src/ai_hola_cb.c
+++ /dev/null
@@ -1,165 +0,0 @@
-/* ai_hola_cb.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 "fsm.h"
-#include "ai_cb.h"
-#include "asserv.h"
-#include "mimot.h"
-#include "bot.h"
-#include "playground.h"
-
-#include "modules/math/fixed/fixed.h"
-
-uint32_t hola_angle;
-int16_t hola_timeout;
-
-/*
- * HOLA_IDLE =start=>
- * => HOLA_WAIT_HOLA
- */
-fsm_branch_t
-ai__HOLA_IDLE__start (void)
-{
- return ai_next (HOLA_IDLE, start);
-}
-
-/*
- * HOLA_WAIT_HOLA =hola_start=>
- * => HOLA_WAIT_JACK_IN
- */
-fsm_branch_t
-ai__HOLA_WAIT_HOLA__hola_start (void)
-{
- return ai_next (HOLA_WAIT_HOLA, hola_start);
-}
-
-/*
- * HOLA_WAIT_JACK_IN =jack_inserted_into_bot=>
- * => HOLA_WAIT_JACK_OUT
- */
-fsm_branch_t
-ai__HOLA_WAIT_JACK_IN__jack_inserted_into_bot (void)
-{
- return ai_next (HOLA_WAIT_JACK_IN, jack_inserted_into_bot);
-}
-
-/*
- * HOLA_WAIT_JACK_OUT =jack_removed_from_bot=>
- * => HOLA_ROUNDS
- */
-fsm_branch_t
-ai__HOLA_WAIT_JACK_OUT__jack_removed_from_bot (void)
-{
- return ai_next (HOLA_WAIT_JACK_OUT, jack_removed_from_bot);
-}
-
-/*
- * HOLA_ROUNDS =asserv_last_cmd_ack=>
- * no_timeout => HOLA_ROUNDS
- * move clamp rounds
- * timeout => HOLA_UP
- * move up
- * rotate
- */
-fsm_branch_t
-ai__HOLA_ROUNDS__asserv_last_cmd_ack (void)
-{
- if (++hola_timeout < 150)
- {
- int32_t cos = fixed_cos_f824 (hola_angle) + 0x1000000;
- int32_t sin = fixed_sin_f824 (hola_angle) + 0x1000000;
- int32_t dx = BOT_CLAMP_OPEN_STEP
- + fixed_mul_f824 (BOT_CLAMP_STROKE_STEP / 3 / 2, cos);
- int32_t dy = BOT_ELEVATOR_ZERO_SPEED
- + fixed_mul_f824 (BOT_ELEVATOR_REST_STEP / 3 / 2, sin);
- mimot_move_motor0_absolute (dx, BOT_CLAMP_SPEED);
- mimot_move_motor1_absolute (dx, BOT_CLAMP_SPEED);
- asserv_move_motor0_absolute (dy, BOT_ELEVATOR_SPEED);
- hola_angle += 0x1000000 / (225 / 12);
- return ai_next_branch (HOLA_ROUNDS, asserv_last_cmd_ack, no_timeout);
- }
- else
- {
- asserv_set_speed (0x10, 0x1c, 0x10, 0x1c);
- asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED / 3);
- asserv_move_angularly (POSITION_A_DEG (174));
- return ai_next_branch (HOLA_ROUNDS, asserv_last_cmd_ack, timeout);
- }
-}
-
-/*
- * HOLA_UP =bot_move_succeed=>
- * => HOLA_CENTER
- * rotate to center
- */
-fsm_branch_t
-ai__HOLA_UP__bot_move_succeed (void)
-{
- asserv_set_speed (0x10, 0x20, 0x10, 0x20);
- asserv_move_angularly (POSITION_A_DEG (-90));
- asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP / 2, BOT_ELEVATOR_SPEED);
- return ai_next (HOLA_UP, bot_move_succeed);
-}
-
-/*
- * HOLA_CENTER =bot_move_succeed=>
- * => HOLA_CLAP_CLOSE
- * close clamp
- */
-fsm_branch_t
-ai__HOLA_CENTER__bot_move_succeed (void)
-{
- mimot_motor0_clamp (BOT_CLAMP_ZERO_SPEED, 0);
- mimot_motor1_clamp (BOT_CLAMP_ZERO_SPEED, 0);
- return ai_next (HOLA_CENTER, bot_move_succeed);
-}
-
-/*
- * HOLA_CLAP_CLOSE =clamp_succeed=>
- * => HOLA_CLAP_OPEN
- * open clamp
- */
-fsm_branch_t
-ai__HOLA_CLAP_CLOSE__clamp_succeed (void)
-{
- mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- return ai_next (HOLA_CLAP_CLOSE, clamp_succeed);
-}
-
-/*
- * HOLA_CLAP_OPEN =clamp_succeed=>
- * => HOLA_CLAP_CLOSE
- * close clamp
- */
-fsm_branch_t
-ai__HOLA_CLAP_OPEN__clamp_succeed (void)
-{
- mimot_motor0_clamp (BOT_CLAMP_SPEED, 0);
- mimot_motor1_clamp (BOT_CLAMP_SPEED, 0);
- return ai_next (HOLA_CLAP_OPEN, clamp_succeed);
-}
-
-
diff --git a/digital/io/src/ai_init_cb.c b/digital/io/src/ai_init_cb.c
deleted file mode 100644
index 444b867b..00000000
--- a/digital/io/src/ai_init_cb.c
+++ /dev/null
@@ -1,213 +0,0 @@
-/* init_cb.c - init FSM callbacks. */
-/* {{{
- *
- * Copyright (C) 2009 Nicolas Haller
- *
- * 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.
- *
- * }}}
- * init:
- * place the bot in the start position for a match.
- */
-#include "common.h"
-#include "fsm.h"
-#include "ai_cb.h"
-#include "asserv.h"
-#include "init.h"
-#include "playground.h"
-#include "main.h"
-#include "bot.h"
-#include "switch.h"
-#include "chrono.h"
-#include "playground.h"
-#include "modules/trace/trace.h"
-#include "modules/path/path.h"
-
-/*
- * INIT_IDLE =start=>
- * => INIT_WAIT_FIRST_JACK_IN
- * do nothing.
- */
-fsm_branch_t
-ai__INIT_IDLE__start (void)
-{
- return ai_next (INIT_IDLE, start);
-}
-
-/*
- * INIT_WAIT_FIRST_JACK_IN =jack_inserted_into_bot=>
- * => INIT_WAIT_FIRST_JACK_OUT
- * do nothing.
- */
-fsm_branch_t
-ai__INIT_WAIT_FIRST_JACK_IN__jack_inserted_into_bot (void)
-{
- return ai_next (INIT_WAIT_FIRST_JACK_IN, jack_inserted_into_bot);
-}
-
-/*
- * INIT_WAIT_FIRST_JACK_OUT =jack_removed_from_bot=>
- * => INIT_WAIT_SECOND_JACK_IN
- * start trace module.
- * get and store the color of the bot.
- */
-fsm_branch_t
-ai__INIT_WAIT_FIRST_JACK_OUT__jack_removed_from_bot (void)
-{
- /* Initialize trace module (erase the flash). */
- trace_init ();
- /* Get the color. */
- bot_color = switch_get_color ();
- return ai_next (INIT_WAIT_FIRST_JACK_OUT, jack_removed_from_bot);
-}
-
-/*
- * INIT_WAIT_SECOND_JACK_IN =jack_inserted_into_bot=>
- * => INIT_WAIT_FOR_HANDS_OUT
- * do nothing.
- */
-fsm_branch_t
-ai__INIT_WAIT_SECOND_JACK_IN__jack_inserted_into_bot (void)
-{
- return ai_next (INIT_WAIT_SECOND_JACK_IN, jack_inserted_into_bot);
-}
-
-/*
- * INIT_WAIT_FOR_HANDS_OUT =state_timeout=>
- * => INIT_GO_TO_THE_WALL
- * fuck the wall in front.
- */
-fsm_branch_t
-ai__INIT_WAIT_FOR_HANDS_OUT__state_timeout (void)
-{
- /* Go to the wall, no backward. */
- asserv_go_to_the_wall (0);
- return ai_next (INIT_WAIT_FOR_HANDS_OUT, state_timeout);
-}
-
-/*
- * INIT_GO_TO_THE_WALL =bot_move_succeed=>
- * => INIT_GO_AWAY_FROM_THE_WALL
- * reset the Y position of the bot.
- * reset the angular position of the bot.
- * move away from the wall (linear move).
- */
-fsm_branch_t
-ai__INIT_GO_TO_THE_WALL__bot_move_succeed (void)
-{
- asserv_set_y_position (PG_Y (PG_LENGTH - BOT_SIZE_FRONT));
- asserv_set_angle_position (PG_A_DEG (90));
- /* Move away from the border. */
- asserv_move_linearly (- INIT_DIST);
- return ai_next (INIT_GO_TO_THE_WALL, bot_move_succeed);
-}
-
-/*
- * INIT_GO_AWAY_FROM_THE_WALL =bot_move_succeed=>
- * => INIT_FACE_OTHER_WALL
- * turn to face the other wall.
- */
-fsm_branch_t
-ai__INIT_GO_AWAY_FROM_THE_WALL__bot_move_succeed (void)
-{
- /* Face the other wall. */
- asserv_goto_angle (PG_A_DEG (180));
- return ai_next (INIT_GO_AWAY_FROM_THE_WALL, bot_move_succeed);
-}
-
-/*
- * INIT_FACE_OTHER_WALL =bot_move_succeed=>
- * => INIT_WAIT_AFTER_ROTATION
- * nothing to do.
- */
-fsm_branch_t
-ai__INIT_FACE_OTHER_WALL__bot_move_succeed (void)
-{
- return ai_next (INIT_FACE_OTHER_WALL, bot_move_succeed);
-}
-
-/*
- * INIT_WAIT_AFTER_ROTATION =state_timeout=>
- * => INIT_GO_TO_THE_WALL_AGAIN
- * fuck the wall in front.
- */
-fsm_branch_t
-ai__INIT_WAIT_AFTER_ROTATION__state_timeout (void)
-{
- /* Go to the wall, no backward. */
- asserv_go_to_the_wall (0);
- return ai_next (INIT_WAIT_AFTER_ROTATION, state_timeout);
-}
-
-/*
- * INIT_GO_TO_THE_WALL_AGAIN =bot_move_succeed=>
- * => INIT_GO_AWAY_FROM_THE_WALL_AGAIN
- * reset the X position of the bot.
- * move away from the wall (linear move).
- */
-fsm_branch_t
-ai__INIT_GO_TO_THE_WALL_AGAIN__bot_move_succeed (void)
-{
- asserv_set_x_position (PG_X (BOT_SIZE_FRONT));
- /* Move away from the border. */
- asserv_move_linearly (- INIT_DIST);
- return ai_next (INIT_GO_TO_THE_WALL_AGAIN, bot_move_succeed);
-}
-
-/*
- * INIT_GO_AWAY_FROM_THE_WALL_AGAIN =bot_move_succeed=>
- * => INIT_GO_TO_START_POSITION
- * go to the start position with a go to movement.
- */
-fsm_branch_t
-ai__INIT_GO_AWAY_FROM_THE_WALL_AGAIN__bot_move_succeed (void)
-{
- /* Move away from the border. */
- asserv_goto_xya (BOT_INIT_XYA, 0);
- return ai_next (INIT_GO_AWAY_FROM_THE_WALL_AGAIN, bot_move_succeed);
-}
-
-/*
- * INIT_GO_TO_START_POSITION =bot_move_succeed=>
- * => INIT_WAIT_SECOND_JACK_OUT
- * nothing to do, the bot is at the start position.
- */
-fsm_branch_t
-ai__INIT_GO_TO_START_POSITION__bot_move_succeed (void)
-{
- return ai_next (INIT_GO_TO_START_POSITION, bot_move_succeed);
-}
-
-/*
- * INIT_WAIT_SECOND_JACK_OUT =jack_removed_from_bot=>
- * => INIT_IDLE
- * tell other FSM the match begins.
- * start the chrono.
- */
-fsm_branch_t
-ai__INIT_WAIT_SECOND_JACK_OUT__jack_removed_from_bot (void)
-{
- /* Set the flag to transmit to other FSM. */
- init_match_is_started = 1;
- /* Start the chrono. */
- chrono_init ();
- return ai_next (INIT_WAIT_SECOND_JACK_OUT, jack_removed_from_bot);
-}
-
-
diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c
deleted file mode 100644
index fdb628a9..00000000
--- a/digital/io/src/ai_loader_cb.c
+++ /dev/null
@@ -1,636 +0,0 @@
-/* ai_loader_cb.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 "fsm.h"
-#include "ai_cb.h"
-#include "asserv.h"
-#include "mimot.h"
-#include "bot.h"
-#include "main.h"
-#include "food.h"
-#include "loader.h"
-
-/*
- * LOADER_IDLE =start=>
- * => LOADER_WAIT_JACK_IN
- */
-fsm_branch_t
-ai__LOADER_IDLE__start (void)
-{
- return ai_next (LOADER_IDLE, start);
-}
-
-/*
- * LOADER_WAIT_JACK_IN =jack_inserted_into_bot=>
- * => LOADER_WAIT_JACK_OUT
- */
-fsm_branch_t
-ai__LOADER_WAIT_JACK_IN__jack_inserted_into_bot (void)
-{
- return ai_next (LOADER_WAIT_JACK_IN, jack_inserted_into_bot);
-}
-
-/*
- * LOADER_WAIT_JACK_IN =hola_start=>
- * => LOADER_IDLE
- */
-fsm_branch_t
-ai__LOADER_WAIT_JACK_IN__hola_start (void)
-{
- return ai_next (LOADER_WAIT_JACK_IN, hola_start);
-}
-
-/*
- * LOADER_WAIT_JACK_OUT =jack_removed_from_bot=>
- * => LOADER_INIT_ELEVATOR_ZERO
- * find elevator zero
- * close clamp
- * find gate zero
- */
-fsm_branch_t
-ai__LOADER_WAIT_JACK_OUT__jack_removed_from_bot (void)
-{
- asserv_motor0_zero_position (-BOT_ELEVATOR_ZERO_SPEED);
- mimot_motor0_clamp (BOT_CLAMP_ZERO_SPEED, 0);
- mimot_motor1_clamp (BOT_CLAMP_ZERO_SPEED, 0);
- asserv_motor1_zero_position (-BOT_GATE_SPEED);
- return ai_next (LOADER_WAIT_JACK_OUT, jack_removed_from_bot);
-}
-
-/*
- * LOADER_INIT_ELEVATOR_ZERO =elevator_succeed=>
- * => LOADER_INIT_CLAMP_CLOSE
- */
-fsm_branch_t
-ai__LOADER_INIT_ELEVATOR_ZERO__elevator_succeed (void)
-{
- return ai_next (LOADER_INIT_ELEVATOR_ZERO, elevator_succeed);
-}
-
-/*
- * LOADER_INIT_CLAMP_CLOSE =clamp_succeed=>
- * => LOADER_INIT_CLAMP_ZERO
- * move elevator up
- * find clamp zero
- */
-fsm_branch_t
-ai__LOADER_INIT_CLAMP_CLOSE__clamp_succeed (void)
-{
- asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP,
- BOT_ELEVATOR_ZERO_SPEED);
- mimot_motor0_zero_position (-BOT_CLAMP_ZERO_SPEED);
- mimot_motor1_zero_position (-BOT_CLAMP_ZERO_SPEED);
- return ai_next (LOADER_INIT_CLAMP_CLOSE, clamp_succeed);
-}
-
-/*
- * LOADER_INIT_CLAMP_ZERO =clamp_succeed=>
- * => LOADER_INIT_GATE_ZERO
- */
-fsm_branch_t
-ai__LOADER_INIT_CLAMP_ZERO__clamp_succeed (void)
-{
- return ai_next (LOADER_INIT_CLAMP_ZERO, clamp_succeed);
-}
-
-/*
- * LOADER_INIT_GATE_ZERO =gate_succeed=>
- * => LOADER_INIT_ELEVATOR_UP
- */
-fsm_branch_t
-ai__LOADER_INIT_GATE_ZERO__gate_succeed (void)
-{
- return ai_next (LOADER_INIT_GATE_ZERO, gate_succeed);
-}
-
-/*
- * LOADER_INIT_ELEVATOR_UP =elevator_succeed=>
- * => LOADER_INIT_GATE_WAIT
- */
-fsm_branch_t
-ai__LOADER_INIT_ELEVATOR_UP__elevator_succeed (void)
-{
- return ai_next (LOADER_INIT_ELEVATOR_UP, elevator_succeed);
-}
-
-/*
- * LOADER_INIT_ELEVATOR_UP =elevator_failed=>
- * => LOADER_IDLE
- * initialisation failure
- */
-fsm_branch_t
-ai__LOADER_INIT_ELEVATOR_UP__elevator_failed (void)
-{
- /* Move so that the operator notice it. */
- asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP / 3,
- BOT_ELEVATOR_ZERO_SPEED);
- return ai_next (LOADER_INIT_ELEVATOR_UP, elevator_failed);
-}
-
-/*
- * LOADER_INIT_GATE_WAIT =state_timeout=>
- * => LOADER_INIT_GATE_CLOSE
- * close gate
- */
-fsm_branch_t
-ai__LOADER_INIT_GATE_WAIT__state_timeout (void)
-{
- asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED);
- return ai_next (LOADER_INIT_GATE_WAIT, state_timeout);
-}
-
-/*
- * LOADER_INIT_GATE_CLOSE =gate_succeed=>
- * => LOADER_UP
- */
-fsm_branch_t
-ai__LOADER_INIT_GATE_CLOSE__gate_succeed (void)
-{
- return ai_next (LOADER_INIT_GATE_CLOSE, gate_succeed);
-}
-
-/*
- * LOADER_UP =loader_down=>
- * => LOADER_DOWNING
- * move down
- */
-fsm_branch_t
-ai__LOADER_UP__loader_down (void)
-{
- asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED);
- return ai_next (LOADER_UP, loader_down);
-}
-
-/*
- * LOADER_DOWN =loader_up=>
- * => LOADER_UPING
- * move up
- */
-fsm_branch_t
-ai__LOADER_DOWN__loader_up (void)
-{
- asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED);
- return ai_next (LOADER_DOWN, loader_up);
-}
-
-/*
- * LOADER_DOWN =loader_element=>
- * => LOADER_LOAD_CLOSING
- * clamp
- */
-fsm_branch_t
-ai__LOADER_DOWN__loader_element (void)
-{
- mimot_motor0_clamp (BOT_CLAMP_SPEED, BOT_CLAMP_PWM);
- mimot_motor1_clamp (BOT_CLAMP_SPEED, BOT_CLAMP_PWM);
- return ai_next (LOADER_DOWN, loader_element);
-}
-
-/*
- * LOADER_UPING =elevator_succeed=>
- * => LOADER_UP
- * post loader_uped event
- */
-fsm_branch_t
-ai__LOADER_UPING__elevator_succeed (void)
-{
- main_post_event (AI_EVENT_loader_uped);
- return ai_next (LOADER_UPING, elevator_succeed);
-}
-
-/*
- * LOADER_UPING =elevator_failed=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_UPING__elevator_failed (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_UPING, elevator_failed);
-}
-
-/*
- * LOADER_UPING =loader_down=>
- * => LOADER_DOWNING
- * move down
- */
-fsm_branch_t
-ai__LOADER_UPING__loader_down (void)
-{
- asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED);
- return ai_next (LOADER_UPING, loader_down);
-}
-
-/*
- * LOADER_DOWNING =elevator_succeed=>
- * => LOADER_DOWN
- * release elevator motor
- * post loader_downed event
- */
-fsm_branch_t
-ai__LOADER_DOWNING__elevator_succeed (void)
-{
- asserv_motor0_free ();
- main_post_event (AI_EVENT_loader_downed);
- return ai_next (LOADER_DOWNING, elevator_succeed);
-}
-
-/*
- * LOADER_DOWNING =elevator_failed=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_DOWNING__elevator_failed (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_DOWNING, elevator_failed);
-}
-
-/*
- * LOADER_DOWNING =loader_element=>
- * => LOADER_ERROR
- * emergency stop
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_DOWNING__loader_element (void)
-{
- asserv_motor0_free ();
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_DOWNING, loader_element);
-}
-
-/*
- * LOADER_DOWNING =loader_up=>
- * => LOADER_UPING
- * move up
- */
-fsm_branch_t
-ai__LOADER_DOWNING__loader_up (void)
-{
- asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED);
- return ai_next (LOADER_DOWNING, loader_up);
-}
-
-/*
- * LOADER_ERROR =loader_down=>
- * => LOADER_ERROR_DOWNING
- * move down
- */
-fsm_branch_t
-ai__LOADER_ERROR__loader_down (void)
-{
- asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED);
- return ai_next (LOADER_ERROR, loader_down);
-}
-
-/*
- * LOADER_ERROR =loader_up=>
- * => LOADER_ERROR_UPING
- * move up
- */
-fsm_branch_t
-ai__LOADER_ERROR__loader_up (void)
-{
- asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP, BOT_ELEVATOR_SPEED);
- return ai_next (LOADER_ERROR, loader_up);
-}
-
-/*
- * LOADER_ERROR_DOWNING =elevator_succeed=>
- * => LOADER_ERROR_DOWNING_OPEN
- * release elevator motor
- * open clamp
- */
-fsm_branch_t
-ai__LOADER_ERROR_DOWNING__elevator_succeed (void)
-{
- asserv_motor0_free ();
- mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- return ai_next (LOADER_ERROR_DOWNING, elevator_succeed);
-}
-
-/*
- * LOADER_ERROR_DOWNING =elevator_failed=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_ERROR_DOWNING__elevator_failed (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_ERROR_DOWNING, elevator_failed);
-}
-
-/*
- * LOADER_ERROR_DOWNING =state_timeout=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_ERROR_DOWNING__state_timeout (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_ERROR_DOWNING, state_timeout);
-}
-
-/*
- * LOADER_ERROR_DOWNING_OPEN =clamp_succeed=>
- * => LOADER_DOWN
- * post loader_downed event
- */
-fsm_branch_t
-ai__LOADER_ERROR_DOWNING_OPEN__clamp_succeed (void)
-{
- main_post_event (AI_EVENT_loader_downed);
- return ai_next (LOADER_ERROR_DOWNING_OPEN, clamp_succeed);
-}
-
-/*
- * LOADER_ERROR_DOWNING_OPEN =clamp_failed=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_ERROR_DOWNING_OPEN__clamp_failed (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_ERROR_DOWNING_OPEN, clamp_failed);
-}
-
-/*
- * LOADER_ERROR_DOWNING_OPEN =state_timeout=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_ERROR_DOWNING_OPEN__state_timeout (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_ERROR_DOWNING_OPEN, state_timeout);
-}
-
-/*
- * LOADER_ERROR_UPING =elevator_succeed=>
- * => LOADER_ERROR_UPING_OPEN
- * open clamp
- */
-fsm_branch_t
-ai__LOADER_ERROR_UPING__elevator_succeed (void)
-{
- mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- return ai_next (LOADER_ERROR_UPING, elevator_succeed);
-}
-
-/*
- * LOADER_ERROR_UPING =elevator_failed=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_ERROR_UPING__elevator_failed (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_ERROR_UPING, elevator_failed);
-}
-
-/*
- * LOADER_ERROR_UPING =state_timeout=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_ERROR_UPING__state_timeout (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_ERROR_UPING, state_timeout);
-}
-
-/*
- * LOADER_ERROR_UPING_OPEN =clamp_succeed=>
- * => LOADER_UP
- * post loader_uped event
- */
-fsm_branch_t
-ai__LOADER_ERROR_UPING_OPEN__clamp_succeed (void)
-{
- main_post_event (AI_EVENT_loader_uped);
- return ai_next (LOADER_ERROR_UPING_OPEN, clamp_succeed);
-}
-
-/*
- * LOADER_ERROR_UPING_OPEN =clamp_failed=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_ERROR_UPING_OPEN__clamp_failed (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_ERROR_UPING_OPEN, clamp_failed);
-}
-
-/*
- * LOADER_ERROR_UPING_OPEN =state_timeout=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_ERROR_UPING_OPEN__state_timeout (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_ERROR_UPING_OPEN, state_timeout);
-}
-
-/*
- * LOADER_LOAD_CLOSING =clamp_succeed=>
- * full => LOADER_LOAD_UPING
- * move up
- * count one element
- * empty => LOADER_LOAD_EMPTY_OPEN
- * open clamp
- */
-fsm_branch_t
-ai__LOADER_LOAD_CLOSING__clamp_succeed (void)
-{
- /* Measure load using clamp position. */
- uint16_t tickness = BOT_CLAMP_WIDTH_STEP
- - mimot_get_motor0_position ()
- - mimot_get_motor1_position ();
- if (tickness > BOT_CLAMP_EMPTY_STEP)
- {
- asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP,
- BOT_ELEVATOR_SPEED);
- position_t robot_position;
- asserv_get_position (&robot_position);
- food_taken (robot_position);
- loader_elements++;
- return ai_next_branch (LOADER_LOAD_CLOSING, clamp_succeed, full);
- }
- else
- {
- mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- return ai_next_branch (LOADER_LOAD_CLOSING, clamp_succeed, empty);
- }
-}
-
-/*
- * LOADER_LOAD_UPING =elevator_unload_position=>
- * => LOADER_LOAD_UNLOADING
- * open clamp
- */
-fsm_branch_t
-ai__LOADER_LOAD_UPING__elevator_unload_position (void)
-{
- mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- return ai_next (LOADER_LOAD_UPING, elevator_unload_position);
-}
-
-/*
- * LOADER_LOAD_UPING =elevator_failed=>
- * => LOADER_ERROR
- * post loader_errored or loader_black event
- * remove one element
- * open clamp
- */
-fsm_branch_t
-ai__LOADER_LOAD_UPING__elevator_failed (void)
-{
- if (asserv_get_motor0_position () < BOT_ELEVATOR_BLACK_THRESHOLD_STEP)
- {
- if (loader_elements)
- loader_elements--;
- main_post_event (AI_EVENT_loader_black);
- }
- else
- main_post_event (AI_EVENT_loader_errored);
- mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- return ai_next (LOADER_LOAD_UPING, elevator_failed);
-}
-
-/*
- * LOADER_LOAD_UNLOADING =elevator_succeed=>
- * => LOADER_LOAD_UNLOADING_OPEN
- */
-fsm_branch_t
-ai__LOADER_LOAD_UNLOADING__elevator_succeed (void)
-{
- return ai_next (LOADER_LOAD_UNLOADING, elevator_succeed);
-}
-
-/*
- * LOADER_LOAD_UNLOADING =elevator_failed=>
- * => LOADER_ERROR
- * post loader_errored event
- * open clamp
- */
-fsm_branch_t
-ai__LOADER_LOAD_UNLOADING__elevator_failed (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- return ai_next (LOADER_LOAD_UNLOADING, elevator_failed);
-}
-
-/*
- * LOADER_LOAD_UNLOADING_OPEN =clamp_succeed=>
- * down => LOADER_DOWNING
- * move down
- * up => LOADER_UPING
- * move up
- */
-fsm_branch_t
-ai__LOADER_LOAD_UNLOADING_OPEN__clamp_succeed (void)
-{
- if (loader_want_up)
- {
- asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP,
- BOT_ELEVATOR_SPEED);
- return ai_next_branch (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed, up);
- }
- else
- {
- asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP,
- BOT_ELEVATOR_SPEED);
- return ai_next_branch (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed, down);
- }
-}
-
-/*
- * LOADER_LOAD_UNLOADING_OPEN =clamp_failed=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_LOAD_UNLOADING_OPEN__clamp_failed (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_LOAD_UNLOADING_OPEN, clamp_failed);
-}
-
-/*
- * LOADER_LOAD_EMPTY_OPEN =clamp_succeed=>
- * down => LOADER_DOWN
- * post loader_downed event
- * up => LOADER_UPING
- * move up
- */
-fsm_branch_t
-ai__LOADER_LOAD_EMPTY_OPEN__clamp_succeed (void)
-{
- if (loader_want_up)
- {
- asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP,
- BOT_ELEVATOR_SPEED);
- return ai_next_branch (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, up);
- }
- else
- {
- main_post_event (AI_EVENT_loader_downed);
- return ai_next_branch (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, down);
- }
-}
-
-/*
- * LOADER_LOAD_EMPTY_OPEN =clamp_failed=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_LOAD_EMPTY_OPEN__clamp_failed (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_LOAD_EMPTY_OPEN, clamp_failed);
-}
-
-
diff --git a/digital/io/src/ai_move_cb.c b/digital/io/src/ai_move_cb.c
deleted file mode 100644
index 32cdd221..00000000
--- a/digital/io/src/ai_move_cb.c
+++ /dev/null
@@ -1,648 +0,0 @@
-/* move_cb.c */
-/* {{{
- *
- * Copyright (C) 2008 Nélio Laranjeiro
- *
- * 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.
- *
- * }}}
- * move:
- * move with avoid obstacle.
- */
-#include "common.h"
-#include "fsm.h"
-#include "ai_cb.h"
-
-#include "asserv.h"
-#include "playground.h"
-#include "move.h"
-#include "bot.h"
-#include "radar.h"
-#include "events.h"
-#include "loader.h"
-
-#include "main.h" /* main_post_event_for_top_fsm */
-#include "modules/math/fixed/fixed.h" /* fixed_* */
-#include "modules/path/path.h"
-#include "modules/utils/utils.h"
-#include "modules/trace/trace.h"
-
-#include "debug.host.h"
-
-#include <math.h>
-
-#define MOVE_LOADER_UNBLOCKING_DISTANCE 70
-
-/** Used to define a grid. If robot is outside the grid, be careful. */
-#define MOVE_GRID_X 450
-#define MOVE_GRID_X_ORIGIN 150
-#define MOVE_GRID_Y 250
-#define MOVE_GRID_Y_ORIGIN 128
-#define MOVE_GRID_MARGIN 50
-
-/** Go to current step, low level function. */
-static void
-move_go (void)
-{
- vect_t dst = move_data.step;
- /* Modify final point if requested. */
- if (move_data.final_move && move_data.shorten)
- {
- /* Compute a vector from destination to robot with lenght
- * 'shorten'. */
- position_t robot_position;
- asserv_get_position (&robot_position);
- vect_t v = robot_position.v;
- vect_sub (&v, &move_data.step);
- int16_t d = vect_norm (&v);
- if (d > move_data.shorten)
- {
- vect_scale_f824 (&v, 0x1000000 / d * move_data.shorten);
- vect_translate (&dst, &v);
- }
- }
- if (move_data.step_with_angle)
- asserv_goto_xya (dst.x, dst.y, move_data.step_angle,
- move_data.step_backward);
- else
- asserv_goto (dst.x, dst.y, move_data.step_backward);
-}
-
-/** Go or rotate toward position, returns 1 for linear move, 2 for angular
- * move. */
-static uint8_t
-move_go_or_rotate (vect_t dst, uint16_t angle, uint8_t with_angle,
- uint8_t backward)
-{
- position_t robot_position;
- /* Remember step. */
- move_data.step = dst;
- move_data.step_angle = angle;
- move_data.step_with_angle = with_angle;
- move_data.step_backward = backward;
- /* Compute angle to destination. */
- asserv_get_position (&robot_position);
- vect_t v = dst; vect_sub (&v, &robot_position.v);
- uint16_t dst_angle = atan2 (v.y, v.x) * ((1l << 16) / (2 * M_PI));
- if (backward & ASSERV_BACKWARD)
- dst_angle += 0x8000;
- if ((backward & ASSERV_REVERT_OK)
- && (dst_angle ^ robot_position.a) & 0x8000)
- dst_angle += 0x8000;
- int16_t diff = dst_angle - robot_position.a;
- /* Move or rotate. */
- if (UTILS_ABS (diff) < 0x1000)
- {
- loader_down ();
- move_go ();
- return 1;
- }
- else
- {
- loader_up ();
- 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);
- }
- TRACE (TRACE_MOVE__GO_TO, dst.x, dst.y);
- /* 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 not on the grid, slow down. */
- int16_t mx = current_pos.v.x % MOVE_GRID_X;
- int16_t my = current_pos.v.y % MOVE_GRID_Y;
- if (mx < MOVE_GRID_X_ORIGIN - MOVE_GRID_MARGIN
- || mx > MOVE_GRID_X_ORIGIN + MOVE_GRID_MARGIN
- || my < MOVE_GRID_Y_ORIGIN - MOVE_GRID_MARGIN
- || my > MOVE_GRID_Y_ORIGIN + MOVE_GRID_MARGIN
- )
- move_data.slow = 1;
- /* 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);
-}
-
-/*
- * MOVE_IDLE =move_start=>
- * path_found_rotate => MOVE_ROTATING
- * rotate towards next position.
- * path_found => MOVE_MOVING
- * move to next position.
- * no_path_found => MOVE_IDLE
- * post failure event.
- */
-fsm_branch_t
-ai__MOVE_IDLE__move_start (void)
-{
- uint8_t next = move_path_init ();
- if (next)
- {
- if (next == 2)
- return ai_next_branch (MOVE_IDLE, move_start, path_found_rotate);
- else
- return ai_next_branch (MOVE_IDLE, move_start, path_found);
- }
- else
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_IDLE, move_start, no_path_found);
- }
-}
-
-/*
- * MOVE_ROTATING =bot_move_succeed=>
- * => MOVE_MOVING
- * move to next position.
- */
-fsm_branch_t
-ai__MOVE_ROTATING__bot_move_succeed (void)
-{
- loader_down ();
- move_go ();
- return ai_next (MOVE_ROTATING, bot_move_succeed);
-}
-
-/*
- * MOVE_ROTATING =bot_move_failed=>
- * => MOVE_MOVING
- * move to next position.
- */
-fsm_branch_t
-ai__MOVE_ROTATING__bot_move_failed (void)
-{
- ai__MOVE_ROTATING__bot_move_succeed ();
- return ai_next (MOVE_ROTATING, bot_move_failed);
-}
-
-/*
- * MOVE_ROTATING =state_timeout=>
- * => MOVE_MOVING
- * move to next position.
- */
-fsm_branch_t
-ai__MOVE_ROTATING__state_timeout (void)
-{
- ai__MOVE_ROTATING__bot_move_succeed ();
- return ai_next (MOVE_ROTATING, state_timeout);
-}
-
-/*
- * MOVE_ROTATING =loader_errored=>
- * => MOVE_LOADER_UNBLOCKING_UPING
- * move backward
- * loader up
- */
-fsm_branch_t
-ai__MOVE_ROTATING__loader_errored (void)
-{
- asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
- loader_up ();
- return ai_next (MOVE_ROTATING, loader_errored);
-}
-
-/*
- * MOVE_MOVING =bot_move_succeed=>
- * done => MOVE_IDLE
- * post success event.
- * path_found_rotate => MOVE_ROTATING
- * rotate towards next position.
- * path_found => MOVE_MOVING
- * move to next position.
- * no_path_found => MOVE_IDLE
- * post failure event.
- */
-fsm_branch_t
-ai__MOVE_MOVING__bot_move_succeed (void)
-{
- if (move_data.final_move)
- {
- main_post_event (AI_EVENT_move_fsm_succeed);
- return ai_next_branch (MOVE_MOVING, bot_move_succeed, done);
- }
- else
- {
- uint8_t next = move_path_next ();
- if (next == 2)
- return ai_next_branch (MOVE_MOVING, bot_move_succeed, path_found_rotate);
- else
- return ai_next_branch (MOVE_MOVING, bot_move_succeed, path_found);
- }
- //return ai_next_branch (MOVE_MOVING, bot_move_succeed, no_path_found);
-}
-
-/*
- * MOVE_MOVING =bot_move_failed=>
- * => MOVE_MOVING_BACKWARD_TO_TURN_FREELY
- * reset final_move.
- * move backward to turn freely.
- */
-fsm_branch_t
-ai__MOVE_MOVING__bot_move_failed (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 () == 1
- ? 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 () == 1 ?
- - 300 : 300);
- return ai_next (MOVE_MOVING, bot_move_failed);
-}
-
-/*
- * MOVE_MOVING =state_timeout=>
- * => MOVE_MOVING_BACKWARD_TO_TURN_FREELY
- * reset final_move.
- * move backward to turn freely.
- */
-fsm_branch_t
-ai__MOVE_MOVING__state_timeout (void)
-{
- ai__MOVE_MOVING__bot_move_failed ();
- return ai_next (MOVE_MOVING, state_timeout);
-}
-
-/*
- * MOVE_MOVING =obstacle_in_front=>
- * tryagain => MOVE_WAIT_FOR_CLEAR_PATH
- * reset final_move.
- * stop the bot.
- * tryout => MOVE_IDLE
- * stop the bot.
- * post failure event.
- */
-fsm_branch_t
-ai__MOVE_MOVING__obstacle_in_front (void)
-{
- move_data.final_move = 0;
- asserv_stop_motor ();
- if (--move_data.try_again_counter == 0)
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_MOVING, obstacle_in_front, tryout);
- }
- else
- return ai_next_branch (MOVE_MOVING, obstacle_in_front, tryagain);
-}
-
-/*
- * MOVE_MOVING =loader_errored=>
- * => MOVE_LOADER_UNBLOCKING_UPING
- * move backward
- * loader up
- */
-fsm_branch_t
-ai__MOVE_MOVING__loader_errored (void)
-{
- asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
- loader_up ();
- return ai_next (MOVE_MOVING, loader_errored);
-}
-
-/*
- * MOVE_MOVING_BACKWARD_TO_TURN_FREELY =bot_move_succeed=>
- * tryout => MOVE_IDLE
- * post failure event.
- * path_found_rotate => MOVE_ROTATING
- * rotate towards next position.
- * path_found => MOVE_MOVING
- * move to next position.
- * no_path_found => MOVE_IDLE
- * post failure event.
- */
-fsm_branch_t
-ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_succeed (void)
-{
- if (--move_data.try_again_counter == 0)
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, tryout);
- }
- else
- {
- uint8_t next = move_path_init ();
- if (next)
- {
- if (next == 2)
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, path_found_rotate);
- else
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, path_found);
- }
- else
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, no_path_found);
- }
- }
-}
-
-/*
- * MOVE_MOVING_BACKWARD_TO_TURN_FREELY =bot_move_failed=>
- * tryout => MOVE_IDLE
- * post failure event.
- * path_found_rotate => MOVE_ROTATING
- * rotate towards next position.
- * path_found => MOVE_MOVING
- * move to next position.
- * no_path_found_tryagain => MOVE_WAIT_FOR_CLEAR_PATH
- * nothing to do.
- * no_path_found_tryout => MOVE_IDLE
- * post failure event.
- */
-fsm_branch_t
-ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_failed (void)
-{
- if (--move_data.try_again_counter == 0)
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, tryout);
- }
- else
- {
- uint8_t next = move_path_init ();
- if (next)
- {
- if (next == 2)
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, path_found_rotate);
- else
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, path_found);
- }
- else
- {
- if (--move_data.try_again_counter == 0)
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_path_found_tryout);
- }
- else
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_path_found_tryagain);
- }
- }
-}
-
-/*
- * MOVE_WAIT_FOR_CLEAR_PATH =state_timeout=>
- * path_found_rotate => MOVE_ROTATING
- * rotate towards next position.
- * path_found => MOVE_MOVING
- * move to next position.
- * no_path_found_tryagain => MOVE_WAIT_FOR_CLEAR_PATH
- * decrement counter.
- * no_path_found_tryout => MOVE_IDLE
- * post failure.
- */
-fsm_branch_t
-ai__MOVE_WAIT_FOR_CLEAR_PATH__state_timeout (void)
-{
- /* Try to move. */
- uint8_t next = move_path_init ();
- if (next)
- {
- if (next == 2)
- return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, path_found_rotate);
- else
- return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, path_found);
- }
- else
- {
- /* Error, no new position, should we try again? */
- if (--move_data.try_again_counter == 0)
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, no_path_found_tryout);
- }
- else
- return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, no_path_found_tryagain);
- }
-}
-
-/*
- * MOVE_LOADER_UNBLOCKING_UPING =bot_move_succeed=>
- * => MOVE_LOADER_UNBLOCKING_DOWNING
- * loader down
- * reset unblocking retry counter
- */
-fsm_branch_t
-ai__MOVE_LOADER_UNBLOCKING_UPING__bot_move_succeed (void)
-{
- loader_down ();
- move_data.loader_unblocking_retry = 2;
- return ai_next (MOVE_LOADER_UNBLOCKING_UPING, bot_move_succeed);
-}
-
-/*
- * MOVE_LOADER_UNBLOCKING_UPING =bot_move_failed=>
- * => MOVE_LOADER_UNBLOCKING_DOWNING
- * loader down
- * reset unblocking retry counter
- */
-fsm_branch_t
-ai__MOVE_LOADER_UNBLOCKING_UPING__bot_move_failed (void)
-{
- loader_down ();
- move_data.loader_unblocking_retry = 2;
- return ai_next (MOVE_LOADER_UNBLOCKING_UPING, bot_move_failed);
-}
-
-/*
- * MOVE_LOADER_UNBLOCKING_DOWNING =loader_downed=>
- * path_found_rotate => MOVE_ROTATING
- * rotate towards next position.
- * path_found => MOVE_MOVING
- * move to next position.
- * no_path_found => MOVE_IDLE
- * post failure.
- */
-fsm_branch_t
-ai__MOVE_LOADER_UNBLOCKING_DOWNING__loader_downed (void)
-{
- /* Try to move. */
- uint8_t next = move_path_init ();
- if (next)
- {
- if (next == 2)
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, path_found_rotate);
- else
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, path_found);
- }
- else
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, no_path_found);
- }
-}
-
-/*
- * MOVE_LOADER_UNBLOCKING_DOWNING =loader_errored=>
- * tryagain => MOVE_LOADER_UNBLOCKING_UPING
- * move backward
- * loader up
- * tryout_path_found_rotate => MOVE_ROTATING
- * rotate towards next position.
- * tryout_path_found => MOVE_ROTATING
- * move to next position.
- * tryout_no_path_found => MOVE_IDLE
- * post failure.
- */
-fsm_branch_t
-ai__MOVE_LOADER_UNBLOCKING_DOWNING__loader_errored (void)
-{
- if (--move_data.loader_unblocking_retry)
- {
- asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
- loader_up ();
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryagain);
- }
- else
- {
- /* Try to move. */
- uint8_t next = move_path_init ();
- if (next)
- {
- if (next == 2)
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_path_found_rotate);
- else
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_path_found);
- }
- else
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_no_path_found);
- }
- }
-}
-
-/*
- * MOVE_LOADER_UNBLOCKING_DOWNING =state_timeout=>
- * tryagain => MOVE_LOADER_UNBLOCKING_UPING
- * move backward
- * loader up
- * tryout_path_found_rotate => MOVE_ROTATING
- * rotate towards next position.
- * tryout_path_found => MOVE_ROTATING
- * move to next position.
- * tryout_no_path_found => MOVE_IDLE
- * post failure.
- */
-fsm_branch_t
-ai__MOVE_LOADER_UNBLOCKING_DOWNING__state_timeout (void)
-{
- if (--move_data.loader_unblocking_retry)
- {
- asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
- loader_up ();
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, state_timeout, tryagain);
- }
- else
- {
- /* Try to move. */
- uint8_t next = move_path_init ();
- if (next)
- {
- if (next == 2)
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, state_timeout, tryout_path_found_rotate);
- else
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, state_timeout, tryout_path_found);
- }
- else
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, state_timeout, tryout_no_path_found);
- }
- }
-}
-
-
diff --git a/digital/io/src/ai_top_cb.c b/digital/io/src/ai_top_cb.c
deleted file mode 100644
index ed1ae038..00000000
--- a/digital/io/src/ai_top_cb.c
+++ /dev/null
@@ -1,434 +0,0 @@
-/* ai_top_cb.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.
- *
- * }}}
- * Main FSM calling other FSM.
- */
-#include "common.h"
-#include "fsm.h"
-#include "ai_cb.h"
-#include "top.h"
-#include "move.h"
-#include "chrono.h"
-#include "playground.h"
-#include "asserv.h"
-#include "loader.h"
-#include "food.h"
-
-/*
- * IDLE =start=>
- * => WAIT_INIT_TO_FINISH
- * nothing to do.
- */
-fsm_branch_t
-ai__IDLE__start (void)
-{
- return ai_next (IDLE, start);
-}
-
-/*
- * WAIT_INIT_TO_FINISH =init_match_is_started=>
- * => FIRST_GO_BEGIN_OF_LINE_FAST
- * the match start
- * set fast speed
- * go to first sequence begin of line
- */
-fsm_branch_t
-ai__WAIT_INIT_TO_FINISH__init_match_is_started (void)
-{
- asserv_set_speed (BOT_MOVE_FAST);
- move_start_noangle (PG_VECT (375, 1503), 0, 0);
- return ai_next (WAIT_INIT_TO_FINISH, init_match_is_started);
-}
-
-/*
- * FIRST_GO_BEGIN_OF_LINE_FAST =move_fsm_succeed=>
- * => FIRST_GO_END_OF_LINE_FAST
- * go to end of line
- * loader down
- */
-fsm_branch_t
-ai__FIRST_GO_BEGIN_OF_LINE_FAST__move_fsm_succeed (void)
-{
- move_start_noangle (PG_VECT (2625, 253), 0, 0);
- loader_down ();
- return ai_next (FIRST_GO_BEGIN_OF_LINE_FAST, move_fsm_succeed);
-}
-
-/*
- * FIRST_GO_BEGIN_OF_LINE_FAST =move_fsm_failed=>
- * => FIRST_GO_BEGIN_OF_LINE_FAST
- * set slow speed
- * retry
- */
-fsm_branch_t
-ai__FIRST_GO_BEGIN_OF_LINE_FAST__move_fsm_failed (void)
-{
- asserv_set_speed (BOT_MOVE_SLOW);
- move_start_noangle (PG_VECT (375, 1503), 0, 0);
- return ai_next (FIRST_GO_BEGIN_OF_LINE_FAST, move_fsm_failed);
-}
-
-/*
- * FIRST_GO_END_OF_LINE_FAST =in_field=>
- * => FIRST_GO_END_OF_LINE_SLOW
- * set slow speed
- */
-fsm_branch_t
-ai__FIRST_GO_END_OF_LINE_FAST__in_field (void)
-{
- asserv_set_speed (BOT_MOVE_SLOW);
- return ai_next (FIRST_GO_END_OF_LINE_FAST, in_field);
-}
-
-/*
- * FIRST_GO_END_OF_LINE_FAST =move_fsm_succeed=>
- * => UNLOAD_LOADER_UP
- * set slow speed
- * move loader up
- */
-fsm_branch_t
-ai__FIRST_GO_END_OF_LINE_FAST__move_fsm_succeed (void)
-{
- asserv_set_speed (BOT_MOVE_SLOW);
- ai__FIRST_GO_END_OF_LINE_SLOW__move_fsm_succeed ();
- return ai_next (FIRST_GO_END_OF_LINE_FAST, move_fsm_succeed);
-}
-
-/*
- * FIRST_GO_END_OF_LINE_FAST =move_fsm_failed=>
- * => FIRST_GO_END_OF_LINE_SLOW
- * set slow speed
- * retry
- */
-fsm_branch_t
-ai__FIRST_GO_END_OF_LINE_FAST__move_fsm_failed (void)
-{
- asserv_set_speed (BOT_MOVE_SLOW);
- ai__FIRST_GO_END_OF_LINE_SLOW__move_fsm_failed ();
- return ai_next (FIRST_GO_END_OF_LINE_FAST, move_fsm_failed);
-}
-
-/*
- * FIRST_GO_END_OF_LINE_SLOW =move_fsm_succeed=>
- * => UNLOAD_LOADER_UP
- * move loader up
- */
-fsm_branch_t
-ai__FIRST_GO_END_OF_LINE_SLOW__move_fsm_succeed (void)
-{
- loader_up ();
- return ai_next (FIRST_GO_END_OF_LINE_SLOW, move_fsm_succeed);
-}
-
-/*
- * FIRST_GO_END_OF_LINE_SLOW =move_fsm_failed=>
- * => FIRST_GO_END_OF_LINE_SLOW
- * retry
- */
-fsm_branch_t
-ai__FIRST_GO_END_OF_LINE_SLOW__move_fsm_failed (void)
-{
- move_start_noangle (PG_VECT (2625, 253), 0, 0);
- return ai_next (FIRST_GO_END_OF_LINE_SLOW, move_fsm_failed);
-}
-
-/*
- * UNLOAD =move_fsm_succeed=>
- * => UNLOAD_LOADER_UP
- * move loader up
- */
-fsm_branch_t
-ai__UNLOAD__move_fsm_succeed (void)
-{
- loader_up ();
- return ai_next (UNLOAD, move_fsm_succeed);
-}
-
-/*
- * UNLOAD =move_fsm_failed=>
- * => UNLOAD
- * retry
- */
-fsm_branch_t
-ai__UNLOAD__move_fsm_failed (void)
-{
- move_start_noangle (PG_VECT (2625, 253), 0, 0);
- return ai_next (UNLOAD, move_fsm_failed);
-}
-
-/*
- * UNLOAD_LOADER_UP =loader_uped=>
- * => UNLOAD_FACE_BIN
- * turn toward bin
- */
-fsm_branch_t
-ai__UNLOAD_LOADER_UP__loader_uped (void)
-{
- asserv_goto_angle (PG_A_DEG (90));
- return ai_next (UNLOAD_LOADER_UP, loader_uped);
-}
-
-/*
- * UNLOAD_LOADER_UP =loader_errored=>
- * => UNLOAD_FACE_BIN
- * turn toward bin
- */
-fsm_branch_t
-ai__UNLOAD_LOADER_UP__loader_errored (void)
-{
- asserv_goto_angle (PG_A_DEG (90));
- return ai_next (UNLOAD_LOADER_UP, loader_errored);
-}
-
-/*
- * UNLOAD_FACE_BIN =bot_move_succeed=>
- * => UNLOAD_BACK_BIN
- * go backward to bin
- */
-fsm_branch_t
-ai__UNLOAD_FACE_BIN__bot_move_succeed (void)
-{
- asserv_move_linearly (-(128 + 250 / 2 - BOT_SIZE_BACK - 50));
- return ai_next (UNLOAD_FACE_BIN, bot_move_succeed);
-}
-
-/*
- * UNLOAD_FACE_BIN =bot_move_failed=>
- * => UNLOAD_FACE_BIN_UNBLOCK
- * move backward
- */
-fsm_branch_t
-ai__UNLOAD_FACE_BIN__bot_move_failed (void)
-{
- asserv_move_linearly (-40);
- return ai_next (UNLOAD_FACE_BIN, bot_move_failed);
-}
-
-/*
- * UNLOAD_FACE_BIN_UNBLOCK =bot_move_succeed=>
- * => UNLOAD_FACE_BIN
- * turn toward bin
- */
-fsm_branch_t
-ai__UNLOAD_FACE_BIN_UNBLOCK__bot_move_succeed (void)
-{
- asserv_goto_angle (PG_A_DEG (90));
- return ai_next (UNLOAD_FACE_BIN_UNBLOCK, bot_move_succeed);
-}
-
-/*
- * UNLOAD_FACE_BIN_UNBLOCK =bot_move_failed=>
- * => UNLOAD_FACE_BIN
- * turn toward bin
- */
-fsm_branch_t
-ai__UNLOAD_FACE_BIN_UNBLOCK__bot_move_failed (void)
-{
- asserv_goto_angle (PG_A_DEG (90));
- return ai_next (UNLOAD_FACE_BIN_UNBLOCK, bot_move_failed);
-}
-
-/*
- * UNLOAD_BACK_BIN =bot_move_succeed=>
- * => UNLOAD_UNLOAD
- * unload
- */
-fsm_branch_t
-ai__UNLOAD_BACK_BIN__bot_move_succeed (void)
-{
- asserv_move_motor1_absolute (0, BOT_GATE_SPEED);
- return ai_next (UNLOAD_BACK_BIN, bot_move_succeed);
-}
-
-/*
- * UNLOAD_BACK_BIN =bot_move_failed=>
- * => UNLOAD_UNLOAD
- * unload
- */
-fsm_branch_t
-ai__UNLOAD_BACK_BIN__bot_move_failed (void)
-{
- ai__UNLOAD_BACK_BIN__bot_move_succeed ();
- return ai_next (UNLOAD_BACK_BIN, bot_move_failed);
-}
-
-/*
- * UNLOAD_UNLOAD =state_timeout=>
- * => COLLECT
- * close gate
- * loader down
- * choose best food to collect
- */
-fsm_branch_t
-ai__UNLOAD_UNLOAD__state_timeout (void)
-{
- loader_elements = 0;
- asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED);
- loader_down ();
- top_collect (1);
- return ai_next (UNLOAD_UNLOAD, state_timeout);
-}
-
-/*
- * COLLECT =move_fsm_succeed=>
- * unload => UNLOAD
- * collect => COLLECT
- * slow_motion => COLLECT_SLOW_MOTION
- * post loader_element event
- */
-fsm_branch_t
-ai__COLLECT__move_fsm_succeed (void)
-{
- int16_t slow_motion = food_slow_motion (top_food);
- if (slow_motion)
- {
- fsm_handle_event (&ai_fsm, AI_EVENT_loader_element);
- return ai_next_branch (COLLECT, move_fsm_succeed, slow_motion);
- }
- else if (top_collect (0))
- return ai_next_branch (COLLECT, move_fsm_succeed, collect);
- else
- return ai_next_branch (COLLECT, move_fsm_succeed, unload);
-}
-
-/*
- * COLLECT =move_fsm_failed=>
- * unload => UNLOAD
- * collect => COLLECT
- * cancel this element
- */
-fsm_branch_t
-ai__COLLECT__move_fsm_failed (void)
-{
- if (top_collect (0))
- {
- food_cancel (top_food);
- return ai_next_branch (COLLECT, move_fsm_failed, collect);
- }
- else
- return ai_next_branch (COLLECT, move_fsm_failed, unload);
-}
-
-/*
- * COLLECT =loader_black=>
- * => COLLECT_BLACK
- * move backward
- * mark as black
- */
-fsm_branch_t
-ai__COLLECT__loader_black (void)
-{
- asserv_move_linearly (-90);
- food_black (top_food);
- return ai_next (COLLECT, loader_black);
-}
-
-/*
- * COLLECT_SLOW_MOTION =loader_downed=>
- * unload => UNLOAD
- * collect => COLLECT
- * collect
- */
-fsm_branch_t
-ai__COLLECT_SLOW_MOTION__loader_downed (void)
-{
- if (top_collect (0))
- return ai_next_branch (COLLECT_SLOW_MOTION, loader_downed, collect);
- else
- return ai_next_branch (COLLECT_SLOW_MOTION, loader_downed, unload);
-}
-
-/*
- * COLLECT_SLOW_MOTION =loader_errored=>
- * unload => UNLOAD
- * collect => COLLECT
- * same as above
- */
-fsm_branch_t
-ai__COLLECT_SLOW_MOTION__loader_errored (void)
-{
- if (top_collect (0))
- return ai_next_branch (COLLECT_SLOW_MOTION, loader_errored, collect);
- else
- return ai_next_branch (COLLECT_SLOW_MOTION, loader_errored, unload);
-}
-
-/*
- * COLLECT_SLOW_MOTION =state_timeout=>
- * unload => UNLOAD
- * collect => COLLECT
- * same as above
- */
-fsm_branch_t
-ai__COLLECT_SLOW_MOTION__state_timeout (void)
-{
- if (top_collect (0))
- return ai_next_branch (COLLECT_SLOW_MOTION, state_timeout, collect);
- else
- return ai_next_branch (COLLECT_SLOW_MOTION, state_timeout, unload);
-}
-
-/*
- * COLLECT_SLOW_MOTION =loader_black=>
- * => COLLECT_BLACK
- * move backward
- * mark as black
- */
-fsm_branch_t
-ai__COLLECT_SLOW_MOTION__loader_black (void)
-{
- asserv_move_linearly (-90);
- food_black (top_food);
- return ai_next (COLLECT_SLOW_MOTION, loader_black);
-}
-
-/*
- * COLLECT_BLACK =bot_move_succeed=>
- * unload => UNLOAD
- * collect => COLLECT
- */
-fsm_branch_t
-ai__COLLECT_BLACK__bot_move_succeed (void)
-{
- if (top_collect (0))
- return ai_next_branch (COLLECT_BLACK, bot_move_succeed, collect);
- else
- return ai_next_branch (COLLECT_BLACK, bot_move_succeed, unload);
-}
-
-/*
- * COLLECT_BLACK =bot_move_failed=>
- * unload => UNLOAD
- * collect => COLLECT
- */
-fsm_branch_t
-ai__COLLECT_BLACK__bot_move_failed (void)
-{
- if (top_collect (0))
- return ai_next_branch (COLLECT_BLACK, bot_move_failed, collect);
- else
- return ai_next_branch (COLLECT_BLACK, bot_move_failed, unload);
-}
-
-
diff --git a/digital/io/src/bot.h b/digital/io/src/bot.h
index 06f6559b..b4112e92 100644
--- a/digital/io/src/bot.h
+++ b/digital/io/src/bot.h
@@ -30,11 +30,6 @@
*/
/**
- * Duration of a match in milliseconds, with margin.
- */
-#define MATCH_DURATION_MS (90000 - 2500)
-
-/**
* The scaling factor, millimeter per step.
*/
#ifdef HOST
@@ -128,18 +123,4 @@
/** Gate stroke in steps. */
#define BOT_GATE_STROKE_STEP -0x203e
-/**
- * Definition of the colors.
- */
-enum team_color_e
-{
- RED_TEAM = 0,
- BLUE_TEAM = 1
-};
-
-/**
- * Our color.
- */
-extern enum team_color_e bot_color;
-
#endif /* bot_h */
diff --git a/digital/io/src/fsm.c b/digital/io/src/fsm.c
deleted file mode 100644
index 45eeb5ba..00000000
--- a/digital/io/src/fsm.c
+++ /dev/null
@@ -1,137 +0,0 @@
-/* fsm.c - Finite State Machine code. */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
- *
- * Copyright (C) 2008 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 "fsm.h"
-
-#include "modules/trace/trace.h"
-#include "modules/proto/proto.h"
-#include "events.h"
-
-#ifdef HOST
-# include <stdio.h>
-#endif
-
-void
-fsm_init (fsm_t *fsm)
-{
- uint8_t i;
- assert (fsm);
- for (i = 0; i < FSM_ACTIVE_STATES_MAX; i++)
- fsm->states_active[i] = fsm->states_init[i];
- for (i = 0; i < fsm->active_states_nb; i++)
- fsm->states_timeout[i] =
- fsm->state_timeout_table[fsm->states_init[i]];
-}
-
-/** Handle an event on the given FSM on a single active state, return 1 if the
- * event was handled. */
-static uint8_t
-fsm_handle_event_single (fsm_t *fsm, uint8_t active, uint8_t event)
-{
- assert (fsm);
- assert (active < fsm->active_states_nb);
- assert (event < fsm->events_nb);
- /* Lookup transition. */
- uint8_t old_state = fsm->states_active[active];
- fsm_transition_t tr = FSM_TRANSITION (fsm, old_state, event);
- /* Ignore unhandled events. */
- if (tr)
- {
- /* Execute transition. */
- uint8_t new_state;
- fsm_branch_t br = tr ();
- /* Change state. */
-#ifdef HOST
- assert (((br >> 16) & 0xff) == fsm->states_active[active]);
- assert (((br >> 8) & 0xff) == event);
- fprintf (stderr, "%s %s =%s=> %s\n", fsm->name,
- fsm->states_names[fsm->states_active[active]],
- fsm->events_names[event], fsm->states_names[br & 0xff]);
- new_state = br & 0xff;
-#else
- new_state = br;
- proto_send3b ('f', old_state, event, new_state);
-#endif
- TRACE (TRACE_FSM__HANDLE_EVENT, (u8) old_state, (u8) event,
- (u8) new_state);
- fsm->states_active[active] = new_state;
- fsm->states_timeout[active] = fsm->state_timeout_table[new_state];
- return 1;
- }
- return 0;
-}
-
-uint8_t
-fsm_handle_event (fsm_t *fsm, uint8_t event)
-{
- uint8_t i, handled = 0;
- assert (fsm);
- assert (event < fsm->events_nb);
- for (i = 0; i < fsm->active_states_nb; i++)
- {
- /* Handle the event for this active state. */
- handled += fsm_handle_event_single (fsm, i, event);
- }
- return handled;
-}
-
-uint8_t
-fsm_handle_timeout (fsm_t *fsm)
-{
- uint8_t i, handled = 0;
- assert (fsm);
- for (i = 0; i < fsm->active_states_nb; i++)
- {
- /* If there is a timeout for this state. */
- if (fsm->states_timeout[i] != 0xffff)
- {
- if (fsm->states_timeout[i])
- fsm->states_timeout[i]--;
- else
- {
- /* Timeout expired, generate corresponding event. */
- handled += fsm_handle_event_single (fsm, i,
- fsm->state_timeout_event);
- }
- }
- }
- return handled;
-}
-
-uint8_t
-fsm_can_handle_event (fsm_t *fsm, uint8_t event)
-{
- uint8_t i;
- assert (fsm);
- assert (event < fsm->events_nb);
- for (i = 0; i < fsm->active_states_nb; i++)
- {
- uint8_t state = fsm->states_active[i];
- if (FSM_TRANSITION (fsm, state, event))
- return 1;
- }
- return 0;
-}
-
diff --git a/digital/io/src/fsm.h b/digital/io/src/fsm.h
deleted file mode 100644
index 2d0e1933..00000000
--- a/digital/io/src/fsm.h
+++ /dev/null
@@ -1,126 +0,0 @@
-#ifndef fsm_h
-#define fsm_h
-/* fsm.h - Finite State Machine code. */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
- *
- * Copyright (C) 2008 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.
- *
- * }}} */
-
-#ifndef HOST
-
-# include <avr/pgmspace.h>
-
-#else
-
-# define PROGMEM
-# define pgm_read_word(addr) (*(addr))
-
-#endif
-
-/**
- * The io board includes an FSM with several concurrent active states.
- *
- * The main loop tests for each possible event and generate the corresponding
- * FSM events. For example:
- *
- * if (asserv_move_cmd_status () == success)
- * fsm_handle_event (&ai_fsm, AI_EVENT_position_reached);
- *
- * Any unhandled event will be ignored.
- *
- * To start a sub machine from the top FSM, call the sub machine start
- * function (for example move_start), which will set parameters, then trigger
- * its start event.
- *
- * The sub machine is expected to generate an event for the top machine to
- * make it continue.
- */
-
-#ifdef HOST
-/** On host, check branches. */
-typedef u32 fsm_branch_t;
-#else
-/** On AVR, no check. */
-typedef u8 fsm_branch_t;
-#endif
-
-/** Transition function. */
-typedef fsm_branch_t (*fsm_transition_t) (void);
-
-/** Maximum number of active states. */
-#define FSM_ACTIVE_STATES_MAX 5
-
-/** Context of an FSM. */
-struct fsm_t
-{
- /** Transition table. */
- const fsm_transition_t *transition_table;
- /** Number of events, used to index the right transition. */
- u8 events_nb;
- /** Initial states. */
- u8 states_init[FSM_ACTIVE_STATES_MAX];
- /** Current active states. */
- u8 states_active[FSM_ACTIVE_STATES_MAX];
- /** Nomber of active states. */
- u8 active_states_nb;
- /** State timeout table. */
- const u16 *state_timeout_table;
- /** Current active states timeout if not 0xffff. */
- u16 states_timeout[FSM_ACTIVE_STATES_MAX];
- /** Event to generate on state timeout. */
- u8 state_timeout_event;
-#ifdef HOST
- /** FSM name. */
- const char *name;
- /** States names. */
- const char * const *states_names;
- /** Events names. */
- const char * const *events_names;
-#endif
-};
-typedef struct fsm_t fsm_t;
-
-#define FSM_TRANSITION(fsm, state, event) \
- ((fsm_transition_t) pgm_read_word ( \
- &(fsm)->transition_table[(state) * (fsm)->events_nb + (event)]))
-
-/** Reset a FSM. */
-void
-fsm_init (fsm_t *fsm);
-
-/** Handle state timeout, return non-zero if at least an event was handled. */
-uint8_t
-fsm_handle_timeout (fsm_t *fsm);
-
-/** Handle an event on the given FSM, return non-zero if at least an event was
- * handled. */
-uint8_t
-fsm_handle_event (fsm_t *fsm, uint8_t event);
-
-/** Return non zero if event can be handled. This can be used if event
- * generation requires expensive computations. */
-uint8_t
-fsm_can_handle_event (fsm_t *fsm, uint8_t event);
-
-#include "ai_fsm.h"
-
-#endif /* fsm_h */
diff --git a/digital/io/src/fsm_templates/template_cb.h b/digital/io/src/fsm_templates/template_cb.h
deleted file mode 100644
index 4f4deeec..00000000
--- a/digital/io/src/fsm_templates/template_cb.h
+++ /dev/null
@@ -1,10 +0,0 @@
-#ifndef %(prefix)s_cb_h
-#define %(prefix)s_cb_h
-/*
- * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT!
- *
- * %(name)s callbacks declaration.
- *
-%(*comments)s */
-
-%(states_template,template_cb_decl.h)s#endif /* %(prefix)s_cb_h */
diff --git a/digital/io/src/fsm_templates/template_cb_decl.h b/digital/io/src/fsm_templates/template_cb_decl.h
deleted file mode 100644
index fd72d678..00000000
--- a/digital/io/src/fsm_templates/template_cb_decl.h
+++ /dev/null
@@ -1,6 +0,0 @@
-/*
- * %(state)s =%(event)s=>
-%(*branches_to)s */
-fsm_branch_t
-%(prefix)s__%(state)s__%(event)s (void);
-
diff --git a/digital/io/src/fsm_templates/template_cb_impl.c b/digital/io/src/fsm_templates/template_cb_impl.c
deleted file mode 100644
index 91ff4fbb..00000000
--- a/digital/io/src/fsm_templates/template_cb_impl.c
+++ /dev/null
@@ -1,9 +0,0 @@
-/*
- * %(state)s =%(event)s=>
-%(*branches_to)s */
-fsm_branch_t
-%(prefix)s__%(state)s__%(event)s (void)
-{
-%(returns)s
-}
-
diff --git a/digital/io/src/fsm_templates/template_fsm.c b/digital/io/src/fsm_templates/template_fsm.c
deleted file mode 100644
index 03ce5abf..00000000
--- a/digital/io/src/fsm_templates/template_fsm.c
+++ /dev/null
@@ -1,55 +0,0 @@
-/*
- * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT!
- *
- * %(name)s
- *
-%(*comments)s */
-#include "common.h"
-#include "fsm.h"
-#include "%(prefix)s_cb.h"
-
-#define NULL ((void *)0L)
-
-/* %(name)s transition table. */
-static const fsm_transition_t PROGMEM
-%(prefix)s_transition_table[%(PREFIX)s_STATE_NB][%(PREFIX)s_EVENT_NB] = {
-%(transition_table)s};
-
-/* %(name)s state timeout table. */
-static const uint16_t
-%(prefix)s_state_timeout_table[%(PREFIX)s_STATE_NB] = {
-%(states,%(@timeout|0xffff)s)s};
-
-#ifdef HOST
-
-/* %(name)s state names. */
-static const char *
-%(prefix)s_states_names[] = {
-%(states_names)s};
-
-/* %(name)s event names. */
-static const char *
-%(prefix)s_events_names[] = {
-%(events_names)s};
-
-#endif /* HOST */
-
-/* %(name)s context. */
-fsm_t %(prefix)s_fsm = {
- &%(prefix)s_transition_table[0][0],
- %(PREFIX)s_EVENT_NB,
- {
- %(initials)s},
- {
- %(initials)s},
- %(initials_nb)s,
- &%(prefix)s_state_timeout_table[0],
- { 0 },
- %(PREFIX)s_EVENT_state_timeout,
-#ifdef HOST
- "%(name)s",
- %(prefix)s_states_names,
- %(prefix)s_events_names,
-#endif
-};
-
diff --git a/digital/io/src/fsm_templates/template_fsm.h b/digital/io/src/fsm_templates/template_fsm.h
deleted file mode 100644
index 9ac98895..00000000
--- a/digital/io/src/fsm_templates/template_fsm.h
+++ /dev/null
@@ -1,52 +0,0 @@
-#ifndef %(prefix)s_fsm_h
-#define %(prefix)s_fsm_h
-/*
- * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT!
- *
- * %(name)s
- *
-%(*comments)s */
-
-/* %(name)s states. */
-enum %(prefix)s_state_t
-{
-%(states)s %(PREFIX)s_STATE_NB
-};
-
-/* %(name)s events. */
-enum %(prefix)s_event_t
-{
-%(events)s %(PREFIX)s_EVENT_NB
-};
-
-/* This macro enables checks for branches used in the wrong state/event
- * combination. */
-#ifdef HOST
-# define _BRANCH(state, event, to) \
- ((%(PREFIX)s_STATE_ ## state) << 16 \
- | (%(PREFIX)s_EVENT_ ## event) << 8 \
- | (%(PREFIX)s_STATE_ ## to))
-#else
-# define _BRANCH(state, event, to) \
- ((%(PREFIX)s_STATE_ ## to))
-#endif
-
-/* %(name)s branches. */
-enum %(prefix)s_branch_t
-{
-%(branches)s};
-
-#undef _BRANCH
-
-/* Value to return to follow the only branch. */
-#define %(prefix)s_next(state, event) \
- %(PREFIX)s_BRANCH__ ## state ## __ ## event ## __
-
-/* Value to return to follow a given branch. */
-#define %(prefix)s_next_branch(state, event, branch) \
- %(PREFIX)s_BRANCH__ ## state ## __ ## event ## __ ## branch
-
-/* %(name)s context. */
-extern fsm_t %(prefix)s_fsm;
-
-#endif /* %(prefix)s_fsm_h */
diff --git a/digital/io/src/fsm_templates/template_hola_cb_skel.c b/digital/io/src/fsm_templates/template_hola_cb_skel.c
deleted file mode 100644
index 980718da..00000000
--- a/digital/io/src/fsm_templates/template_hola_cb_skel.c
+++ /dev/null
@@ -1,11 +0,0 @@
-/*
- * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT!
- *
- * Skeleton for %(name)s hola callbacks implementation.
- *
-%(*comments)s */
-#include "common.h"
-#include "fsm.h"
-#include "%(prefix)s_cb.h"
-
-%(states_template,template_cb_impl.c,origin=hola)s
diff --git a/digital/io/src/fsm_templates/template_init_cb_skel.c b/digital/io/src/fsm_templates/template_init_cb_skel.c
deleted file mode 100644
index fea9270e..00000000
--- a/digital/io/src/fsm_templates/template_init_cb_skel.c
+++ /dev/null
@@ -1,11 +0,0 @@
-/*
- * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT!
- *
- * Skeleton for %(name)s init callbacks implementation.
- *
-%(*comments)s */
-#include "common.h"
-#include "fsm.h"
-#include "%(prefix)s_cb.h"
-
-%(states_template,template_cb_impl.c,origin=init)s
diff --git a/digital/io/src/fsm_templates/template_loader_cb_skel.c b/digital/io/src/fsm_templates/template_loader_cb_skel.c
deleted file mode 100644
index 06398952..00000000
--- a/digital/io/src/fsm_templates/template_loader_cb_skel.c
+++ /dev/null
@@ -1,11 +0,0 @@
-/*
- * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT!
- *
- * Skeleton for %(name)s loader callbacks implementation.
- *
-%(*comments)s */
-#include "common.h"
-#include "fsm.h"
-#include "%(prefix)s_cb.h"
-
-%(states_template,template_cb_impl.c,origin=loader)s
diff --git a/digital/io/src/fsm_templates/template_move_cb_skel.c b/digital/io/src/fsm_templates/template_move_cb_skel.c
deleted file mode 100644
index 533ffbfd..00000000
--- a/digital/io/src/fsm_templates/template_move_cb_skel.c
+++ /dev/null
@@ -1,11 +0,0 @@
-/*
- * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT!
- *
- * Skeleton for %(name)s move callbacks implementation.
- *
-%(*comments)s */
-#include "common.h"
-#include "fsm.h"
-#include "%(prefix)s_cb.h"
-
-%(states_template,template_cb_impl.c,origin=move)s
diff --git a/digital/io/src/fsm_templates/template_top_cb_skel.c b/digital/io/src/fsm_templates/template_top_cb_skel.c
deleted file mode 100644
index 8895dc64..00000000
--- a/digital/io/src/fsm_templates/template_top_cb_skel.c
+++ /dev/null
@@ -1,11 +0,0 @@
-/*
- * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT!
- *
- * Skeleton for %(name)s top callbacks implementation.
- *
-%(*comments)s */
-#include "common.h"
-#include "fsm.h"
-#include "%(prefix)s_cb.h"
-
-%(states_template,template_cb_impl.c,origin=top)s
diff --git a/digital/io/src/hola.c b/digital/io/src/hola.c
index 1fd15f18..c7d07e21 100644
--- a/digital/io/src/hola.c
+++ b/digital/io/src/hola.c
@@ -23,3 +23,118 @@
*
* }}} */
+#define FSM_NAME AI
+
+#include "common.h"
+#include "fsm.h"
+#include "asserv.h"
+#include "mimot.h"
+#include "bot.h"
+#include "playground.h"
+
+#include "modules/math/fixed/fixed.h"
+
+uint32_t hola_angle;
+int16_t hola_timeout;
+
+FSM_STATES (
+ /* waiting start */
+ HOLA_IDLE,
+ /* wait hola signal */
+ HOLA_WAIT_HOLA,
+ /* wait until the jack is inserted for the first time */
+ HOLA_WAIT_JACK_IN,
+ /* wait until the jack is removed to initialise robot */
+ HOLA_WAIT_JACK_OUT,
+ /* moving clamps in rounds */
+ HOLA_ROUNDS,
+ /* moving clamp up */
+ HOLA_UP,
+ HOLA_CENTER,
+ HOLA_CLAP_CLOSE,
+ HOLA_CLAP_OPEN)
+
+FSM_EVENTS (hola_start)
+
+FSM_START_WITH (HOLA_IDLE)
+
+FSM_TRANS (HOLA_IDLE, start, HOLA_WAIT_HOLA)
+{
+ return FSM_NEXT (HOLA_IDLE, start);
+}
+
+FSM_TRANS (HOLA_WAIT_HOLA, hola_start, HOLA_WAIT_JACK_IN)
+{
+ return FSM_NEXT (HOLA_WAIT_HOLA, hola_start);
+}
+
+FSM_TRANS (HOLA_WAIT_JACK_IN, jack_inserted_into_bot, HOLA_WAIT_JACK_OUT)
+{
+ return FSM_NEXT (HOLA_WAIT_JACK_IN, jack_inserted_into_bot);
+}
+
+FSM_TRANS (HOLA_WAIT_JACK_OUT, jack_removed_from_bot, HOLA_ROUNDS)
+{
+ return FSM_NEXT (HOLA_WAIT_JACK_OUT, jack_removed_from_bot);
+}
+
+FSM_TRANS (HOLA_ROUNDS, asserv_last_cmd_ack,
+ /* move clamp rounds */
+ no_timeout, HOLA_ROUNDS,
+ /* move up, rotate */
+ timeout, HOLA_UP)
+{
+ if (++hola_timeout < 150)
+ {
+ int32_t cos = fixed_cos_f824 (hola_angle) + 0x1000000;
+ int32_t sin = fixed_sin_f824 (hola_angle) + 0x1000000;
+ int32_t dx = BOT_CLAMP_OPEN_STEP
+ + fixed_mul_f824 (BOT_CLAMP_STROKE_STEP / 3 / 2, cos);
+ int32_t dy = BOT_ELEVATOR_ZERO_SPEED
+ + fixed_mul_f824 (BOT_ELEVATOR_REST_STEP / 3 / 2, sin);
+ mimot_move_motor0_absolute (dx, BOT_CLAMP_SPEED);
+ mimot_move_motor1_absolute (dx, BOT_CLAMP_SPEED);
+ asserv_move_motor0_absolute (dy, BOT_ELEVATOR_SPEED);
+ hola_angle += 0x1000000 / (225 / 12);
+ return FSM_NEXT (HOLA_ROUNDS, asserv_last_cmd_ack, no_timeout);
+ }
+ else
+ {
+ asserv_set_speed (0x10, 0x1c, 0x10, 0x1c);
+ asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED / 3);
+ asserv_move_angularly (POSITION_A_DEG (174));
+ return FSM_NEXT (HOLA_ROUNDS, asserv_last_cmd_ack, timeout);
+ }
+}
+
+FSM_TRANS (HOLA_UP, bot_move_succeed, HOLA_CENTER)
+{
+ asserv_set_speed (0x10, 0x20, 0x10, 0x20);
+ asserv_move_angularly (POSITION_A_DEG (-90));
+ asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP / 2, BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (HOLA_UP, bot_move_succeed);
+}
+
+FSM_TRANS (HOLA_CENTER, bot_move_succeed, HOLA_CLAP_CLOSE)
+{
+ mimot_motor0_clamp (BOT_CLAMP_ZERO_SPEED, 0);
+ mimot_motor1_clamp (BOT_CLAMP_ZERO_SPEED, 0);
+ return FSM_NEXT (HOLA_CENTER, bot_move_succeed);
+}
+
+FSM_TRANS (HOLA_CLAP_CLOSE, clamp_succeed, HOLA_CLAP_OPEN)
+{
+ mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ return FSM_NEXT (HOLA_CLAP_CLOSE, clamp_succeed);
+}
+
+/*
+ * close clamp
+ */
+FSM_TRANS (HOLA_CLAP_OPEN, clamp_succeed, HOLA_CLAP_CLOSE)
+{
+ mimot_motor0_clamp (BOT_CLAMP_SPEED, 0);
+ mimot_motor1_clamp (BOT_CLAMP_SPEED, 0);
+ return FSM_NEXT (HOLA_CLAP_OPEN, clamp_succeed);
+}
diff --git a/digital/io/src/hola.fsm b/digital/io/src/hola.fsm
deleted file mode 100644
index ca219491..00000000
--- a/digital/io/src/hola.fsm
+++ /dev/null
@@ -1,59 +0,0 @@
-# Hola FSM
-hola
- entertain people
-
-States:
- *HOLA_IDLE
- waiting start
- HOLA_WAIT_HOLA
- wait hola signal
- HOLA_WAIT_JACK_IN
- wait until the jack is inserted for the first time
- HOLA_WAIT_JACK_OUT
- wait until the jack is removed to initialise robot
- HOLA_ROUNDS
- moving clamps in rounds
- HOLA_UP
- moving clamp up
- HOLA_CENTER
- HOLA_CLAP_CLOSE
- HOLA_CLAP_OPEN
-
-Events:
- hola_start
- start hola
-
-HOLA_IDLE:
- start -> HOLA_WAIT_HOLA
-
-HOLA_WAIT_HOLA:
- hola_start -> HOLA_WAIT_JACK_IN
-
-HOLA_WAIT_JACK_IN:
- jack_inserted_into_bot -> HOLA_WAIT_JACK_OUT
-
-HOLA_WAIT_JACK_OUT:
- jack_removed_from_bot -> HOLA_ROUNDS
-
-HOLA_ROUNDS:
- asserv_last_cmd_ack: no_timeout -> .
- move clamp rounds
- asserv_last_cmd_ack: timeout -> HOLA_UP
- move up
- rotate
-
-HOLA_UP:
- bot_move_succeed -> HOLA_CENTER
- rotate to center
-
-HOLA_CENTER:
- bot_move_succeed -> HOLA_CLAP_CLOSE
- close clamp
-
-HOLA_CLAP_CLOSE:
- clamp_succeed -> HOLA_CLAP_OPEN
- open clamp
-
-HOLA_CLAP_OPEN:
- clamp_succeed -> HOLA_CLAP_CLOSE
- close clamp
diff --git a/digital/io/src/init.c b/digital/io/src/init.c
index 1d0d8d82..58c94333 100644
--- a/digital/io/src/init.c
+++ b/digital/io/src/init.c
@@ -22,8 +22,212 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
-#include "common.h"
+#define FSM_NAME AI
+#include "common.h"
+#include "fsm.h"
+#include "asserv.h"
#include "init.h"
+#include "playground_2010.h"
+#include "main.h"
+#include "bot.h"
+#include "switch.h"
+#include "chrono.h"
+#include "playground.h"
+#include "modules/trace/trace.h"
+#include "modules/path/path.h"
uint8_t init_match_is_started = 0;
+
+FSM_STATES (
+ /* waiting for the beginning of the top FSM. */
+ INIT_IDLE,
+ /* wait for the jack to be inserted into the bot for the first time. */
+ INIT_WAIT_FIRST_JACK_IN,
+ /* wait the jack to be removed from the bot for the first time. */
+ INIT_WAIT_FIRST_JACK_OUT,
+ /* wait for the jack to be inserted into the bot for the second time. */
+ INIT_WAIT_SECOND_JACK_IN,
+ /* just wait for operator hands to be removed from the jack. */
+ INIT_WAIT_FOR_HANDS_OUT,
+ /* go to the wall for the first time. */
+ INIT_GO_TO_THE_WALL,
+ /* reset the Y position of the bot.
+ * reset the angular position of the bot.
+ * go backward from the wall for a specific distance. */
+ INIT_GO_AWAY_FROM_THE_WALL,
+ /* make the bot face the other wall. */
+ INIT_FACE_OTHER_WALL,
+ /* make sure rotation is really finished before continuing. */
+ INIT_WAIT_AFTER_ROTATION,
+ /* go to the wall for the second time. */
+ INIT_GO_TO_THE_WALL_AGAIN,
+ /* reset the X position of the bot.
+ * go backward from the wall for a specific distance. */
+ INIT_GO_AWAY_FROM_THE_WALL_AGAIN,
+ /* go to the start position. */
+ INIT_GO_TO_START_POSITION,
+ /* wait the jack to be removed from the bot for the second time. */
+ INIT_WAIT_SECOND_JACK_OUT)
+
+FSM_EVENTS (
+ /* the jack is inserted into the bot. */
+ jack_inserted_into_bot,
+ /* the jack is removed from the bot. */
+ jack_removed_from_bot)
+
+FSM_START_WITH (INIT_IDLE)
+
+/*
+ * do nothing.
+ */
+FSM_TRANS (INIT_IDLE, start, INIT_WAIT_FIRST_JACK_IN)
+{
+ return FSM_NEXT (INIT_IDLE, start);
+}
+
+/*
+ * do nothing.
+ */
+FSM_TRANS (INIT_WAIT_FIRST_JACK_IN,
+ jack_inserted_into_bot,
+ INIT_WAIT_FIRST_JACK_OUT)
+{
+ return FSM_NEXT (INIT_WAIT_FIRST_JACK_IN, jack_inserted_into_bot);
+}
+
+/*
+ * start trace module.
+ * get and store the color of the bot.
+ */
+FSM_TRANS (INIT_WAIT_FIRST_JACK_OUT,
+ jack_removed_from_bot,
+ INIT_WAIT_SECOND_JACK_IN)
+{
+ /* Initialize trace module (erase the flash). */
+ trace_init ();
+ /* Get the color. */
+ team_color = switch_get_color ();
+ return FSM_NEXT (INIT_WAIT_FIRST_JACK_OUT, jack_removed_from_bot);
+}
+
+/*
+ * do nothing.
+ */
+FSM_TRANS (INIT_WAIT_SECOND_JACK_IN,
+ jack_inserted_into_bot,
+ INIT_WAIT_FOR_HANDS_OUT)
+{
+ return FSM_NEXT (INIT_WAIT_SECOND_JACK_IN, jack_inserted_into_bot);
+}
+
+/*
+ * fuck the wall in front.
+ */
+FSM_TRANS_TIMEOUT (INIT_WAIT_FOR_HANDS_OUT, 225,
+ INIT_GO_TO_THE_WALL)
+{
+ /* Go to the wall, no backward. */
+ asserv_go_to_the_wall (0);
+ return FSM_NEXT_TIMEOUT (INIT_WAIT_FOR_HANDS_OUT);
+}
+
+/*
+ * reset the Y position of the bot.
+ * reset the angular position of the bot.
+ * move away from the wall (linear move).
+ */
+FSM_TRANS (INIT_GO_TO_THE_WALL,
+ bot_move_succeed,
+ INIT_GO_AWAY_FROM_THE_WALL)
+{
+ asserv_set_y_position (PG_Y (PG_LENGTH - BOT_SIZE_FRONT));
+ asserv_set_angle_position (PG_A_DEG (90));
+ /* Move away from the border. */
+ asserv_move_linearly (- INIT_DIST);
+ return FSM_NEXT (INIT_GO_TO_THE_WALL, bot_move_succeed);
+}
+
+/*
+ * turn to face the other wall.
+ */
+FSM_TRANS (INIT_GO_AWAY_FROM_THE_WALL,
+ bot_move_succeed,
+ INIT_FACE_OTHER_WALL)
+{
+ /* Face the other wall. */
+ asserv_goto_angle (PG_A_DEG (180));
+ return FSM_NEXT (INIT_GO_AWAY_FROM_THE_WALL, bot_move_succeed);
+}
+
+/*
+ * nothing to do.
+ */
+FSM_TRANS (INIT_FACE_OTHER_WALL,
+ bot_move_succeed,
+ INIT_WAIT_AFTER_ROTATION)
+{
+ return FSM_NEXT (INIT_FACE_OTHER_WALL, bot_move_succeed);
+}
+
+/*
+ * fuck the wall in front.
+ */
+FSM_TRANS_TIMEOUT (INIT_WAIT_AFTER_ROTATION, 100,
+ INIT_GO_TO_THE_WALL_AGAIN)
+{
+ /* Go to the wall, no backward. */
+ asserv_go_to_the_wall (0);
+ return FSM_NEXT_TIMEOUT (INIT_WAIT_AFTER_ROTATION);
+}
+
+/*
+ * reset the X position of the bot.
+ * move away from the wall (linear move).
+ */
+FSM_TRANS (INIT_GO_TO_THE_WALL_AGAIN,
+ bot_move_succeed,
+ INIT_GO_AWAY_FROM_THE_WALL_AGAIN)
+{
+ asserv_set_x_position (PG_X (BOT_SIZE_FRONT));
+ /* Move away from the border. */
+ asserv_move_linearly (- INIT_DIST);
+ return FSM_NEXT (INIT_GO_TO_THE_WALL_AGAIN, bot_move_succeed);
+}
+
+/*
+ * go to the start position with a go to movement.
+ */
+FSM_TRANS (INIT_GO_AWAY_FROM_THE_WALL_AGAIN,
+ bot_move_succeed,
+ INIT_GO_TO_START_POSITION)
+{
+ /* Move away from the border. */
+ asserv_goto_xya (BOT_INIT_XYA, 0);
+ return FSM_NEXT (INIT_GO_AWAY_FROM_THE_WALL_AGAIN, bot_move_succeed);
+}
+
+/*
+ * nothing to do, the bot is at the start position.
+ */
+FSM_TRANS (INIT_GO_TO_START_POSITION,
+ bot_move_succeed,
+ INIT_WAIT_SECOND_JACK_OUT)
+{
+ return FSM_NEXT (INIT_GO_TO_START_POSITION, bot_move_succeed);
+}
+
+/*
+ * tell other FSM the match begins.
+ * start the chrono.
+ */
+FSM_TRANS (INIT_WAIT_SECOND_JACK_OUT,
+ jack_removed_from_bot,
+ INIT_IDLE)
+{
+ /* Set the flag to transmit to other FSM. */
+ init_match_is_started = 1;
+ /* Start the chrono. */
+ chrono_init ();
+ return FSM_NEXT (INIT_WAIT_SECOND_JACK_OUT, jack_removed_from_bot);
+}
diff --git a/digital/io/src/init.fsm b/digital/io/src/init.fsm
deleted file mode 100644
index f780ba45..00000000
--- a/digital/io/src/init.fsm
+++ /dev/null
@@ -1,106 +0,0 @@
-# init FSM
-# This FSM is used to place the bot at a "start position" for a match. It is
-# more precise than a human hand and reset the asserv board to the start
-# position.
-# Some remarks:
-# - for security reason, we need to insert the jack then removed it, then
-# insert it back. Why? Because it can already be inserted.
-# - other FSM can do a similar thing by using the first jack out to
-# initialize.
-init
- place the bot in the start position for a match.
-
-States:
- *INIT_IDLE
- waiting for the beginning of the top FSM.
- INIT_WAIT_FIRST_JACK_IN
- wait for the jack to be inserted into the bot for the first time.
- INIT_WAIT_FIRST_JACK_OUT
- wait the jack to be removed from the bot for the first time.
- INIT_WAIT_SECOND_JACK_IN
- wait for the jack to be inserted into the bot for the second time.
- INIT_WAIT_FOR_HANDS_OUT[timeout=225]
- just wait for operator hands to be removed from the jack.
- INIT_GO_TO_THE_WALL
- go to the wall for the first time.
- INIT_GO_AWAY_FROM_THE_WALL
- reset the Y position of the bot.
- reset the angular position of the bot.
- go backward from the wall for a specific distance.
- INIT_FACE_OTHER_WALL
- make the bot face the other wall.
- INIT_WAIT_AFTER_ROTATION [timeout=100]
- make sure rotation is really finished before continuing.
- INIT_GO_TO_THE_WALL_AGAIN
- go to the wall for the second time.
- INIT_GO_AWAY_FROM_THE_WALL_AGAIN
- reset the X position of the bot.
- go backward from the wall for a specific distance.
- INIT_GO_TO_START_POSITION
- go to the start position.
- INIT_WAIT_SECOND_JACK_OUT
- wait the jack to be removed from the bot for the second time.
-
-Events:
- jack_inserted_into_bot
- the jack is inserted into the bot.
- jack_removed_from_bot
- the jack is removed from the bot.
-
-INIT_IDLE:
- start -> INIT_WAIT_FIRST_JACK_IN
- do nothing.
-
-INIT_WAIT_FIRST_JACK_IN:
- jack_inserted_into_bot -> INIT_WAIT_FIRST_JACK_OUT
- do nothing.
-
-INIT_WAIT_FIRST_JACK_OUT:
- jack_removed_from_bot -> INIT_WAIT_SECOND_JACK_IN
- start trace module.
- get and store the color of the bot.
-
-INIT_WAIT_SECOND_JACK_IN:
- jack_inserted_into_bot -> INIT_WAIT_FOR_HANDS_OUT
- do nothing.
-
-INIT_WAIT_FOR_HANDS_OUT:
- state_timeout -> INIT_GO_TO_THE_WALL
- fuck the wall in front.
-
-INIT_GO_TO_THE_WALL:
- bot_move_succeed -> INIT_GO_AWAY_FROM_THE_WALL
- reset the Y position of the bot.
- reset the angular position of the bot.
- move away from the wall (linear move).
-
-INIT_GO_AWAY_FROM_THE_WALL:
- bot_move_succeed -> INIT_FACE_OTHER_WALL
- turn to face the other wall.
-
-INIT_FACE_OTHER_WALL:
- bot_move_succeed -> INIT_WAIT_AFTER_ROTATION
- nothing to do.
-
-INIT_WAIT_AFTER_ROTATION:
- state_timeout -> INIT_GO_TO_THE_WALL_AGAIN
- fuck the wall in front.
-
-INIT_GO_TO_THE_WALL_AGAIN:
- bot_move_succeed -> INIT_GO_AWAY_FROM_THE_WALL_AGAIN
- reset the X position of the bot.
- move away from the wall (linear move).
-
-INIT_GO_AWAY_FROM_THE_WALL_AGAIN:
- bot_move_succeed -> INIT_GO_TO_START_POSITION
- go to the start position with a go to movement.
-
-INIT_GO_TO_START_POSITION:
- bot_move_succeed -> INIT_WAIT_SECOND_JACK_OUT
- nothing to do, the bot is at the start position.
-
-INIT_WAIT_SECOND_JACK_OUT:
- jack_removed_from_bot -> INIT_IDLE
- tell other FSM the match begins.
- start the chrono.
-
diff --git a/digital/io/src/loader.c b/digital/io/src/loader.c
index 4157023e..e8f8c139 100644
--- a/digital/io/src/loader.c
+++ b/digital/io/src/loader.c
@@ -22,10 +22,16 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+#define FSM_NAME AI
+
#include "common.h"
#include "loader.h"
-
#include "fsm.h"
+#include "asserv.h"
+#include "mimot.h"
+#include "bot.h"
+#include "fsm_queue.h"
+#include "food.h"
uint8_t loader_elements;
uint8_t loader_want_up;
@@ -34,13 +40,614 @@ void
loader_up (void)
{
loader_want_up = 1;
- fsm_handle_event (&ai_fsm, AI_EVENT_loader_up);
+ FSM_HANDLE (AI, loader_up);
}
void
loader_down (void)
{
loader_want_up = 0;
- fsm_handle_event (&ai_fsm, AI_EVENT_loader_down);
+ FSM_HANDLE (AI, loader_down);
+}
+
+FSM_STATES (
+ /* wait start */
+ LOADER_IDLE,
+ /* wait until the jack is inserted for the first time */
+ LOADER_WAIT_JACK_IN,
+ /* wait until the jack is removed to initialise robot */
+ LOADER_WAIT_JACK_OUT,
+ /* move the elevator down to find zero position */
+ LOADER_INIT_ELEVATOR_ZERO,
+ /* test clamp closing */
+ LOADER_INIT_CLAMP_CLOSE,
+ /* find clamp zero position */
+ LOADER_INIT_CLAMP_ZERO,
+ /* find gate zero */
+ LOADER_INIT_GATE_ZERO,
+ /* move the elevator up so that the robot can go to the wall */
+ LOADER_INIT_ELEVATOR_UP,
+ /* wait with gate open */
+ LOADER_INIT_GATE_WAIT,
+ /* close gate before start */
+ LOADER_INIT_GATE_CLOSE,
+ /* up position, clamp is protected */
+ LOADER_UP,
+ /* down position, ready to take an element */
+ LOADER_DOWN,
+ /* moving to the up position */
+ LOADER_UPING,
+ /* moving to the down position */
+ LOADER_DOWNING,
+ /* error while moving up, stop in an unknown state */
+ LOADER_ERROR,
+ /* error recovery moving down */
+ LOADER_ERROR_DOWNING,
+ /* error recovery moving down, open clamp */
+ LOADER_ERROR_DOWNING_OPEN,
+ /* error recovery moving up */
+ LOADER_ERROR_UPING,
+ /* error recovery moving up, open clamp */
+ LOADER_ERROR_UPING_OPEN,
+ /* close clamp */
+ LOADER_LOAD_CLOSING,
+ /* move load up to the unload position */
+ LOADER_LOAD_UPING,
+ /* open clamp to unload, wait until unload position */
+ LOADER_LOAD_UNLOADING,
+ /* check clamp is open */
+ LOADER_LOAD_UNLOADING_OPEN,
+ /* open clamp due to no elements to pick */
+ LOADER_LOAD_EMPTY_OPEN)
+
+FSM_EVENTS (
+ /* asserv success result */
+ elevator_succeed,
+ /* asserv failure result */
+ elevator_failed,
+ /* elevator above unloading position */
+ elevator_unload_position,
+ /* asserv failure result */
+ clamp_failed,
+ /* asserv success result */
+ gate_succeed,
+ /* asserv failure result */
+ gate_failed,
+ /* element sensed between clamp to be loaded */
+ loader_element,
+ /* make the loader ready to load an element */
+ loader_down,
+ /* protect the clamp by moving it up */
+ loader_up,
+ /* posted when loader successfully moved down */
+ loader_downed,
+ /* posted when loader successfully moved up */
+ loader_uped,
+ /* posted when loader in an unknown state due to error while moving up */
+ loader_errored,
+ /* loader blocked, due to a black corn */
+ loader_black)
+
+FSM_START_WITH (LOADER_IDLE)
+
+FSM_TRANS (LOADER_IDLE, start, LOADER_WAIT_JACK_IN)
+{
+ return FSM_NEXT (LOADER_IDLE, start);
+}
+
+FSM_TRANS (LOADER_WAIT_JACK_IN,
+ jack_inserted_into_bot,
+ LOADER_WAIT_JACK_OUT)
+{
+ return FSM_NEXT (LOADER_WAIT_JACK_IN, jack_inserted_into_bot);
+}
+
+FSM_TRANS (LOADER_WAIT_JACK_IN,
+ hola_start,
+ LOADER_IDLE)
+{
+ return FSM_NEXT (LOADER_WAIT_JACK_IN, hola_start);
+}
+
+/*
+ * find elevator zero
+ * close clamp
+ * find gate zero
+ */
+FSM_TRANS (LOADER_WAIT_JACK_OUT,
+ jack_removed_from_bot,
+ LOADER_INIT_ELEVATOR_ZERO)
+{
+ asserv_motor0_zero_position (-BOT_ELEVATOR_ZERO_SPEED);
+ mimot_motor0_clamp (BOT_CLAMP_ZERO_SPEED, 0);
+ mimot_motor1_clamp (BOT_CLAMP_ZERO_SPEED, 0);
+ asserv_motor1_zero_position (-BOT_GATE_SPEED);
+ return FSM_NEXT (LOADER_WAIT_JACK_OUT, jack_removed_from_bot);
+}
+
+FSM_TRANS (LOADER_INIT_ELEVATOR_ZERO,
+ elevator_succeed,
+ LOADER_INIT_CLAMP_CLOSE)
+{
+ return FSM_NEXT (LOADER_INIT_ELEVATOR_ZERO, elevator_succeed);
+}
+
+/*
+ * move elevator up
+ * find clamp zero
+ */
+FSM_TRANS (LOADER_INIT_CLAMP_CLOSE,
+ clamp_succeed,
+ LOADER_INIT_CLAMP_ZERO)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP,
+ BOT_ELEVATOR_ZERO_SPEED);
+ mimot_motor0_zero_position (-BOT_CLAMP_ZERO_SPEED);
+ mimot_motor1_zero_position (-BOT_CLAMP_ZERO_SPEED);
+ return FSM_NEXT (LOADER_INIT_CLAMP_CLOSE, clamp_succeed);
+}
+
+FSM_TRANS (LOADER_INIT_CLAMP_ZERO,
+ clamp_succeed,
+ LOADER_INIT_GATE_ZERO)
+{
+ return FSM_NEXT (LOADER_INIT_CLAMP_ZERO, clamp_succeed);
+}
+
+FSM_TRANS (LOADER_INIT_GATE_ZERO,
+ gate_succeed,
+ LOADER_INIT_ELEVATOR_UP)
+{
+ return FSM_NEXT (LOADER_INIT_GATE_ZERO, gate_succeed);
+}
+
+FSM_TRANS (LOADER_INIT_ELEVATOR_UP,
+ elevator_succeed,
+ LOADER_INIT_GATE_WAIT)
+{
+ return FSM_NEXT (LOADER_INIT_ELEVATOR_UP, elevator_succeed);
+}
+
+/*
+ * initialisation failure
+ */
+FSM_TRANS (LOADER_INIT_ELEVATOR_UP,
+ elevator_failed,
+ LOADER_IDLE)
+{
+ /* Move so that the operator notice it. */
+ asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP / 3,
+ BOT_ELEVATOR_ZERO_SPEED);
+ return FSM_NEXT (LOADER_INIT_ELEVATOR_UP, elevator_failed);
}
+/*
+ * close gate
+ */
+FSM_TRANS_TIMEOUT (LOADER_INIT_GATE_WAIT, 225,
+ LOADER_INIT_GATE_CLOSE)
+{
+ asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED);
+ return FSM_NEXT_TIMEOUT (LOADER_INIT_GATE_WAIT);
+}
+
+FSM_TRANS (LOADER_INIT_GATE_CLOSE,
+ gate_succeed,
+ LOADER_UP)
+{
+ return FSM_NEXT (LOADER_INIT_GATE_CLOSE, gate_succeed);
+}
+
+/*
+ * move down
+ */
+FSM_TRANS (LOADER_UP,
+ loader_down,
+ LOADER_DOWNING)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (LOADER_UP, loader_down);
+}
+
+/*
+ * move up
+ */
+FSM_TRANS (LOADER_DOWN,
+ loader_up,
+ LOADER_UPING)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (LOADER_DOWN, loader_up);
+}
+
+/*
+ * clamp
+ */
+FSM_TRANS (LOADER_DOWN,
+ loader_element,
+ LOADER_LOAD_CLOSING)
+{
+ mimot_motor0_clamp (BOT_CLAMP_SPEED, BOT_CLAMP_PWM);
+ mimot_motor1_clamp (BOT_CLAMP_SPEED, BOT_CLAMP_PWM);
+ return FSM_NEXT (LOADER_DOWN, loader_element);
+}
+
+/*
+ * post loader_uped event
+ */
+FSM_TRANS (LOADER_UPING,
+ elevator_succeed,
+ LOADER_UP)
+{
+ fsm_queue_post_event (FSM_EVENT (AI,loader_uped));
+ return FSM_NEXT (LOADER_UPING, elevator_succeed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS (LOADER_UPING,
+ elevator_failed,
+ LOADER_ERROR)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT (LOADER_UPING, elevator_failed);
+}
+
+/*
+ * move down
+ */
+FSM_TRANS (LOADER_UPING,
+ loader_down,
+ LOADER_DOWNING)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (LOADER_UPING, loader_down);
+}
+
+/*
+ * release elevator motor
+ * post loader_downed event
+ */
+FSM_TRANS (LOADER_DOWNING,
+ elevator_succeed,
+ LOADER_DOWN)
+{
+ asserv_motor0_free ();
+ fsm_queue_post_event (FSM_EVENT (AI, loader_downed));
+ return FSM_NEXT (LOADER_DOWNING, elevator_succeed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS (LOADER_DOWNING,
+ elevator_failed,
+ LOADER_ERROR)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT (LOADER_DOWNING, elevator_failed);
+}
+
+/*
+ * emergency stop
+ * post loader_errored event
+ */
+FSM_TRANS (LOADER_DOWNING,
+ loader_element,
+ LOADER_ERROR)
+{
+ asserv_motor0_free ();
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT (LOADER_DOWNING, loader_element);
+}
+
+/*
+ * move up
+ */
+FSM_TRANS (LOADER_DOWNING,
+ loader_up,
+ LOADER_UPING)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (LOADER_DOWNING, loader_up);
+}
+
+/*
+ * move down
+ */
+FSM_TRANS (LOADER_ERROR,
+ loader_down,
+ LOADER_ERROR_DOWNING)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (LOADER_ERROR, loader_down);
+}
+
+/*
+ * move up
+ */
+FSM_TRANS (LOADER_ERROR,
+ loader_up,
+ LOADER_ERROR_UPING)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP, BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (LOADER_ERROR, loader_up);
+}
+
+/*
+ * release elevator motor
+ * open clamp
+ */
+FSM_TRANS (LOADER_ERROR_DOWNING,
+ elevator_succeed,
+ LOADER_ERROR_DOWNING_OPEN)
+{
+ asserv_motor0_free ();
+ mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ return FSM_NEXT (LOADER_ERROR_DOWNING, elevator_succeed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS (LOADER_ERROR_DOWNING,
+ elevator_failed,
+ LOADER_ERROR)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT (LOADER_ERROR_DOWNING, elevator_failed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS_TIMEOUT (LOADER_ERROR_DOWNING, 225,
+ LOADER_ERROR)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT_TIMEOUT (LOADER_ERROR_DOWNING);
+}
+
+/*
+ * post loader_downed event
+ */
+FSM_TRANS (LOADER_ERROR_DOWNING_OPEN,
+ clamp_succeed,
+ LOADER_DOWN)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_downed));
+ return FSM_NEXT (LOADER_ERROR_DOWNING_OPEN, clamp_succeed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS (LOADER_ERROR_DOWNING_OPEN,
+ clamp_failed,
+ LOADER_ERROR)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT (LOADER_ERROR_DOWNING_OPEN, clamp_failed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS_TIMEOUT (LOADER_ERROR_DOWNING_OPEN, 225,
+ LOADER_ERROR)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT_TIMEOUT (LOADER_ERROR_DOWNING_OPEN);
+}
+
+/*
+ * open clamp
+ */
+FSM_TRANS (LOADER_ERROR_UPING,
+ elevator_succeed,
+ LOADER_ERROR_UPING_OPEN)
+{
+ mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ return FSM_NEXT (LOADER_ERROR_UPING, elevator_succeed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS (LOADER_ERROR_UPING,
+ elevator_failed,
+ LOADER_ERROR)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT (LOADER_ERROR_UPING, elevator_failed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS_TIMEOUT (LOADER_ERROR_UPING, 225,
+ LOADER_ERROR)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT_TIMEOUT (LOADER_ERROR_UPING);
+}
+
+/*
+ * post loader_uped event
+ */
+FSM_TRANS (LOADER_ERROR_UPING_OPEN,
+ clamp_succeed,
+ LOADER_UP)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_uped));
+ return FSM_NEXT (LOADER_ERROR_UPING_OPEN, clamp_succeed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS (LOADER_ERROR_UPING_OPEN,
+ clamp_failed,
+ LOADER_ERROR)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT (LOADER_ERROR_UPING_OPEN, clamp_failed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS_TIMEOUT (LOADER_ERROR_UPING_OPEN, 225,
+ LOADER_ERROR)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT_TIMEOUT (LOADER_ERROR_UPING_OPEN);
+}
+
+FSM_TRANS (LOADER_LOAD_CLOSING, clamp_succeed,
+ /* move up
+ * count one element */
+ full, LOADER_LOAD_UPING,
+ /* open clamp */
+ empty, LOADER_LOAD_EMPTY_OPEN)
+{
+ /* Measure load using clamp position. */
+ uint16_t tickness = BOT_CLAMP_WIDTH_STEP
+ - mimot_get_motor0_position ()
+ - mimot_get_motor1_position ();
+ if (tickness > BOT_CLAMP_EMPTY_STEP)
+ {
+ asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP,
+ BOT_ELEVATOR_SPEED);
+ position_t robot_position;
+ asserv_get_position (&robot_position);
+ food_taken (robot_position);
+ loader_elements++;
+ return FSM_NEXT (LOADER_LOAD_CLOSING, clamp_succeed, full);
+ }
+ else
+ {
+ mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ return FSM_NEXT (LOADER_LOAD_CLOSING, clamp_succeed, empty);
+ }
+}
+
+/*
+ * open clamp
+ */
+FSM_TRANS (LOADER_LOAD_UPING,
+ elevator_unload_position,
+ LOADER_LOAD_UNLOADING)
+{
+ mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ return FSM_NEXT (LOADER_LOAD_UPING, elevator_unload_position);
+}
+
+/*
+ * post loader_errored or loader_black event
+ * remove one element
+ * open clamp
+ */
+FSM_TRANS (LOADER_LOAD_UPING,
+ elevator_failed,
+ LOADER_ERROR)
+{
+ if (asserv_get_motor0_position () < BOT_ELEVATOR_BLACK_THRESHOLD_STEP)
+ {
+ if (loader_elements)
+ loader_elements--;
+ fsm_queue_post_event (FSM_EVENT (AI, loader_black));
+ }
+ else
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ return FSM_NEXT (LOADER_LOAD_UPING, elevator_failed);
+}
+
+FSM_TRANS (LOADER_LOAD_UNLOADING,
+ elevator_succeed,
+ LOADER_LOAD_UNLOADING_OPEN)
+{
+ return FSM_NEXT (LOADER_LOAD_UNLOADING, elevator_succeed);
+}
+
+/*
+ * post loader_errored event
+ * open clamp
+ */
+FSM_TRANS (LOADER_LOAD_UNLOADING,
+ elevator_failed,
+ LOADER_ERROR)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ return FSM_NEXT (LOADER_LOAD_UNLOADING, elevator_failed);
+}
+
+FSM_TRANS (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed,
+ /* move down */
+ down, LOADER_DOWNING,
+ /* move up */
+ up, LOADER_UPING)
+{
+ if (loader_want_up)
+ {
+ asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP,
+ BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed, up);
+ }
+ else
+ {
+ asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP,
+ BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed, down);
+ }
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS (LOADER_LOAD_UNLOADING_OPEN,
+ clamp_failed,
+ LOADER_ERROR)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT (LOADER_LOAD_UNLOADING_OPEN, clamp_failed);
+}
+
+FSM_TRANS (LOADER_LOAD_EMPTY_OPEN, clamp_succeed,
+ /* post loader_downed event */
+ down, LOADER_DOWN,
+ /* move up */
+ up, LOADER_UPING)
+{
+ if (loader_want_up)
+ {
+ asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP,
+ BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, up);
+ }
+ else
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, loader_downed));
+ return FSM_NEXT (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, down);
+ }
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS (LOADER_LOAD_EMPTY_OPEN,
+ clamp_failed,
+ LOADER_ERROR)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT (LOADER_LOAD_EMPTY_OPEN, clamp_failed);
+}
diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm
deleted file mode 100644
index a188194e..00000000
--- a/digital/io/src/loader.fsm
+++ /dev/null
@@ -1,227 +0,0 @@
-# loader FSM
-loader
- Handle Eurobot 2010 robot clamp, elevator and output door.
-
-States:
- *LOADER_IDLE
- wait start
- LOADER_WAIT_JACK_IN
- wait until the jack is inserted for the first time
- LOADER_WAIT_JACK_OUT
- wait until the jack is removed to initialise robot
- LOADER_INIT_ELEVATOR_ZERO
- move the elevator down to find zero position
- LOADER_INIT_CLAMP_CLOSE
- test clamp closing
- LOADER_INIT_CLAMP_ZERO
- find clamp zero position
- LOADER_INIT_GATE_ZERO
- find gate zero
- LOADER_INIT_ELEVATOR_UP
- move the elevator up so that the robot can go to the wall
- LOADER_INIT_GATE_WAIT[timeout=225]
- wait with gate open
- LOADER_INIT_GATE_CLOSE
- close gate before start
- LOADER_UP
- up position, clamp is protected
- LOADER_DOWN
- down position, ready to take an element
- LOADER_UPING
- moving to the up position
- LOADER_DOWNING
- moving to the down position
- LOADER_ERROR
- error while moving up, stop in an unknown state
- LOADER_ERROR_DOWNING [timeout=225]
- error recovery moving down
- LOADER_ERROR_DOWNING_OPEN [timeout=225]
- error recovery moving down, open clamp
- LOADER_ERROR_UPING [timeout=225]
- error recovery moving up
- LOADER_ERROR_UPING_OPEN [timeout=225]
- error recovery moving up, open clamp
- LOADER_LOAD_CLOSING
- close clamp
- LOADER_LOAD_UPING
- move load up to the unload position
- LOADER_LOAD_UNLOADING
- open clamp to unload, wait until unload position
- LOADER_LOAD_UNLOADING_OPEN
- check clamp is open
- LOADER_LOAD_EMPTY_OPEN
- open clamp due to no elements to pick
-
-Events:
- elevator_succeed
- asserv success result
- elevator_failed
- asserv failure result
- elevator_unload_position
- elevator above unloading position
- clamp_failed
- asserv failure result
- gate_succeed
- asserv success result
- gate_failed
- asserv failure result
- loader_element
- element sensed between clamp to be loaded
- loader_down
- make the loader ready to load an element
- loader_up
- protect the clamp by moving it up
- loader_downed
- posted when loader successfully moved down
- loader_uped
- posted when loader successfully moved up
- loader_errored
- posted when loader in an unknown state due to error while moving up
- loader_black
- loader blocked, due to a black corn
-
-LOADER_IDLE:
- start -> LOADER_WAIT_JACK_IN
-
-LOADER_WAIT_JACK_IN:
- jack_inserted_into_bot -> LOADER_WAIT_JACK_OUT
- hola_start -> LOADER_IDLE
-
-LOADER_WAIT_JACK_OUT:
- jack_removed_from_bot -> LOADER_INIT_ELEVATOR_ZERO
- find elevator zero
- close clamp
- find gate zero
-
-LOADER_INIT_ELEVATOR_ZERO:
- elevator_succeed -> LOADER_INIT_CLAMP_CLOSE
-
-LOADER_INIT_CLAMP_CLOSE:
- clamp_succeed -> LOADER_INIT_CLAMP_ZERO
- move elevator up
- find clamp zero
-
-LOADER_INIT_CLAMP_ZERO:
- clamp_succeed -> LOADER_INIT_GATE_ZERO
-
-LOADER_INIT_GATE_ZERO:
- gate_succeed -> LOADER_INIT_ELEVATOR_UP
-
-LOADER_INIT_ELEVATOR_UP:
- elevator_succeed -> LOADER_INIT_GATE_WAIT
- elevator_failed -> LOADER_IDLE
- initialisation failure
-
-LOADER_INIT_GATE_WAIT:
- state_timeout -> LOADER_INIT_GATE_CLOSE
- close gate
-
-LOADER_INIT_GATE_CLOSE:
- gate_succeed -> LOADER_UP
-
-LOADER_UP:
- loader_down -> LOADER_DOWNING
- move down
-
-LOADER_DOWN:
- loader_up -> LOADER_UPING
- move up
- loader_element -> LOADER_LOAD_CLOSING
- clamp
-
-LOADER_ERROR:
- loader_down -> LOADER_ERROR_DOWNING
- move down
- loader_up -> LOADER_ERROR_UPING
- move up
-
-LOADER_ERROR_DOWNING:
- elevator_succeed -> LOADER_ERROR_DOWNING_OPEN
- release elevator motor
- open clamp
- elevator_failed -> LOADER_ERROR
- post loader_errored event
- state_timeout -> LOADER_ERROR
- post loader_errored event
-
-LOADER_ERROR_DOWNING_OPEN:
- clamp_succeed -> LOADER_DOWN
- post loader_downed event
- clamp_failed -> LOADER_ERROR
- post loader_errored event
- state_timeout -> LOADER_ERROR
- post loader_errored event
-
-LOADER_ERROR_UPING:
- elevator_succeed -> LOADER_ERROR_UPING_OPEN
- open clamp
- elevator_failed -> LOADER_ERROR
- post loader_errored event
- state_timeout -> LOADER_ERROR
- post loader_errored event
-
-LOADER_ERROR_UPING_OPEN:
- clamp_succeed -> LOADER_UP
- post loader_uped event
- clamp_failed -> LOADER_ERROR
- post loader_errored event
- state_timeout -> LOADER_ERROR
- post loader_errored event
-
-LOADER_DOWNING:
- elevator_succeed -> LOADER_DOWN
- release elevator motor
- post loader_downed event
- elevator_failed -> LOADER_ERROR
- post loader_errored event
- loader_element -> LOADER_ERROR
- emergency stop
- post loader_errored event
- loader_up -> LOADER_UPING
- move up
-
-LOADER_UPING:
- elevator_succeed -> LOADER_UP
- post loader_uped event
- elevator_failed -> LOADER_ERROR
- post loader_errored event
- loader_down -> LOADER_DOWNING
- move down
-
-LOADER_LOAD_CLOSING:
- clamp_succeed: full -> LOADER_LOAD_UPING
- move up
- count one element
- clamp_succeed: empty -> LOADER_LOAD_EMPTY_OPEN
- open clamp
-
-LOADER_LOAD_UPING:
- elevator_unload_position -> LOADER_LOAD_UNLOADING
- open clamp
- elevator_failed -> LOADER_ERROR
- post loader_errored or loader_black event
- remove one element
- open clamp
-
-LOADER_LOAD_UNLOADING:
- elevator_succeed -> LOADER_LOAD_UNLOADING_OPEN
- elevator_failed -> LOADER_ERROR
- post loader_errored event
- open clamp
-
-LOADER_LOAD_UNLOADING_OPEN:
- clamp_succeed: down -> LOADER_DOWNING
- move down
- clamp_succeed: up -> LOADER_UPING
- move up
- clamp_failed -> LOADER_ERROR
- post loader_errored event
-
-LOADER_LOAD_EMPTY_OPEN:
- clamp_succeed: down -> LOADER_DOWN
- post loader_downed event
- clamp_succeed: up -> LOADER_UPING
- move up
- clamp_failed -> LOADER_ERROR
- post loader_errored event
-
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index 35ee83c1..22964ddd 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -35,23 +35,27 @@
/* AVR include, non HOST */
#ifndef HOST
# include "switch.h" /* Manage switches (jack, color selector) */
+#else
+#include <string.h>
#endif /* HOST */
-#include "main_timer.h"
+#include "timer.h"
#include "simu.host.h"
#include "asserv.h"
#include "mimot.h"
#include "twi_master.h"
#include "eeprom.h" /* Parameters loaded/stored in the EEPROM */
-#include "fsm.h" /* fsm_* */
+#define FSM_NAME AI
+#include "fsm.h"
+#include "fsm_queue.h"
#include "bot.h"
#include "servo_pos.h"
#include "usdist.h"
#include "radar.h"
#include "chrono.h" /* chrono_end_match */
#include "pwm.h"
-#include "playground.h"
+#include "playground_2010.h"
#include "contact.h"
#include "init.h"
#include "move.h"
@@ -73,19 +77,7 @@ static void main_loop (void);
/**
* Our color.
*/
-enum team_color_e bot_color;
-
-/** Maximum number of events in post queue. */
-#define MAIN_POST_EVENT_QUEUE_SIZE 8
-
-/** Events to post to the FSM in next iteration. */
-uint8_t main_post_event_queue[MAIN_POST_EVENT_QUEUE_SIZE];
-
-/** Number of events in the post queue. */
-uint8_t main_post_event_queue_nb;
-
-/** First event in the post queue. */
-uint8_t main_post_event_queue_head;
+enum team_color_e team_color;
/** Obstacles positions, updated using radar module. */
vect_t main_obstacles_pos[2];
@@ -111,29 +103,6 @@ static uint8_t main_stats_asserv_, main_stats_asserv_cpt_;
*/
static uint8_t main_stats_timer_;
-void
-main_post_event (uint8_t event)
-{
- assert (main_post_event_queue_nb < MAIN_POST_EVENT_QUEUE_SIZE);
- uint8_t tail = (main_post_event_queue_head + main_post_event_queue_nb)
- % MAIN_POST_EVENT_QUEUE_SIZE;
- main_post_event_queue[tail] = event;
- main_post_event_queue_nb++;
-}
-
-/** Pop one event from the event queue. */
-static uint8_t
-main_pop_event (void)
-{
- uint8_t e;
- assert (main_post_event_queue_nb > 0);
- e = main_post_event_queue[main_post_event_queue_head];
- main_post_event_queue_nb--;
- main_post_event_queue_head = (main_post_event_queue_head + 1)
- % MAIN_POST_EVENT_QUEUE_SIZE;
- return e;
-}
-
/**
* Main events management.
* This function is responsible to get all events and send them to the
@@ -144,16 +113,18 @@ 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_EVENT(fsm, event) \
- do { if (fsm_handle_event ((fsm), (event))) return; } while (0)
-#define FSM_HANDLE_TIMEOUT(fsm) \
- do { if (fsm_handle_timeout (fsm)) return; } while (0)
+#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 (&ai_fsm);
+ FSM_HANDLE_TIMEOUT_E (AI);
/* If we have entering this function, last command of the asserv board has
* been aquited. */
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_asserv_last_cmd_ack);
+ FSM_HANDLE_E (AI, asserv_last_cmd_ack);
asserv_status_e
move_status = none,
@@ -171,65 +142,65 @@ main_event_to_fsm (void)
/* Check commands move status. */
if (move_status == success)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_bot_move_succeed);
+ FSM_HANDLE_E (AI, bot_move_succeed);
else if (move_status == failure)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_bot_move_failed);
+ FSM_HANDLE_E (AI, bot_move_failed);
if (motor0_status == success)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_elevator_succeed);
+ FSM_HANDLE_E (AI, elevator_succeed);
else if (motor0_status == failure)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_elevator_failed);
+ FSM_HANDLE_E (AI, elevator_failed);
if (motor1_status == success)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_gate_succeed);
+ FSM_HANDLE_E (AI, gate_succeed);
else if (motor1_status == failure)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_gate_failed);
+ FSM_HANDLE_E (AI, gate_failed);
if (motorm0_status == success && motorm1_status == success)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_clamp_succeed);
+ FSM_HANDLE_E (AI, clamp_succeed);
else if (motorm0_status == failure || motorm1_status == failure)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_clamp_failed);
+ FSM_HANDLE_E (AI, clamp_failed);
/* Check positions. */
if (asserv_get_motor0_position () > BOT_ELEVATOR_UNLOAD_STEP)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_elevator_unload_position);
+ FSM_HANDLE_E (AI, elevator_unload_position);
/* Contacts. */
if (asserv_get_motor0_position () < BOT_ELEVATOR_REST_STEP
&& (!IO_GET (CONTACT_BUMPER0) || !IO_GET (CONTACT_BUMPER1)))
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_loader_element);
+ FSM_HANDLE_E (AI, loader_element);
if (!IO_GET (CONTACT_STRATEGY))
{
if (switch_get_color ())
loader_down ();
else
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_hola_start);
+ FSM_HANDLE_E (AI, hola_start);
}
/* Jack */
if (switch_get_jack ())
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_jack_removed_from_bot);
+ FSM_HANDLE_E (AI, jack_removed_from_bot);
else
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_jack_inserted_into_bot);
+ FSM_HANDLE_E (AI, jack_inserted_into_bot);
if (init_match_is_started)
{
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_init_match_is_started);
+ FSM_HANDLE_E (AI, init_match_is_started);
/* This must be done in the last part of this block. */
init_match_is_started = 0;
}
/* Event generated at the end of the sub FSM to post to the top FSM */
- if (main_post_event_queue_nb)
+ if (fsm_queue_poll ())
{
/* We must post the event at the end of this block because it
* will issue a continue and every instruction after will
* never be executed. */
/* We need to save the event before reseting it */
- uint8_t save_event = main_pop_event ();
+ uint8_t save_event = fsm_queue_pop_event ();
/* Post the event */
- FSM_HANDLE_EVENT (&ai_fsm, save_event);
+ FSM_HANDLE_VAR_E (AI, save_event);
}
/* Check obstacles. */
@@ -240,7 +211,7 @@ main_event_to_fsm (void)
position_t robot_pos;
asserv_get_position (&robot_pos);
if (robot_pos.v.y < PG_FIELD_Y_MAX)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_in_field);
+ FSM_HANDLE_E (AI, in_field);
/* TODO: Check other sensors */
}
@@ -256,7 +227,7 @@ main_init (void)
/* Enable interrupts */
sei ();
/* Main timer */
- main_timer_init ();
+ timer_init ();
/* Load parameters */
eeprom_load_param ();
/* Dirty fix */
@@ -276,10 +247,8 @@ main_init (void)
usdist_init ();
/* Top. */
top_init ();
- /* Init FSM. */
- fsm_init (&ai_fsm);
/* Start FSM. */
- fsm_handle_event (&ai_fsm, AI_EVENT_start);
+ FSM_HANDLE (AI, start);
/* PWM module */
pwm_init ();
/* Servo pos init. */
@@ -302,15 +271,15 @@ main_loop (void)
{
/* Wait for an overflow of the main timer (4.444ms) */
{
- uint8_t timer_count = main_timer_wait ();
+ uint8_t timer_count = timer_wait ();
if (main_stats_timer_)
proto_send1b('M', timer_count);
- if (timer_count == 1)
- {
- /* Main timer has reached overflow earlier!
- We are late and this is really bad. */
- TRACE (TRACE_MAIN_TIMER__LATE);
- }
+ if (timer_count == 1)
+ {
+ /* Main timer has reached overflow earlier!
+ We are late and this is really bad. */
+ TRACE (TRACE_TIMER__LATE);
+ }
}
/* Update chrono. */
@@ -706,6 +675,18 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
int
main (int argc, char **argv)
{
+ /* produce AVR's FSM headers. */
+#ifdef HOST
+ 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);
/* Initialize the main and its subsystems */
diff --git a/digital/io/src/main.h b/digital/io/src/main.h
index 9096b532..2185b90c 100644
--- a/digital/io/src/main.h
+++ b/digital/io/src/main.h
@@ -29,8 +29,4 @@ extern vect_t main_obstacles_pos[2];
extern uint8_t main_obstacles_nb;
-/** Post an event to be processed in the next main loop. */
-void
-main_post_event (uint8_t event);
-
#endif /* main_h */
diff --git a/digital/io/src/move.c b/digital/io/src/move.c
index e4f59c39..60db0272 100644
--- a/digital/io/src/move.c
+++ b/digital/io/src/move.c
@@ -22,6 +22,9 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+
+#define FSM_NAME AI
+
#include "common.h"
#include "move.h"
#include "fsm.h"
@@ -29,9 +32,25 @@
#include "radar.h"
#include "asserv.h"
#include "main.h"
-
+#include "events.h"
+#include "playground.h"
+#include "loader.h"
#include "modules/path/path.h"
#include "modules/utils/utils.h"
+#include "fsm_queue.h"
+#include "modules/math/fixed/fixed.h" /* fixed_* */
+#include "modules/trace/trace.h"
+#include "debug.host.h"
+#include <math.h>
+
+#define MOVE_LOADER_UNBLOCKING_DISTANCE 70
+
+/** Used to define a grid. If robot is outside the grid, be careful. */
+#define MOVE_GRID_X 450
+#define MOVE_GRID_X_ORIGIN 150
+#define MOVE_GRID_Y 250
+#define MOVE_GRID_Y_ORIGIN 128
+#define MOVE_GRID_MARGIN 50
/**
* Internal data used by the move FSM.
@@ -50,7 +69,7 @@ move_start (position_t position, uint8_t backward)
/* Reset try counter. */
move_data.try_again_counter = 3;
/* Start the FSM. */
- fsm_handle_event (&ai_fsm, AI_EVENT_move_start);
+ FSM_HANDLE (AI, move_start);
}
void
@@ -65,7 +84,7 @@ move_start_noangle (vect_t position, uint8_t backward, int16_t shorten)
/* Reset try counter. */
move_data.try_again_counter = 3;
/* Start the FSM. */
- fsm_handle_event (&ai_fsm, AI_EVENT_move_start);
+ FSM_HANDLE (AI, move_start);
}
void
@@ -80,15 +99,591 @@ move_obstacles_update (void)
uint8_t
move_check_obstacles (void)
{
- if (fsm_can_handle_event (&ai_fsm, AI_EVENT_obstacle_in_front))
+ 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_event (&ai_fsm, AI_EVENT_obstacle_in_front))
+ if (FSM_HANDLE (AI, obstacle_in_front))
return 1;
}
return 0;
}
+/* Define FSM */
+FSM_STATES (
+ /* waiting for the beginning of the move FSM. */
+ 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,
+ /* moving loader up and move backward to unblock loader. */
+ MOVE_LOADER_UNBLOCKING_UPING,
+ /* moving loader down. */
+ MOVE_LOADER_UNBLOCKING_DOWNING)
+
+FSM_EVENTS (
+ /* initialize the FSM and start the movement directly. */
+ move_start,
+ /* the bot has seen something (front is the same when going backward). */
+ obstacle_in_front)
+
+FSM_START_WITH (MOVE_IDLE)
+
+/** Go to current step, low level function. */
+static void
+move_go (void)
+{
+ vect_t dst = move_data.step;
+ /* Modify final point if requested. */
+ if (move_data.final_move && move_data.shorten)
+ {
+ /* Compute a vector from destination to robot with lenght
+ * 'shorten'. */
+ position_t robot_position;
+ asserv_get_position (&robot_position);
+ vect_t v = robot_position.v;
+ vect_sub (&v, &move_data.step);
+ int16_t d = vect_norm (&v);
+ if (d > move_data.shorten)
+ {
+ vect_scale_f824 (&v, 0x1000000 / d * move_data.shorten);
+ vect_translate (&dst, &v);
+ }
+ }
+ if (move_data.step_with_angle)
+ asserv_goto_xya (dst.x, dst.y, move_data.step_angle,
+ move_data.step_backward);
+ else
+ asserv_goto (dst.x, dst.y, move_data.step_backward);
+}
+
+/** Go or rotate toward position, returns 1 for linear move, 2 for angular
+ * move. */
+static uint8_t
+move_go_or_rotate (vect_t dst, uint16_t angle, uint8_t with_angle,
+ uint8_t backward)
+{
+ position_t robot_position;
+ /* Remember step. */
+ move_data.step = dst;
+ move_data.step_angle = angle;
+ move_data.step_with_angle = with_angle;
+ move_data.step_backward = backward;
+ /* Compute angle to destination. */
+ asserv_get_position (&robot_position);
+ vect_t v = dst; vect_sub (&v, &robot_position.v);
+ uint16_t dst_angle = atan2 (v.y, v.x) * ((1l << 16) / (2 * M_PI));
+ if (backward & ASSERV_BACKWARD)
+ dst_angle += 0x8000;
+ if ((backward & ASSERV_REVERT_OK)
+ && (dst_angle ^ robot_position.a) & 0x8000)
+ dst_angle += 0x8000;
+ int16_t diff = dst_angle - robot_position.a;
+ /* Move or rotate. */
+ if (UTILS_ABS (diff) < 0x1000)
+ {
+ loader_down ();
+ move_go ();
+ return 1;
+ }
+ else
+ {
+ loader_up ();
+ 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);
+ }
+ TRACE (TRACE_MOVE__GO_TO, dst.x, dst.y);
+ /* 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 not on the grid, slow down. */
+ int16_t mx = current_pos.v.x % MOVE_GRID_X;
+ int16_t my = current_pos.v.y % MOVE_GRID_Y;
+ if (mx < MOVE_GRID_X_ORIGIN - MOVE_GRID_MARGIN
+ || mx > MOVE_GRID_X_ORIGIN + MOVE_GRID_MARGIN
+ || my < MOVE_GRID_Y_ORIGIN - MOVE_GRID_MARGIN
+ || my > MOVE_GRID_Y_ORIGIN + MOVE_GRID_MARGIN
+ )
+ move_data.slow = 1;
+ /* 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,
+ /* rotate towards next position. */
+ path_found_rotate, MOVE_ROTATING,
+ /* move to next position. */
+ path_found, MOVE_MOVING,
+ /* post failure event. */
+ 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_fsm_failed));
+ return FSM_NEXT (MOVE_IDLE, move_start, no_path_found);
+ }
+}
+
+/*
+ * move to next position.
+ */
+FSM_TRANS (MOVE_ROTATING,
+ bot_move_succeed,
+ MOVE_MOVING)
+{
+ loader_down ();
+ move_go ();
+ return FSM_NEXT (MOVE_ROTATING, bot_move_succeed);
+}
+
+/*
+ * move to next position.
+ */
+FSM_TRANS (MOVE_ROTATING,
+ bot_move_failed,
+ MOVE_MOVING)
+{
+ loader_down ();
+ move_go ();
+ return FSM_NEXT (MOVE_ROTATING, bot_move_failed);
+}
+
+/*
+ * move to next position.
+ */
+FSM_TRANS_TIMEOUT (MOVE_ROTATING, 1125,
+ MOVE_MOVING)
+{
+ loader_down ();
+ move_go ();
+ return FSM_NEXT_TIMEOUT (MOVE_ROTATING);
+}
+
+/*
+ * move backward
+ * loader up
+ */
+FSM_TRANS (MOVE_ROTATING,
+ loader_errored,
+ MOVE_LOADER_UNBLOCKING_UPING)
+{
+ asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
+ loader_up ();
+ return FSM_NEXT (MOVE_ROTATING, loader_errored);
+}
+
+FSM_TRANS (MOVE_MOVING, bot_move_succeed,
+ /* post success event. */
+ done, MOVE_IDLE,
+ /* rotate towards next position. */
+ path_found_rotate, MOVE_ROTATING,
+ /* move to next position. */
+ path_found, MOVE_MOVING,
+ /* post failure event */
+ no_path_found, MOVE_IDLE)
+{
+ if (move_data.final_move)
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_fsm_succeed));
+ return FSM_NEXT (MOVE_MOVING, bot_move_succeed, done);
+ }
+ else
+ {
+ uint8_t next = move_path_next ();
+ if (next == 2)
+ return FSM_NEXT (MOVE_MOVING, bot_move_succeed, path_found_rotate);
+ else
+ return FSM_NEXT (MOVE_MOVING, bot_move_succeed, path_found);
+ }
+ //return FSM_NEXT (MOVE_MOVING, bot_move_succeed, no_path_found);
+}
+
+
+/*
+ * reset final_move.
+ * move backward to turn freely.
+ */
+void
+move_MOVE_MOVING_bot_move_failed_MOVE_MOVING_BACKWARD_TO_TURN_FREELY ()
+{
+ 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 () == 1
+ ? 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 () == 1 ?
+ - 300 : 300);
+}
+
+FSM_TRANS (MOVE_MOVING,
+ bot_move_failed,
+ MOVE_MOVING_BACKWARD_TO_TURN_FREELY)
+{
+ move_MOVE_MOVING_bot_move_failed_MOVE_MOVING_BACKWARD_TO_TURN_FREELY ();
+ return FSM_NEXT (MOVE_MOVING, bot_move_failed);
+}
+
+/*
+ * reset final_move.
+ * move backward to turn freely.
+ */
+FSM_TRANS_TIMEOUT (MOVE_MOVING, 2250,
+ MOVE_MOVING_BACKWARD_TO_TURN_FREELY)
+{
+ move_MOVE_MOVING_bot_move_failed_MOVE_MOVING_BACKWARD_TO_TURN_FREELY ();
+ return FSM_NEXT_TIMEOUT (MOVE_MOVING);
+}
+
+FSM_TRANS (MOVE_MOVING, obstacle_in_front,
+ /* reset final_move.
+ * stop the bot. */
+ tryagain, MOVE_WAIT_FOR_CLEAR_PATH,
+ /* stop the bot.
+ * post failure event. */
+ 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_fsm_failed));
+ return FSM_NEXT (MOVE_MOVING, obstacle_in_front, tryout);
+ }
+ else
+ return FSM_NEXT (MOVE_MOVING, obstacle_in_front, tryagain);
+}
+
+/*
+ * move backward
+ * loader up
+ */
+FSM_TRANS (MOVE_MOVING,
+ loader_errored,
+ MOVE_LOADER_UNBLOCKING_UPING)
+{
+ asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
+ loader_up ();
+ return FSM_NEXT (MOVE_MOVING, loader_errored);
+}
+
+FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed,
+ /* post failure event. */
+ tryout, MOVE_IDLE,
+ /* rotate towards next position. */
+ path_found_rotate, MOVE_ROTATING,
+ /* move to next position. */
+ path_found, MOVE_MOVING,
+ /* post failure event. */
+ no_path_found, MOVE_IDLE)
+{
+ if (--move_data.try_again_counter == 0)
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_fsm_failed));
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, tryout);
+ }
+ else
+ {
+ uint8_t next = move_path_init ();
+ if (next)
+ {
+ if (next == 2)
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, path_found_rotate);
+ else
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, path_found);
+ }
+ else
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_fsm_failed));
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, no_path_found);
+ }
+ }
+}
+
+FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed,
+ /* post failure event. */
+ tryout, MOVE_IDLE,
+ /* rotate towards next position. */
+ path_found_rotate, MOVE_ROTATING,
+ /* move to next position. */
+ path_found, MOVE_MOVING,
+ /* nothing to do. */
+ no_path_found_tryagain, MOVE_WAIT_FOR_CLEAR_PATH,
+ /* post failure event. */
+ no_path_found_tryout, MOVE_IDLE)
+{
+ if (--move_data.try_again_counter == 0)
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_fsm_failed));
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, tryout);
+ }
+ else
+ {
+ uint8_t next = move_path_init ();
+ if (next)
+ {
+ if (next == 2)
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, path_found_rotate);
+ else
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, path_found);
+ }
+ else
+ {
+ if (--move_data.try_again_counter == 0)
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_fsm_failed));
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_path_found_tryout);
+ }
+ else
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_path_found_tryagain);
+ }
+ }
+}
+
+FSM_TRANS_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, 255,
+ /* rotate towards next position. */
+ path_found_rotate, MOVE_ROTATING,
+ /* move to next position. */
+ path_found, MOVE_MOVING,
+ /* decrement counter. */
+ no_path_found_tryagain, MOVE_WAIT_FOR_CLEAR_PATH,
+ /* post failure. */
+ 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_fsm_failed));
+ 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);
+ }
+}
+
+/*
+ * loader down
+ * reset unblocking retry counter
+ */
+FSM_TRANS (MOVE_LOADER_UNBLOCKING_UPING,
+ bot_move_succeed,
+ MOVE_LOADER_UNBLOCKING_DOWNING)
+{
+ loader_down ();
+ move_data.loader_unblocking_retry = 2;
+ return FSM_NEXT (MOVE_LOADER_UNBLOCKING_UPING, bot_move_succeed);
+}
+
+/*
+ * loader down
+ * reset unblocking retry counter
+ */
+FSM_TRANS (MOVE_LOADER_UNBLOCKING_UPING,
+ bot_move_failed,
+ MOVE_LOADER_UNBLOCKING_DOWNING)
+{
+ loader_down ();
+ move_data.loader_unblocking_retry = 2;
+ return FSM_NEXT (MOVE_LOADER_UNBLOCKING_UPING, bot_move_failed);
+}
+
+FSM_TRANS (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed,
+ /* rotate towards next position. */
+ path_found_rotate, MOVE_ROTATING,
+ /* move to next position. */
+ path_found, MOVE_MOVING,
+ /* post failure. */
+ no_path_found, MOVE_IDLE)
+{
+ /* Try to move. */
+ uint8_t next = move_path_init ();
+ if (next)
+ {
+ if (next == 2)
+ return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, path_found_rotate);
+ else
+ return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, path_found);
+ }
+ else
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_fsm_failed));
+ return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, no_path_found);
+ }
+}
+
+FSM_TRANS (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored,
+ /* move backward.
+ * loader up. */
+ tryagain, MOVE_LOADER_UNBLOCKING_UPING,
+ /* rotate towards next position. */
+ tryout_path_found_rotate, MOVE_ROTATING,
+ /* move to next position. */
+ tryout_path_found, MOVE_ROTATING,
+ /* post failure. */
+ tryout_no_path_found, MOVE_IDLE)
+{
+ if (--move_data.loader_unblocking_retry)
+ {
+ asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
+ loader_up ();
+ return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryagain);
+ }
+ else
+ {
+ /* Try to move. */
+ uint8_t next = move_path_init ();
+ if (next)
+ {
+ if (next == 2)
+ return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_path_found_rotate);
+ else
+ return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_path_found);
+ }
+ else
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_fsm_failed));
+ return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_no_path_found);
+ }
+ }
+}
+
+FSM_TRANS_TIMEOUT (MOVE_LOADER_UNBLOCKING_DOWNING, 450,
+ /* move backward.
+ * loader up. */
+ tryagain, MOVE_LOADER_UNBLOCKING_UPING,
+ /* rotate towards next position. */
+ tryout_path_found_rotate, MOVE_ROTATING,
+ /* move to next position. */
+ tryout_path_found, MOVE_ROTATING,
+ /* post failure. */
+ tryout_no_path_found, MOVE_IDLE)
+{
+ if (--move_data.loader_unblocking_retry)
+ {
+ asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
+ loader_up ();
+ return FSM_NEXT_TIMEOUT (MOVE_LOADER_UNBLOCKING_DOWNING, tryagain);
+ }
+ else
+ {
+ /* Try to move. */
+ uint8_t next = move_path_init ();
+ if (next)
+ {
+ if (next == 2)
+ return FSM_NEXT_TIMEOUT (MOVE_LOADER_UNBLOCKING_DOWNING, tryout_path_found_rotate);
+ else
+ return FSM_NEXT_TIMEOUT (MOVE_LOADER_UNBLOCKING_DOWNING, tryout_path_found);
+ }
+ else
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_fsm_failed));
+ return FSM_NEXT_TIMEOUT (MOVE_LOADER_UNBLOCKING_DOWNING, tryout_no_path_found);
+ }
+ }
+}
+
diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm
deleted file mode 100644
index add29b05..00000000
--- a/digital/io/src/move.fsm
+++ /dev/null
@@ -1,137 +0,0 @@
-# Move FSM
-# This FSM is responsible to move the bot from the current position to the
-# desired one. It will try to avoid obstacle and manage the case when the
-# asserv is blocked.
-move
- move with avoid obstacle.
-
-States:
- *MOVE_IDLE
- waiting for the beginning of the move FSM.
- MOVE_ROTATING [timeout=1125]
- rotating towards next point.
- MOVE_MOVING [timeout=2250]
- moving to a position (intermediate or final).
- MOVE_MOVING_BACKWARD_TO_TURN_FREELY
- moving backward to go away from what is blocking the bot.
- MOVE_WAIT_FOR_CLEAR_PATH [timeout=255]
- waiting for obstacle to disappear.
- MOVE_LOADER_UNBLOCKING_UPING
- moving loader up and move backward to unblock loader.
- MOVE_LOADER_UNBLOCKING_DOWNING [timeout=450]
- moving loader down.
-
-Events:
- move_start
- initialize the FSM and start the movement directly.
- obstacle_in_front
- the bot has seen something (front is the same when going backward).
-
-MOVE_IDLE:
- move_start: path_found_rotate -> MOVE_ROTATING
- rotate towards next position.
- move_start: path_found -> MOVE_MOVING
- move to next position.
- move_start: no_path_found -> MOVE_IDLE
- post failure event.
-
-MOVE_ROTATING:
- bot_move_succeed -> MOVE_MOVING
- move to next position.
- bot_move_failed -> MOVE_MOVING
- move to next position.
- state_timeout -> MOVE_MOVING
- move to next position.
- loader_errored -> MOVE_LOADER_UNBLOCKING_UPING
- move backward
- loader up
-
-MOVE_MOVING:
- bot_move_succeed: done -> MOVE_IDLE
- post success event.
- bot_move_succeed: path_found_rotate -> MOVE_ROTATING
- rotate towards next position.
- bot_move_succeed: path_found -> MOVE_MOVING
- move to next position.
- bot_move_succeed: no_path_found -> MOVE_IDLE
- post failure event.
- bot_move_failed -> MOVE_MOVING_BACKWARD_TO_TURN_FREELY
- reset final_move.
- move backward to turn freely.
- state_timeout -> MOVE_MOVING_BACKWARD_TO_TURN_FREELY
- reset final_move.
- move backward to turn freely.
- obstacle_in_front: tryagain -> MOVE_WAIT_FOR_CLEAR_PATH
- reset final_move.
- stop the bot.
- obstacle_in_front: tryout -> MOVE_IDLE
- stop the bot.
- post failure event.
- loader_errored -> MOVE_LOADER_UNBLOCKING_UPING
- move backward
- loader up
-
-MOVE_MOVING_BACKWARD_TO_TURN_FREELY:
- bot_move_succeed: tryout -> MOVE_IDLE
- post failure event.
- bot_move_succeed: path_found_rotate -> MOVE_ROTATING
- rotate towards next position.
- bot_move_succeed: path_found -> MOVE_MOVING
- move to next position.
- bot_move_succeed: no_path_found -> MOVE_IDLE
- post failure event.
- bot_move_failed: tryout -> MOVE_IDLE
- post failure event.
- bot_move_failed: path_found_rotate -> MOVE_ROTATING
- rotate towards next position.
- bot_move_failed: path_found -> MOVE_MOVING
- move to next position.
- bot_move_failed: no_path_found_tryagain -> MOVE_WAIT_FOR_CLEAR_PATH
- nothing to do.
- bot_move_failed: no_path_found_tryout -> MOVE_IDLE
- post failure event.
-
-MOVE_WAIT_FOR_CLEAR_PATH:
- state_timeout: path_found_rotate -> MOVE_ROTATING
- rotate towards next position.
- state_timeout: path_found -> MOVE_MOVING
- move to next position.
- state_timeout: no_path_found_tryagain -> .
- decrement counter.
- state_timeout: no_path_found_tryout -> MOVE_IDLE
- post failure.
-
-MOVE_LOADER_UNBLOCKING_UPING:
- bot_move_succeed -> MOVE_LOADER_UNBLOCKING_DOWNING
- loader down
- reset unblocking retry counter
- bot_move_failed -> MOVE_LOADER_UNBLOCKING_DOWNING
- loader down
- reset unblocking retry counter
-
-MOVE_LOADER_UNBLOCKING_DOWNING:
- loader_downed: path_found_rotate -> MOVE_ROTATING
- rotate towards next position.
- loader_downed: path_found -> MOVE_MOVING
- move to next position.
- loader_downed: no_path_found -> MOVE_IDLE
- post failure.
- loader_errored: tryagain -> MOVE_LOADER_UNBLOCKING_UPING
- move backward
- loader up
- loader_errored: tryout_path_found_rotate -> MOVE_ROTATING
- rotate towards next position.
- loader_errored: tryout_path_found -> MOVE_ROTATING
- move to next position.
- loader_errored: tryout_no_path_found -> MOVE_IDLE
- post failure.
- state_timeout: tryagain -> MOVE_LOADER_UNBLOCKING_UPING
- move backward
- loader up
- state_timeout: tryout_path_found_rotate -> MOVE_ROTATING
- rotate towards next position.
- state_timeout: tryout_path_found -> MOVE_ROTATING
- move to next position.
- state_timeout: tryout_no_path_found -> MOVE_IDLE
- post failure.
-
diff --git a/digital/io/src/playground.h b/digital/io/src/playground.h
deleted file mode 100644
index 5c251ce5..00000000
--- a/digital/io/src/playground.h
+++ /dev/null
@@ -1,96 +0,0 @@
-#ifndef playground_h
-#define playground_h
-// playground.h
-// io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
-//
-// Copyright (C) 2008 Dufour Jérémy
-//
-// Robot APB Team/Efrei 2004.
-// Web: http://assos.efrei.fr/robot/
-// Email: robot AT efrei DOT fr
-//
-// 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.
-//
-// }}}
-
-/**
- * @file Some defines for the playground.
- * For example, you can find the size of the table, the positions of the
- * distributors, ...
- * A few important remarks:
- * - the (0,0) position is the bottom left position on the table (when you
- * see it with the two start zone at the top of the scheme).
- */
-
-#include "defs.h"
-#include "bot.h"
-
-/**
- * The width of the table, in millimeters.
- */
-#define PG_WIDTH 3000
-
-/**
- * The length of the table, in millimeters.
- */
-#define PG_LENGTH 2100
-
-/**
- * The distance from table border for movements.
- */
-#define PG_BORDER_DISTANCE 250
-
-/**
- * Considering there is a symmetry axis on X, this macro will compute the
- * value for on the X axis depending on the color.
- */
-#define PG_X(x) (bot_color ? (x) : PG_WIDTH - (x))
-
-/** Same as PG_Y, but for Y coordinate. Actually nothing is done, there is no
- * symmetry. */
-#define PG_Y(y) (y)
-
-/**
- * Considering there is a symmetry axis on X, this macro will compute the
- * value of the angle depending on the color.
- *
- * Takes degrees as input.
- */
-#define PG_A_DEG(a) \
- (bot_color ? POSITION_A_DEG (a) : POSITION_A_DEG (180 - (a)))
-
-/** Initialiser for position_t applying symmetry according to color. Takes
- * degrees for angle. */
-#define PG_POSITION_DEG(x, y, a) \
- { { PG_X (x), PG_Y (y) }, PG_A_DEG (a) }
-
-/** Initialiser for vect_t applying symmetry according to color. */
-#define PG_VECT(x, y) \
- (vect_t) { PG_X (x), PG_Y (y) }
-
-/**
- * Start zone.
- */
-#define PG_START_ZONE_LENGTH 500
-#define PG_START_ZONE_WIDTH 500
-
-/** Size of the unclimbable slope zone (Eurobot 2010). */
-#define PG_SLOPE_WIDTH (500 + 519 + 500)
-#define PG_SLOPE_LENGTH (500 + 22)
-
-/** Start of field zone. */
-#define PG_FIELD_Y_MAX 1128
-
-#endif // playground_h
diff --git a/digital/io/src/playground_2010.h b/digital/io/src/playground_2010.h
new file mode 100644
index 00000000..11682c5e
--- /dev/null
+++ b/digital/io/src/playground_2010.h
@@ -0,0 +1,46 @@
+#ifndef playground_2010_h
+#define playground_2010_h
+/* playground_2010.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.
+ *
+ * }}} */
+#include "playground.h"
+
+/**
+ * Eurobot 2010 specific defines.
+ */
+
+/**
+ * Start zone.
+ */
+#define PG_START_ZONE_LENGTH 500
+#define PG_START_ZONE_WIDTH 500
+
+/** Size of the unclimbable slope zone. */
+#define PG_SLOPE_WIDTH (500 + 519 + 500)
+#define PG_SLOPE_LENGTH (500 + 22)
+
+/** Start of field zone. */
+#define PG_FIELD_Y_MAX 1128
+
+#endif /* playground_2010_h */
diff --git a/digital/io/src/radar.c b/digital/io/src/radar.c
index 1ba2a437..1ac982c7 100644
--- a/digital/io/src/radar.c
+++ b/digital/io/src/radar.c
@@ -25,7 +25,8 @@
#include "common.h"
#include "radar.h"
-#include "playground.h"
+#include "playground_2010.h"
+#include "bot.h"
#include "usdist.h"
#include "modules/math/geometry/geometry.h"
diff --git a/digital/io/src/simu.host.c b/digital/io/src/simu.host.c
index 276c3b14..f1b7e0d2 100644
--- a/digital/io/src/simu.host.c
+++ b/digital/io/src/simu.host.c
@@ -214,13 +214,13 @@ switch_update (void)
}
void
-main_timer_init (void)
+timer_init (void)
{
simu_init ();
}
uint8_t
-main_timer_wait (void)
+timer_wait (void)
{
mex_node_wait_date (mex_node_date () + 4);
simu_step ();
diff --git a/digital/io/src/switch.avr.c b/digital/io/src/switch.avr.c
index 659b597a..9907a86f 100644
--- a/digital/io/src/switch.avr.c
+++ b/digital/io/src/switch.avr.c
@@ -22,10 +22,10 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+#include "common.h"
#include "switch.h"
-#include "common.h"
#include "modules/utils/utils.h" /* set_bit */
#include "io.h" /* PORT/PIN, bit_is_set */
diff --git a/digital/io/src/switch.h b/digital/io/src/switch.h
index ac8d7dbc..9a804d1b 100644
--- a/digital/io/src/switch.h
+++ b/digital/io/src/switch.h
@@ -29,8 +29,7 @@
* @file Module to manage 'switchs'. For example, colors selector and jack.
*/
-#include "bot.h"
-#include "common.h"
+#include "defs.h"
/**
* Initialize the switch module.
diff --git a/digital/io/src/top.c b/digital/io/src/top.c
index 5bfeaa4d..e899d718 100644
--- a/digital/io/src/top.c
+++ b/digital/io/src/top.c
@@ -22,6 +22,10 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+
+#define FSM_NAME AI
+
+#include "fsm.h"
#include "common.h"
#include "defs.h"
#include "top.h"
@@ -31,6 +35,7 @@
#include "move.h"
#include "chrono.h"
#include "playground.h"
+#include "bot.h"
#include "modules/utils/utils.h"
@@ -88,3 +93,358 @@ top_collect (uint8_t force)
}
}
+FSM_INIT
+
+FSM_STATES (IDLE,
+ WAIT_INIT_TO_FINISH,
+ FIRST_GO_BEGIN_OF_LINE_FAST,
+ FIRST_GO_END_OF_LINE_FAST,
+ FIRST_GO_END_OF_LINE_SLOW,
+ UNLOAD,
+ UNLOAD_LOADER_UP,
+ UNLOAD_FACE_BIN,
+ UNLOAD_FACE_BIN_UNBLOCK,
+ UNLOAD_BACK_BIN,
+ UNLOAD_UNLOAD,
+ COLLECT,
+ COLLECT_SLOW_MOTION,
+ COLLECT_BLACK)
+
+FSM_EVENTS (start,
+ move_fsm_succeed,
+ move_fsm_failed,
+ bot_move_succeed,
+ bot_move_failed,
+ init_match_is_started,
+ in_field,
+ asserv_last_cmd_ack,
+ clamp_succeed)
+
+FSM_START_WITH (IDLE)
+
+/* Nothing to do. */
+FSM_TRANS (IDLE, start, WAIT_INIT_TO_FINISH)
+{
+ return FSM_NEXT (IDLE, start);
+}
+
+/*
+ * the match start
+ * set fast speed
+ * go to first sequence begin of line
+ */
+FSM_TRANS (WAIT_INIT_TO_FINISH,
+ init_match_is_started,
+ FIRST_GO_BEGIN_OF_LINE_FAST)
+{
+ asserv_set_speed (BOT_MOVE_FAST);
+ move_start_noangle (PG_VECT (375, 1503), 0, 0);
+ return FSM_NEXT (WAIT_INIT_TO_FINISH, init_match_is_started);
+}
+
+/*
+ * go to end of line
+ * loader down
+ */
+FSM_TRANS (FIRST_GO_BEGIN_OF_LINE_FAST,
+ move_fsm_succeed,
+ FIRST_GO_END_OF_LINE_FAST)
+{
+ move_start_noangle (PG_VECT (2625, 253), 0, 0);
+ loader_down ();
+ return FSM_NEXT (FIRST_GO_BEGIN_OF_LINE_FAST, move_fsm_succeed);
+}
+
+/*
+ * set slow speed
+ * retry
+ */
+FSM_TRANS (FIRST_GO_BEGIN_OF_LINE_FAST,
+ move_fsm_failed,
+ FIRST_GO_BEGIN_OF_LINE_FAST)
+{
+ asserv_set_speed (BOT_MOVE_SLOW);
+ move_start_noangle (PG_VECT (375, 1503), 0, 0);
+ return FSM_NEXT (FIRST_GO_BEGIN_OF_LINE_FAST, move_fsm_failed);
+}
+
+/*
+ * set slow speed
+ */
+FSM_TRANS (FIRST_GO_END_OF_LINE_FAST,
+ in_field,
+ FIRST_GO_END_OF_LINE_SLOW)
+{
+ asserv_set_speed (BOT_MOVE_SLOW);
+ return FSM_NEXT (FIRST_GO_END_OF_LINE_FAST, in_field);
+}
+
+/*
+ * set slow speed
+ * move loader up
+ */
+FSM_TRANS (FIRST_GO_END_OF_LINE_FAST,
+ move_fsm_succeed,
+ UNLOAD_LOADER_UP)
+{
+ asserv_set_speed (BOT_MOVE_SLOW);
+ loader_up ();
+ return FSM_NEXT (FIRST_GO_END_OF_LINE_FAST, move_fsm_succeed);
+}
+
+/*
+ * set slow speed
+ * retry
+ */
+FSM_TRANS (FIRST_GO_END_OF_LINE_FAST,
+ move_fsm_failed,
+ FIRST_GO_END_OF_LINE_SLOW)
+{
+ asserv_set_speed (BOT_MOVE_SLOW);
+ move_start_noangle (PG_VECT (2625, 253), 0, 0);
+ return FSM_NEXT (FIRST_GO_END_OF_LINE_FAST, move_fsm_failed);
+}
+
+/*
+ * move loader up
+ */
+FSM_TRANS (FIRST_GO_END_OF_LINE_SLOW,
+ move_fsm_succeed,
+ UNLOAD_LOADER_UP)
+{
+ loader_up ();
+ return FSM_NEXT (FIRST_GO_END_OF_LINE_SLOW, move_fsm_succeed);
+}
+
+/*
+ * retry
+ */
+FSM_TRANS (FIRST_GO_END_OF_LINE_SLOW,
+ move_fsm_failed,
+ FIRST_GO_END_OF_LINE_SLOW)
+{
+ move_start_noangle (PG_VECT (2625, 253), 0, 0);
+ return FSM_NEXT (FIRST_GO_END_OF_LINE_SLOW, move_fsm_failed);
+}
+
+/*
+ * move loader up
+ */
+FSM_TRANS (UNLOAD, move_fsm_succeed, UNLOAD_LOADER_UP)
+{
+ loader_up ();
+ return FSM_NEXT (UNLOAD, move_fsm_succeed);
+}
+
+/*
+ * retry
+ */
+FSM_TRANS (UNLOAD, move_fsm_failed, UNLOAD)
+{
+ move_start_noangle (PG_VECT (2625, 253), 0, 0);
+ return FSM_NEXT (UNLOAD, move_fsm_failed);
+}
+
+/*
+ * turn toward bin
+ */
+FSM_TRANS (UNLOAD_LOADER_UP, loader_uped, UNLOAD_FACE_BIN)
+{
+ asserv_goto_angle (PG_A_DEG (90));
+ return FSM_NEXT (UNLOAD_LOADER_UP, loader_uped);
+}
+
+/*
+ * turn toward bin
+ */
+FSM_TRANS (UNLOAD_LOADER_UP, loader_errored, UNLOAD_FACE_BIN)
+{
+ asserv_goto_angle (PG_A_DEG (90));
+ return FSM_NEXT (UNLOAD_LOADER_UP, loader_errored);
+}
+
+/*
+ * go backward to bin
+ */
+FSM_TRANS (UNLOAD_FACE_BIN, bot_move_succeed, UNLOAD_BACK_BIN)
+{
+ asserv_move_linearly (-(128 + 250 / 2 - BOT_SIZE_BACK - 50));
+ return FSM_NEXT (UNLOAD_FACE_BIN, bot_move_succeed);
+}
+
+/*
+ * move backward
+ */
+FSM_TRANS (UNLOAD_FACE_BIN, bot_move_failed, UNLOAD_FACE_BIN_UNBLOCK)
+{
+ asserv_move_linearly (-40);
+ return FSM_NEXT (UNLOAD_FACE_BIN, bot_move_failed);
+}
+
+/*
+ * turn toward bin
+ */
+FSM_TRANS (UNLOAD_FACE_BIN_UNBLOCK, bot_move_succeed, UNLOAD_FACE_BIN)
+{
+ asserv_goto_angle (PG_A_DEG (90));
+ return FSM_NEXT (UNLOAD_FACE_BIN_UNBLOCK, bot_move_succeed);
+}
+
+/*
+ * turn toward bin
+ */
+FSM_TRANS (UNLOAD_FACE_BIN_UNBLOCK, bot_move_failed, UNLOAD_FACE_BIN)
+{
+ asserv_goto_angle (PG_A_DEG (90));
+ return FSM_NEXT (UNLOAD_FACE_BIN_UNBLOCK, bot_move_failed);
+}
+
+/*
+ * unload
+ */
+FSM_TRANS (UNLOAD_BACK_BIN, bot_move_succeed, UNLOAD_UNLOAD)
+{
+ asserv_move_motor1_absolute (0, BOT_GATE_SPEED);
+ return FSM_NEXT (UNLOAD_BACK_BIN, bot_move_succeed);
+}
+
+/*
+ * unload
+ */
+FSM_TRANS (UNLOAD_BACK_BIN, bot_move_failed, UNLOAD_UNLOAD)
+{
+ asserv_move_motor1_absolute (0, BOT_GATE_SPEED);
+ return FSM_NEXT (UNLOAD_BACK_BIN, bot_move_failed);
+}
+
+/*
+ * close gate
+ * loader down
+ * choose best food to collect
+ */
+FSM_TRANS_TIMEOUT (UNLOAD_UNLOAD, 255, COLLECT)
+{
+ loader_elements = 0;
+ asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED);
+ loader_down ();
+ top_collect (1);
+ return FSM_NEXT_TIMEOUT (UNLOAD_UNLOAD);
+}
+
+/*
+ * post loader_element event
+ */
+FSM_TRANS (COLLECT, move_fsm_succeed,
+ unload, UNLOAD,
+ collect, COLLECT,
+ slow_motion, COLLECT_SLOW_MOTION)
+{
+ int16_t slow_motion = food_slow_motion (top_food);
+ if (slow_motion)
+ {
+ FSM_HANDLE (AI, loader_element);
+ return FSM_NEXT (COLLECT, move_fsm_succeed, slow_motion);
+ }
+ else if (top_collect (0))
+ return FSM_NEXT (COLLECT, move_fsm_succeed, collect);
+ else
+ return FSM_NEXT (COLLECT, move_fsm_succeed, unload);
+}
+
+/*
+ * cancel this element
+ */
+FSM_TRANS (COLLECT, move_fsm_failed,
+ unload, UNLOAD,
+ collect, COLLECT)
+{
+ if (top_collect (0))
+ {
+ food_cancel (top_food);
+ return FSM_NEXT (COLLECT, move_fsm_failed, collect);
+ }
+ else
+ return FSM_NEXT (COLLECT, move_fsm_failed, unload);
+}
+
+/*
+ * move backward
+ * mark as black
+ */
+FSM_TRANS (COLLECT, loader_black, COLLECT_BLACK)
+{
+ asserv_move_linearly (-90);
+ food_black (top_food);
+ return FSM_NEXT (COLLECT, loader_black);
+}
+
+/*
+ * collect
+ */
+FSM_TRANS (COLLECT_SLOW_MOTION, loader_downed,
+ unload, UNLOAD,
+ collect, COLLECT)
+{
+ if (top_collect (0))
+ return FSM_NEXT (COLLECT_SLOW_MOTION, loader_downed, collect);
+ else
+ return FSM_NEXT (COLLECT_SLOW_MOTION, loader_downed, unload);
+}
+
+/*
+ * same as above
+ */
+FSM_TRANS (COLLECT_SLOW_MOTION, loader_errored,
+ unload, UNLOAD,
+ collect, COLLECT)
+{
+ if (top_collect (0))
+ return FSM_NEXT (COLLECT_SLOW_MOTION, loader_errored, collect);
+ else
+ return FSM_NEXT (COLLECT_SLOW_MOTION, loader_errored, unload);
+}
+
+/*
+ * same as above
+ */
+FSM_TRANS_TIMEOUT (COLLECT_SLOW_MOTION, 450,
+ unload, UNLOAD,
+ collect, COLLECT)
+{
+ if (top_collect (0))
+ return FSM_NEXT_TIMEOUT (COLLECT_SLOW_MOTION, collect);
+ else
+ return FSM_NEXT_TIMEOUT (COLLECT_SLOW_MOTION, unload);
+}
+
+/*
+ * move backward
+ * mark as black
+ */
+FSM_TRANS (COLLECT_SLOW_MOTION, loader_black, COLLECT_BLACK)
+{
+ asserv_move_linearly (-90);
+ food_black (top_food);
+ return FSM_NEXT (COLLECT_SLOW_MOTION, loader_black);
+}
+
+FSM_TRANS (COLLECT_BLACK, bot_move_succeed,
+ unload, UNLOAD,
+ collect, COLLECT)
+{
+ if (top_collect (0))
+ return FSM_NEXT (COLLECT_BLACK, bot_move_succeed, collect);
+ else
+ return FSM_NEXT (COLLECT_BLACK, bot_move_succeed, unload);
+}
+
+FSM_TRANS (COLLECT_BLACK, bot_move_failed,
+ unload, UNLOAD,
+ collect, COLLECT)
+{
+ if (top_collect (0))
+ return FSM_NEXT (COLLECT_BLACK, bot_move_failed, collect);
+ else
+ return FSM_NEXT (COLLECT_BLACK, bot_move_failed, unload);
+}
+
diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm
deleted file mode 100644
index 277f0a5a..00000000
--- a/digital/io/src/top.fsm
+++ /dev/null
@@ -1,162 +0,0 @@
-# Top FSM.
-top
- Main FSM calling other FSM.
-
-States:
- *IDLE
- waiting for the beginning of the top FSM.
- WAIT_INIT_TO_FINISH
- waiting for the end of init.
- FIRST_GO_BEGIN_OF_LINE_FAST
- first sequence, go to start of diagonal line
- FIRST_GO_END_OF_LINE_FAST
- first sequence, go to end of line
- FIRST_GO_END_OF_LINE_SLOW
- slow down to take elements
- UNLOAD
- go to unloading position
- UNLOAD_LOADER_UP
- move loader to up position
- UNLOAD_FACE_BIN
- orient robot to unload angle
- UNLOAD_FACE_BIN_UNBLOCK
- move backward to unblock
- UNLOAD_BACK_BIN
- move back to unload bin
- UNLOAD_UNLOAD[timeout=225]
- unloading, wait
- COLLECT
- collecting elements
- COLLECT_SLOW_MOTION[timeout=450]
- picking element before bumper contact
- COLLECT_BLACK
- moving backward after trying to collect a black corn
-
-Events:
- start
- initialize the FSM.
- move_fsm_succeed
- when the move FSM returns with success.
- move_fsm_failed
- when the move FSM returns with an error.
- bot_move_succeed
- when the bot has succeed its asserv movement.
- bot_move_failed
- when the bot has failed its asserv movement.
- init_match_is_started
- when the init FSM has finished and the match start.
- in_field
- the robot is in the zone containing interesting elements.
- state_timeout
- timeout in cycle count.
- asserv_last_cmd_ack
- last command sent to the asserv board has been acquitted.
- clamp_succeed
- asserv success result
-
-import init.fsm
-import hola.fsm
-import loader.fsm
-import move.fsm
-
-IDLE:
- start -> WAIT_INIT_TO_FINISH
- nothing to do.
-
-WAIT_INIT_TO_FINISH:
- init_match_is_started -> FIRST_GO_BEGIN_OF_LINE_FAST
- the match start
- set fast speed
- go to first sequence begin of line
-
-FIRST_GO_BEGIN_OF_LINE_FAST:
- move_fsm_succeed -> FIRST_GO_END_OF_LINE_FAST
- go to end of line
- loader down
- move_fsm_failed -> .
- set slow speed
- retry
-
-FIRST_GO_END_OF_LINE_FAST:
- in_field -> FIRST_GO_END_OF_LINE_SLOW
- set slow speed
- move_fsm_succeed -> UNLOAD_LOADER_UP
- set slow speed
- move loader up
- move_fsm_failed -> FIRST_GO_END_OF_LINE_SLOW
- set slow speed
- retry
-
-FIRST_GO_END_OF_LINE_SLOW:
- move_fsm_succeed -> UNLOAD_LOADER_UP
- move loader up
- move_fsm_failed -> FIRST_GO_END_OF_LINE_SLOW
- retry
-
-UNLOAD:
- move_fsm_succeed -> UNLOAD_LOADER_UP
- move loader up
- move_fsm_failed -> .
- retry
-
-UNLOAD_LOADER_UP:
- loader_uped -> UNLOAD_FACE_BIN
- turn toward bin
- loader_errored -> UNLOAD_FACE_BIN
- turn toward bin
-
-UNLOAD_FACE_BIN:
- bot_move_succeed -> UNLOAD_BACK_BIN
- go backward to bin
- bot_move_failed -> UNLOAD_FACE_BIN_UNBLOCK
- move backward
-
-UNLOAD_FACE_BIN_UNBLOCK:
- bot_move_succeed -> UNLOAD_FACE_BIN
- turn toward bin
- bot_move_failed -> UNLOAD_FACE_BIN
- turn toward bin
-
-UNLOAD_BACK_BIN:
- bot_move_succeed -> UNLOAD_UNLOAD
- unload
- bot_move_failed -> UNLOAD_UNLOAD
- unload
-
-UNLOAD_UNLOAD:
- state_timeout -> COLLECT
- close gate
- loader down
- choose best food to collect
-
-COLLECT:
- move_fsm_succeed: unload -> UNLOAD
- move_fsm_succeed: collect -> COLLECT
- move_fsm_succeed: slow_motion -> COLLECT_SLOW_MOTION
- post loader_element event
- move_fsm_failed: unload -> UNLOAD
- move_fsm_failed: collect -> COLLECT
- cancel this element
- loader_black -> COLLECT_BLACK
- move backward
- mark as black
-
-COLLECT_SLOW_MOTION:
- loader_downed: unload -> UNLOAD
- loader_downed: collect -> COLLECT
- collect
- loader_errored: unload -> UNLOAD
- loader_errored: collect -> COLLECT
- same as above
- state_timeout: unload -> UNLOAD
- state_timeout: collect -> COLLECT
- same as above
- loader_black -> COLLECT_BLACK
- move backward
- mark as black
-
-COLLECT_BLACK:
- bot_move_succeed: unload -> UNLOAD
- bot_move_succeed: collect -> COLLECT
- bot_move_failed: unload -> UNLOAD
- bot_move_failed: collect -> COLLECT
diff --git a/digital/io/src/trace.trc b/digital/io/src/trace.trc
index 4cdcf8e0..1da5012b 100644
--- a/digital/io/src/trace.trc
+++ b/digital/io/src/trace.trc
@@ -1,4 +1,4 @@
-EVENT main_timer__late "MainTimer: we are late."
+EVENT timer__late "Timer: we are late."
EVENT fsm__handle_event from 1 event 1 to 1 "FSM: %i =%i=> %i."
EVENT move__go_to xd 2 yd 2 "Move: (%d, %d)."
EVENT food__best food 1 "Food best: %d."
diff --git a/digital/io/src/usdist.c b/digital/io/src/usdist.c
index df26ea79..d6fd9730 100644
--- a/digital/io/src/usdist.c
+++ b/digital/io/src/usdist.c
@@ -24,7 +24,7 @@
* }}} */
#include "common.h"
#include "usdist.h"
-#include "main_timer.h"
+#include "timer.h"
#include "modules/adc/adc.h"
#include "modules/utils/utils.h"
diff --git a/digital/io/src/usdist.h b/digital/io/src/usdist.h
index 541d1c93..9a4a6646 100644
--- a/digital/io/src/usdist.h
+++ b/digital/io/src/usdist.h
@@ -42,7 +42,7 @@
#define USDIST_MM_TOO_FAR 650
/** Measuring period in cycles. */
-#define USDIST_PERIOD_CYCLE (uint8_t) (8.0 / MT_TC0_PERIOD)
+#define USDIST_PERIOD_CYCLE (uint8_t) (8.0 / TIMER_PERIOD_MS)
/** Array containing the last measures in millimeters. */
extern uint16_t usdist_mm[USDIST_NB];
diff --git a/digital/io/tools/dump_usdist.py b/digital/io/tools/dump_usdist.py
index 43e0724e..001a8cc7 100644
--- a/digital/io/tools/dump_usdist.py
+++ b/digital/io/tools/dump_usdist.py
@@ -1,11 +1,8 @@
-import sys
-
import io
import io.init
-import serial
+from utils.init_proto import init_proto
-fd = serial.Serial (sys.argv[1])
-p = io.Proto (fd, **io.init.target)
+p = init_proto ('marcel', io.Proto, io.init)
def cb (*val):
l = [ ]
for v in val:
diff --git a/digital/io/tools/io/init.py b/digital/io/tools/io/init.py
index 059ea408..ebcb1ec9 100644
--- a/digital/io/tools/io/init.py
+++ b/digital/io/tools/io/init.py
@@ -1,6 +1,12 @@
"""Default parameters for io."""
-host = dict (
- servo_pos = ((1, 255), (255, 1), (1, 255), (1, 255), (1, 255),
- (1, 255), (1, 255), (1, 255)),
- )
+host = {
+ 'aquajim': dict (
+ servo_pos = ((1, 255), (255, 1), (1, 255), (1, 255), (1, 255),
+ (1, 255), (1, 255), (1, 255)),
+ ),
+ 'marcel': dict (
+ ),
+ 'robospierre': dict (
+ ),
+ }
target = host
diff --git a/digital/io/tools/io/io.py b/digital/io/tools/io/io.py
index 87471bbd..edefbd9d 100644
--- a/digital/io/tools/io/io.py
+++ b/digital/io/tools/io/io.py
@@ -34,8 +34,9 @@ class Proto:
def send_param (self):
p = self.param
- for i, t in enumerate (p['servo_pos']):
- self.proto.send ('p', 'BBB', i, *t)
+ if ('servo_pos') in p:
+ for i, t in enumerate (p['servo_pos']):
+ self.proto.send ('p', 'BBB', i, *t)
def write_eeprom (self):
self.proto.send ('p', 'cc', 'E', 's')
@@ -53,7 +54,7 @@ class Proto:
self.proto.send ('s', 'BB', servo, high_time)
def pwm_set (self, value, timer):
- self.proto.send ('w', 'Hh', value, timer)
+ self.proto.send ('w', 'hH', value, timer)
def loader (self, command):
self.proto.send ('c', 'c', command)
diff --git a/digital/mimot/src/dirty/counter_ext.avr.c b/digital/mimot/src/dirty/counter_ext.avr.c
index 630f6cec..54bfa9f7 100644
--- a/digital/mimot/src/dirty/counter_ext.avr.c
+++ b/digital/mimot/src/dirty/counter_ext.avr.c
@@ -156,6 +156,7 @@ counter_update (void)
counter_update_step ();
/* Left counter. */
uint16_t left = counter_left_new_step;
+ left &= 0xffff << COUNTER_LEFT_SHIFT; /* Reset unused bits. */
#if !COUNTER_LEFT_REVERSE
counter_left_diff = (int16_t) (left - counter_left_old);
#else
@@ -166,6 +167,7 @@ counter_update (void)
counter_left += counter_left_diff;
/* Right counter. */
uint16_t right = counter_right_new_step;
+ right &= 0xffff << COUNTER_RIGHT_SHIFT; /* Reset unused bits. */
#if !COUNTER_RIGHT_REVERSE
counter_right_diff = (int16_t) (right - counter_right_old);
#else
diff --git a/digital/mimot/tools/inter_mimot.py b/digital/mimot/tools/inter_mimot.py
index 219c432b..414b61df 100644
--- a/digital/mimot/tools/inter_mimot.py
+++ b/digital/mimot/tools/inter_mimot.py
@@ -26,8 +26,7 @@ import math
import mimot
import mimot.init
-import proto.popen_io
-import serial
+from utils.init_proto import init_proto
from inter.inter import Inter
from Tkinter import *
@@ -35,15 +34,9 @@ from Tkinter import *
class InterMimot (Inter):
"""Inter, communicating with the mimot board."""
- def __init__ (self, argv):
+ def __init__ (self):
# Asserv.
- if argv[0] == '!':
- io = proto.popen_io.PopenIO (argv[1:])
- i = mimot.init.host
- else:
- io = serial.Serial (argv[0])
- i = mimot.init.target
- self.a = mimot.Proto (io, **i)
+ self.a = init_proto ('giboulee', mimot.Proto, mimot.init)
self.a.async = True
# Inter.
Inter.__init__ (self)
@@ -128,8 +121,7 @@ class InterMimot (Inter):
self.a.ftw (self.backwardVar.get ())
if __name__ == '__main__':
- import sys
- app = InterMimot (sys.argv[1:])
+ app = InterMimot ()
try:
app.mainloop ()
finally:
diff --git a/digital/mimot/tools/mimot/init.py b/digital/mimot/tools/mimot/init.py
index 471a27bb..1900de67 100644
--- a/digital/mimot/tools/mimot/init.py
+++ b/digital/mimot/tools/mimot/init.py
@@ -1,5 +1,5 @@
"""Default parameters for asserv."""
-host = dict (
+host_marcel = dict (
scale = 0.0395840674352314, f = 0xdd1,
tkp = 1, tkd = 16,
ta = 0.75, tsm = 0x20, tss = 0x10,
@@ -8,7 +8,12 @@ host = dict (
E = 0x3ff, D = 0x1ff,
l = 0x1000,
)
-target = dict (
+host = {
+ 'tazg': host_marcel,
+ 'giboulee': host_marcel,
+ 'marcel': host_marcel,
+ }
+target_marcel = dict (
scale = 0.0415178942124, f = 0xcef,
c = float (0x00ffbabf) / (1 << 24),
tkp = 1, tkd = 16,
@@ -19,3 +24,8 @@ target = dict (
l = 0x1000,
w = 0x00,
)
+target = {
+ 'tazg': target_marcel,
+ 'giboulee': target_marcel,
+ 'marcel': target_marcel,
+ }
diff --git a/digital/mimot/tools/write_eeprom.py b/digital/mimot/tools/write_eeprom.py
index ab2ae315..7fe54342 100644
--- a/digital/mimot/tools/write_eeprom.py
+++ b/digital/mimot/tools/write_eeprom.py
@@ -1,16 +1,7 @@
-import sys
-
import mimot
import mimot.init
-import proto.popen_io
-import serial
+from utils.init_proto import init_proto
-if sys.argv[1] == '!':
- io = proto.popen_io.PopenIO (sys.argv[2:])
- init = mimot.init.host
-else:
- io = serial.Serial (sys.argv[1])
- init = mimot.init.target
-a = mimot.Proto (io, **init)
-a.write_eeprom ()
-a.close ()
+m = init_proto (None, mimot.Proto, mimot.init)
+m.write_eeprom ()
+m.close ()