summaryrefslogtreecommitdiff
path: root/digital/ai/src
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/ai/src
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/ai/src')
-rw-r--r--digital/ai/src/common/debug.host.h42
-rw-r--r--digital/ai/src/common/defs.h56
-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.c449
-rw-r--r--digital/ai/src/twi_master/asserv.h297
-rw-r--r--digital/ai/src/twi_master/mimot.c183
-rw-r--r--digital/ai/src/twi_master/mimot.h95
-rw-r--r--digital/ai/src/twi_master/twi_master.c219
-rw-r--r--digital/ai/src/twi_master/twi_master.h68
-rw-r--r--digital/ai/src/utils/chrono.c150
-rw-r--r--digital/ai/src/utils/chrono.h99
-rw-r--r--digital/ai/src/utils/timer.avr.c68
-rw-r--r--digital/ai/src/utils/timer.h68
17 files changed, 4107 insertions, 0 deletions
diff --git a/digital/ai/src/common/debug.host.h b/digital/ai/src/common/debug.host.h
new file mode 100644
index 00000000..3fb7e23f
--- /dev/null
+++ b/digital/ai/src/common/debug.host.h
@@ -0,0 +1,42 @@
+#ifndef debug_host_h
+#define debug_host_h
+/* debug.host.h */
+/* ai - Robot Artificial Intelligence. {{{
+ *
+ * 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.
+ *
+ * }}} */
+
+/**
+ * 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...) \
+ do { fprintf (stderr, (format), ## args); } while (0)
+#else /* HOST */
+# define DPRINTF(format, args...)
+#endif /* HOST */
+
+#endif /* debug_host_h */
diff --git a/digital/ai/src/common/defs.h b/digital/ai/src/common/defs.h
new file mode 100644
index 00000000..f16e9e52
--- /dev/null
+++ b/digital/ai/src/common/defs.h
@@ -0,0 +1,56 @@
+#ifndef defs_h
+#define defs_h
+/* defs.h */
+/* ai - Robot Artificial Intelligence. {{{
+ *
+ * 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 "modules/math/geometry/geometry.h"
+#include "modules/math/geometry/vect.h"
+
+/** General purpose defines. */
+
+/** Structure defining a position with an angle. */
+struct position_t
+{
+ /** Cartesian position in millimeters, (0, 0) is at bottom left. */
+ vect_t v;
+ /** Angle, counter-clockwise, [0, 1), f0.16.
+ * For example, 0x8000 means 0.5, which means 180 degrees. */
+ uint16_t a;
+};
+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/ai/src/twi_master/asserv.c b/digital/ai/src/twi_master/asserv.c
new file mode 100644
index 00000000..d066737b
--- /dev/null
+++ b/digital/ai/src/twi_master/asserv.c
@@ -0,0 +1,449 @@
+/* asserv.c */
+/* ai - Robot Artificial Intelligence. {{{
+ *
+ * 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 "common.h"
+#include "asserv.h"
+
+#include "twi_master.h"
+
+#include "modules/utils/byte.h"
+#include "modules/math/fixed/fixed.h"
+#include "bot.h"
+#include "io.h"
+
+/**
+ * Flag bit position value for the status byte of the asserv.
+ */
+enum asserv_status_flag_e
+{
+ /** Bot movement finished with success. */
+ asserv_status_flag_move_succeed = 0,
+ /** Bot movement finished with failure: the bot is blocked. */
+ asserv_status_flag_move_failed = 1,
+ /** Bot is moving forward (linear speed greater than 0). */
+ asserv_status_flag_move_forward = 2,
+ /** Bot is moving backward (linear speed smaller than 0). */
+ asserv_status_flag_move_backward = 3,
+ /** Motor0 movement finished with success. */
+ asserv_status_flag_motor0_succeed = 4,
+ /** Motor0 movement finished with failure. */
+ asserv_status_flag_motor0_failed = 5,
+ /** Motor1 movement finished with success. */
+ asserv_status_flag_motor1_succeed = 6,
+ /** Motor1 movement finished with failure. */
+ asserv_status_flag_motor1_failed = 7,
+};
+typedef enum asserv_status_flag_e asserv_status_flag_e;
+
+/** Scaling factor. */
+static uint32_t asserv_scale;
+
+/** Scaling factor inverse. */
+static uint32_t asserv_scale_inv;
+
+/** Last moving direction. */
+static uint8_t asserv_last_moving_direction;
+
+/**
+ * Structure for storing a position for the bot using asserv units.
+ */
+typedef struct asserv_position_t
+{
+ /** X position. */
+ uint32_t x;
+ /** Y position. */
+ uint32_t y;
+ /** Angle. */
+ uint16_t a;
+} asserv_position_t;
+
+/**
+ * Status structure maintains by the update command.
+ */
+typedef struct asserv_struct_s
+{
+ /** Status flags. */
+ uint8_t status;
+ /** Asserv board input port. */
+ uint8_t input_port;
+ /** Bot position. */
+ asserv_position_t position;
+ /** Motor0 position. */
+ uint16_t motor0_position;
+ /** Motor1 position. */
+ uint16_t motor1_position;
+} asserv_struct_s;
+
+/**
+ * Status variable.
+ */
+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)
+{
+ asserv_set_scale (BOT_SCALE * (1L << 24));
+}
+
+void
+asserv_status_cb (uint8_t *status)
+{
+ /* Parse received data and store them. */
+ asserv_status.status = status[0];
+ asserv_status.input_port = status[1];
+ asserv_status.position.x = v8_to_v32 (0, status[3], status[4], status[5]);
+ asserv_status.position.y = v8_to_v32 (0, status[6], status[7], status[8]);
+ asserv_status.position.a = v8_to_v16 (status[9], status[10]);
+ asserv_status.motor0_position = v8_to_v16 (status[11], status[12]);
+ asserv_status.motor1_position = v8_to_v16 (status[13], status[14]);
+ /* Update moving direction. */
+ if (asserv_get_moving_direction () != 0)
+ asserv_last_moving_direction = asserv_get_moving_direction ();
+}
+
+asserv_status_e
+asserv_move_cmd_status (void)
+{
+ /* Check Motor Finished flag */
+ if (asserv_status.status & _BV (asserv_status_flag_move_succeed))
+ return success;
+ /* Check Motor Blocked flag */
+ else if (asserv_status.status & _BV (asserv_status_flag_move_failed))
+ return failure;
+ /* Otherwise, not finished nor failure */
+ return none;
+}
+
+asserv_status_e
+asserv_motor0_cmd_status (void)
+{
+ /* Check Motor0 Finished flag */
+ if (asserv_status.status & _BV (asserv_status_flag_motor0_succeed))
+ return success;
+ /* Check Motor0 Blocked flag */
+ else if (asserv_status.status & _BV (asserv_status_flag_motor0_failed))
+ return failure;
+ /* Otherwise, not finished nor failure */
+ return none;
+}
+
+asserv_status_e
+asserv_motor1_cmd_status (void)
+{
+ /* Check Motor1 Finished flag */
+ if (asserv_status.status & _BV (asserv_status_flag_motor1_succeed))
+ return success;
+ /* Check Motor1 Blocked flag */
+ else if (asserv_status.status & _BV (asserv_status_flag_motor1_failed))
+ return failure;
+ /* Otherwise, not finished nor failure */
+ return none;
+}
+
+void
+asserv_get_position (position_t *current_position)
+{
+ assert (current_position);
+ /* Copy last received status buffer information to current position */
+ current_position->v.x = fixed_mul_f824 (asserv_status.position.x,
+ asserv_scale);
+ current_position->v.y = fixed_mul_f824 (asserv_status.position.y,
+ asserv_scale);
+ current_position->a = asserv_status.position.a;
+}
+
+uint16_t
+asserv_get_motor0_position (void)
+{
+ /* Return the position of the motor0 of the current status buffer */
+ return asserv_status.motor0_position;
+}
+
+uint16_t
+asserv_get_motor1_position (void)
+{
+ /* Return the position of the motor1 of the current status buffer */
+ return asserv_status.motor1_position;
+}
+
+uint8_t
+asserv_get_moving_direction (void)
+{
+ /* Foward move? */
+ if (asserv_status.status & _BV (asserv_status_flag_move_forward))
+ return 1;
+ /* Backward move? */
+ if (asserv_status.status & _BV (asserv_status_flag_move_backward))
+ return 2;
+ /* Not moving */
+ return 0;
+}
+
+uint8_t
+asserv_get_last_moving_direction (void)
+{
+ return asserv_last_moving_direction;
+}
+
+void
+asserv_reset (void)
+{
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'z';
+ twi_master_send_buffer (1);
+}
+
+void
+asserv_free_motor (void)
+{
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'w';
+ twi_master_send_buffer (1);
+}
+
+void
+asserv_stop_motor (void)
+{
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 's';
+ twi_master_send_buffer (1);
+}
+
+void
+asserv_move_linearly (int32_t distance)
+{
+ distance = fixed_mul_f824 (distance, asserv_scale_inv);
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'l';
+ buffer[1] = v32_to_v8 (distance, 2);
+ buffer[2] = v32_to_v8 (distance, 1);
+ buffer[3] = v32_to_v8 (distance, 0);
+ twi_master_send_buffer (4);
+}
+
+void
+asserv_move_angularly (int16_t angle)
+{
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'a';
+ buffer[1] = v16_to_v8 (angle, 1);
+ buffer[2] = v16_to_v8 (angle, 0);
+ twi_master_send_buffer (3);
+}
+
+void
+asserv_goto_angle (int16_t angle)
+{
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'y';
+ buffer[1] = v16_to_v8 (angle, 1);
+ buffer[2] = v16_to_v8 (angle, 0);
+ twi_master_send_buffer (3);
+}
+
+void
+asserv_goto_xya (uint32_t x, uint32_t y, int16_t a, uint8_t backward)
+{
+ x = fixed_mul_f824 (x, asserv_scale_inv);
+ y = fixed_mul_f824 (y, asserv_scale_inv);
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'X';
+ buffer[1] = v32_to_v8 (x, 2);
+ buffer[2] = v32_to_v8 (x, 1);
+ buffer[3] = v32_to_v8 (x, 0);
+ buffer[4] = v32_to_v8 (y, 2);
+ buffer[5] = v32_to_v8 (y, 1);
+ buffer[6] = v32_to_v8 (y, 0);
+ buffer[7] = v16_to_v8 (a, 1);
+ buffer[8] = v16_to_v8 (a, 0);
+ buffer[9] = backward;
+ twi_master_send_buffer (10);
+}
+
+void
+asserv_go_to_the_wall (uint8_t backward)
+{
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'f';
+ buffer[1] = backward;
+ twi_master_send_buffer (2);
+}
+
+void
+asserv_move_motor0_absolute (uint16_t position, uint8_t speed)
+{
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'b';
+ buffer[1] = v16_to_v8 (position, 1);
+ buffer[2] = v16_to_v8 (position, 0);
+ buffer[3] = speed;
+ twi_master_send_buffer (4);
+}
+
+void
+asserv_move_motor1_absolute (uint16_t position, uint8_t speed)
+{
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'c';
+ buffer[1] = v16_to_v8 (position, 1);
+ buffer[2] = v16_to_v8 (position, 0);
+ buffer[3] = speed;
+ twi_master_send_buffer (4);
+}
+
+void
+asserv_set_x_position (int32_t x)
+{
+ x = fixed_mul_f824 (x, asserv_scale_inv);
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'p';
+ buffer[1] = 'X';
+ buffer[2] = v32_to_v8 (x, 2);
+ buffer[3] = v32_to_v8 (x, 1);
+ buffer[4] = v32_to_v8 (x, 0);
+ twi_master_send_buffer (5);
+}
+
+void
+asserv_set_y_position (int32_t y)
+{
+ y = fixed_mul_f824 (y, asserv_scale_inv);
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'p';
+ buffer[1] = 'Y';
+ buffer[2] = v32_to_v8 (y, 2);
+ buffer[3] = v32_to_v8 (y, 1);
+ buffer[4] = v32_to_v8 (y, 0);
+ twi_master_send_buffer (5);
+}
+
+void
+asserv_set_angle_position (int16_t angle)
+{
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'p';
+ buffer[1] = 'A';
+ buffer[2] = v32_to_v8 (angle, 1);
+ buffer[3] = v32_to_v8 (angle, 0);
+ twi_master_send_buffer (4);
+}
+
+void
+asserv_set_speed (uint8_t linear_high, uint8_t angular_high,
+ uint8_t linear_low, uint8_t angular_low)
+{
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'p';
+ buffer[1] = 's';
+ buffer[2] = linear_high;
+ buffer[3] = angular_high;
+ buffer[4] = linear_low;
+ buffer[5] = angular_low;
+ twi_master_send_buffer (6);
+}
+
+void
+asserv_set_position (int32_t x, int32_t y, int16_t angle)
+{
+ x = fixed_mul_f824 (x, asserv_scale_inv);
+ y = fixed_mul_f824 (y, asserv_scale_inv);
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'p';
+ buffer[1] = 'X';
+ buffer[2] = v32_to_v8 (x, 2);
+ buffer[3] = v32_to_v8 (x, 1);
+ buffer[4] = v32_to_v8 (x, 0);
+ buffer[5] = 'Y';
+ buffer[6] = v32_to_v8 (y, 2);
+ buffer[7] = v32_to_v8 (y, 1);
+ buffer[8] = v32_to_v8 (y, 0);
+ buffer[9] = 'A';
+ buffer[10] = v32_to_v8 (angle, 1);
+ buffer[11] = v32_to_v8 (angle, 0);
+ twi_master_send_buffer (12);
+}
+
+void
+asserv_goto (uint32_t x, uint32_t y, uint8_t backward)
+{
+ x = fixed_mul_f824 (x, asserv_scale_inv);
+ y = fixed_mul_f824 (y, asserv_scale_inv);
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'x';
+ buffer[1] = v32_to_v8 (x, 2);
+ buffer[2] = v32_to_v8 (x, 1);
+ buffer[3] = v32_to_v8 (x, 0);
+ buffer[4] = v32_to_v8 (y, 2);
+ buffer[5] = v32_to_v8 (y, 1);
+ buffer[6] = v32_to_v8 (y, 0);
+ buffer[7] = backward;
+ twi_master_send_buffer (8);
+}
+
+void
+asserv_motor0_zero_position (int8_t speed)
+{
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'B';
+ buffer[1] = speed;
+ twi_master_send_buffer (2);
+}
+
+void
+asserv_motor1_zero_position (int8_t speed)
+{
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'C';
+ buffer[1] = speed;
+ twi_master_send_buffer (2);
+}
+
+void
+asserv_motor0_free (void)
+{
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'r';
+ buffer[1] = 0;
+ twi_master_send_buffer (2);
+}
+
+void
+asserv_motor1_free (void)
+{
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'r';
+ buffer[1] = 1;
+ twi_master_send_buffer (2);
+}
+
diff --git a/digital/ai/src/twi_master/asserv.h b/digital/ai/src/twi_master/asserv.h
new file mode 100644
index 00000000..0d29ba96
--- /dev/null
+++ b/digital/ai/src/twi_master/asserv.h
@@ -0,0 +1,297 @@
+#ifndef asserv_h
+#define asserv_h
+/* asserv.h */
+/* ai - Robot Artificial Intelligence. {{{
+ *
+ * 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 "defs.h"
+
+/**
+ * @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.
+ */
+
+/** Slave number in twi_master list. */
+#define ASSERV_SLAVE 0
+
+/** Asserv TWI address. */
+#define ASSERV_TWI_ADDRESS 4
+
+/** Length of the status buffer (not including CRC). */
+#define ASSERV_STATUS_LENGTH 15
+
+/** Use backward movements. */
+#define ASSERV_BACKWARD 1
+/** Authorise reverse the requested movement direction, may be or'ed with the
+ * previous define. */
+#define ASSERV_REVERT_OK 2
+
+/** Initialize the asserv control module. */
+void
+asserv_init (void);
+
+/** Called when a new status buffer is received, update the asserv
+ * information. */
+void
+asserv_status_cb (uint8_t *status);
+
+/**
+ * Status of a move or motor class command.
+ * It is return by status functions.
+ */
+typedef enum asserv_status_e
+{
+ /** No status is available. The command is not finished yet. */
+ none,
+ /** The command has succeed. */
+ success,
+ /** The command has failed. The bot or the motor is blocked */
+ failure
+} asserv_status_e;
+
+/**
+ * Is last move class command has successfully ended?
+ * This function is used to know the status of the last move command. It looks
+ * at the status register.
+ * @return the status of the last move class command.
+ */
+asserv_status_e
+asserv_move_cmd_status (void);
+
+/**
+ * Is last motor0 class command has successfully ended?
+ * This function is used to know the status of the last motor0 command. It
+ * looks at the status register.
+ * @return the status of the last move class command.
+ */
+asserv_status_e
+asserv_motor0_cmd_status (void);
+
+/**
+ * Is last motor1 class command has successfully ended?
+ * This function is used to know the status of the last motor1 command. It
+ * looks at the status register.
+ * @return the status of the last move class command.
+ */
+asserv_status_e
+asserv_motor1_cmd_status (void);
+
+/**
+ * Get the current position of the bot.
+ * @param current_position the current position to update.
+ */
+void
+asserv_get_position (position_t *current_position);
+
+/**
+ * Get the motor0 position.
+ * @return the position of the motor0 (in steps).
+ */
+uint16_t
+asserv_get_motor0_position (void);
+
+/**
+ * Get the motor1 position.
+ * @return the position of the motor1 (in steps).
+ */
+uint16_t
+asserv_get_motor1_position (void);
+
+/**
+ * Are we moving forward/backward?
+ * @return
+ * - 0 we are not moving;
+ * - 1 we are moving forward;
+ * - 2 we are moving backward.
+ */
+uint8_t
+asserv_get_moving_direction (void);
+
+/**
+ * Get the last moving direction of the bot.
+ * @return 1 is forward, 2 is backward.
+ */
+uint8_t
+asserv_get_last_moving_direction (void);
+
+/**
+ * Reset the asserv board.
+ * Other class command.
+ */
+void
+asserv_reset (void);
+
+/**
+ * Free the motors (stop controlling them).
+ * Other class command.
+ */
+void
+asserv_free_motor (void);
+
+/**
+ * Stop the motor (and the bot).
+ * Other class command.
+ */
+void
+asserv_stop_motor (void);
+
+/**
+ * Move linearly.
+ * Move class command.
+ * @param distance the distance to move (mm).
+ */
+void
+asserv_move_linearly (int32_t distance);
+
+/**
+ * Move angularly (turn).
+ * Move class command.
+ * @param angle the angle to turn.
+ */
+void
+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
+ */
+void
+asserv_goto_angle (int16_t angle);
+
+/**
+ * Go to an absolute position and then an absolute angle.
+ * @param x the absolute position on the X axis.
+ * @param y the absolute position on the Y axis.
+ * @param a the absolute angle.
+ * @param backward 0 no backward, ASSERV_BACKWARD backward compulsary,
+ * ASSERV_REVERT_OK backward allowed.
+ */
+void
+asserv_goto_xya (uint32_t x, uint32_t y, int16_t a, uint8_t backward);
+
+/**
+ * Go to the wall (moving backward).
+ * Move class command.
+ */
+void
+asserv_go_to_the_wall (uint8_t backward);
+
+/**
+ * Move the motor0.
+ * Motor0 class command.
+ * This function take the number of steps you want to move to. This is an
+ * absolute position.
+ * @param position desired goal position (in step).
+ * @param speed speed of the movement.
+ */
+void
+asserv_move_motor0_absolute (uint16_t position, uint8_t speed);
+
+/**
+ * Move the motor1.
+ * Motor1 class command.
+ * This function take the number of steps you want to move to. This is an
+ * absolute position.
+ * @param position desired goal position (in step).
+ * @param speed speed of the movement.
+ */
+void
+asserv_move_motor1_absolute (uint16_t position, uint8_t speed);
+
+/**
+ * Set current X position.
+ * Other class command.
+ * @param x X position.
+ */
+void
+asserv_set_x_position (int32_t x);
+
+/**
+ * Set current Y position.
+ * Other class command.
+ * @param y Y position.
+ */
+void
+asserv_set_y_position (int32_t y);
+
+/**
+ * Set current angular position.
+ * Other class command.
+ * @param angle angular position.
+ */
+void
+asserv_set_angle_position (int16_t angle);
+
+/**
+ * Set speeds of movements.
+ * Other class command.
+ * @param linear_high linear high speed
+ * @param angular_high angular high speed
+ * @param linear_low linear low speed
+ * @param angular_low angular low speed
+ */
+void
+asserv_set_speed (uint8_t linear_high, uint8_t angular_high,
+ uint8_t linear_low, uint8_t angular_low);
+
+/**
+ * Set the complete position of the bot.
+ * This is an helpful function preventing you from calling multiples other
+ * ones.
+ * It calls other class commands.
+ * @param x X position
+ * @param y Y position
+ * @param angle angular position
+ */
+void
+asserv_set_position (int32_t x, int32_t y, int16_t angle);
+
+/**
+ * Go to an absolute position in (X, Y).
+ * @param x the x position on the table.
+ * @param y the y position on the table.
+ * @param backward 0 no backward, ASSERV_BACKWARD backward compulsary,
+ * ASSERV_REVERT_OK backward allowed.
+ */
+void
+asserv_goto (uint32_t x, uint32_t y, uint8_t backward);
+
+/** Reset the motor0 to the zero position. */
+void
+asserv_motor0_zero_position (int8_t speed);
+
+/** Reset the motor1 to the zero position. */
+void
+asserv_motor1_zero_position (int8_t speed);
+
+/** Set PWM to zero for motor0. */
+void
+asserv_motor0_free (void);
+
+/** Set PWM to zero for motor1. */
+void
+asserv_motor1_free (void);
+
+#endif /* asserv_h */
diff --git a/digital/ai/src/twi_master/mimot.c b/digital/ai/src/twi_master/mimot.c
new file mode 100644
index 00000000..af6ff5f9
--- /dev/null
+++ b/digital/ai/src/twi_master/mimot.c
@@ -0,0 +1,183 @@
+/* mimot.c */
+/* ai - Robot Artificial Intelligence. {{{
+ *
+ * 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 "mimot.h"
+
+#include "twi_master.h"
+
+#include "modules/utils/byte.h"
+#include "io.h"
+
+/** Flag bit positions in mimot status byte. */
+enum mimot_status_flag_t
+{
+ /** Motor0 movement finished with success. */
+ mimot_status_flag_motor0_succeed = 0,
+ /** Motor0 movement finished with failure. */
+ mimot_status_flag_motor0_failed = 1,
+ /** Motor1 movement finished with success. */
+ mimot_status_flag_motor1_succeed = 2,
+ /** Motor1 movement finished with failure. */
+ mimot_status_flag_motor1_failed = 3,
+};
+
+/** Status structure. */
+struct mimot_status_t
+{
+ /** Status flags. */
+ uint8_t status;
+ /** Mimot input port. */
+ uint8_t input_port;
+ /** Motor0 position. */
+ uint16_t motor0_position;
+ /** Motor1 position. */
+ uint16_t motor1_position;
+};
+
+/** Current mimot status. */
+struct mimot_status_t mimot_status;
+
+void
+mimot_init (void)
+{
+ /* Nothing to do. */
+}
+
+void
+mimot_status_cb (uint8_t *status)
+{
+ /* Parse received data and store them. */
+ mimot_status.status = status[0];
+ mimot_status.input_port = status[1];
+ mimot_status.motor0_position = v8_to_v16 (status[3], status[4]);
+ mimot_status.motor1_position = v8_to_v16 (status[5], status[6]);
+}
+
+asserv_status_e
+mimot_motor0_cmd_status (void)
+{
+ if (mimot_status.status & _BV (mimot_status_flag_motor0_succeed))
+ return success;
+ else if (mimot_status.status & _BV (mimot_status_flag_motor0_failed))
+ return failure;
+ else
+ return none;
+}
+
+asserv_status_e
+mimot_motor1_cmd_status (void)
+{
+ if (mimot_status.status & _BV (mimot_status_flag_motor1_succeed))
+ return success;
+ else if (mimot_status.status & _BV (mimot_status_flag_motor1_failed))
+ return failure;
+ else
+ return none;
+}
+
+uint16_t
+mimot_get_motor0_position (void)
+{
+ return mimot_status.motor0_position;
+}
+
+uint16_t
+mimot_get_motor1_position (void)
+{
+ return mimot_status.motor1_position;
+}
+
+void
+mimot_reset (void)
+{
+ uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
+ buffer[0] = 'z';
+ twi_master_send_buffer (1);
+}
+
+void
+mimot_move_motor0_absolute (uint16_t position, uint8_t speed)
+{
+ uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
+ buffer[0] = 'b';
+ buffer[1] = v16_to_v8 (position, 1);
+ buffer[2] = v16_to_v8 (position, 0);
+ buffer[3] = speed;
+ twi_master_send_buffer (4);
+}
+
+void
+mimot_move_motor1_absolute (uint16_t position, uint8_t speed)
+{
+ uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
+ buffer[0] = 'c';
+ buffer[1] = v16_to_v8 (position, 1);
+ buffer[2] = v16_to_v8 (position, 0);
+ buffer[3] = speed;
+ twi_master_send_buffer (4);
+}
+
+void
+mimot_motor0_zero_position (int8_t speed)
+{
+ uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
+ buffer[0] = 'B';
+ buffer[1] = speed;
+ twi_master_send_buffer (2);
+}
+
+void
+mimot_motor1_zero_position (int8_t speed)
+{
+ uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
+ buffer[0] = 'C';
+ buffer[1] = speed;
+ twi_master_send_buffer (2);
+}
+
+void
+mimot_motor0_clamp (int8_t speed, int16_t pwm)
+{
+ uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
+ buffer[0] = 'l';
+ buffer[1] = 0;
+ buffer[2] = speed;
+ buffer[3] = v16_to_v8 (pwm, 1);
+ buffer[4] = v16_to_v8 (pwm, 0);
+ twi_master_send_buffer (5);
+}
+
+void
+mimot_motor1_clamp (int8_t speed, int16_t pwm)
+{
+ uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
+ buffer[0] = 'l';
+ buffer[1] = 1;
+ buffer[2] = speed;
+ buffer[3] = v16_to_v8 (pwm, 1);
+ buffer[4] = v16_to_v8 (pwm, 0);
+ twi_master_send_buffer (5);
+}
+
diff --git a/digital/ai/src/twi_master/mimot.h b/digital/ai/src/twi_master/mimot.h
new file mode 100644
index 00000000..60811c10
--- /dev/null
+++ b/digital/ai/src/twi_master/mimot.h
@@ -0,0 +1,95 @@
+#ifndef mimot_h
+#define mimot_h
+/* mimot.h */
+/* ai - Robot Artificial Intelligence. {{{
+ *
+ * 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 "asserv.h"
+
+/**
+ * Provide interface to mimot board using the TWI protocol.
+ */
+
+/** Slave number in the twi_master list. */
+#define MIMOT_SLAVE 1
+
+/** Mimot TWI address. */
+#define MIMOT_TWI_ADDRESS 6
+
+/** Length of status buffer (not including CRC). */
+#define MIMOT_STATUS_LENGTH 7
+
+/** Initialise module. */
+void
+mimot_init (void);
+
+/** Called when a new status buffer is received, update the mimot
+ * information. */
+void
+mimot_status_cb (uint8_t *status);
+
+/** Return motor0 last command status. */
+asserv_status_e
+mimot_motor0_cmd_status (void);
+
+/** Return motor1 last command status. */
+asserv_status_e
+mimot_motor1_cmd_status (void);
+
+/** Get motor0 position in steps. */
+uint16_t
+mimot_get_motor0_position (void);
+
+/** Get motor1 position in steps. */
+uint16_t
+mimot_get_motor1_position (void);
+
+/** Reset mimot board. */
+void
+mimot_reset (void);
+
+/** Move motor0 to absolute position in steps. */
+void
+mimot_move_motor0_absolute (uint16_t position, uint8_t speed);
+
+/** Move motor1 to absolute position in steps. */
+void
+mimot_move_motor1_absolute (uint16_t position, uint8_t speed);
+
+/** Reset motor0 to zero position. */
+void
+mimot_motor0_zero_position (int8_t speed);
+
+/** Reset motor1 to zero position. */
+void
+mimot_motor1_zero_position (int8_t speed);
+
+/** Clamp motor0. */
+void
+mimot_motor0_clamp (int8_t speed, int16_t pwm);
+
+/** Clamp motor1. */
+void
+mimot_motor1_clamp (int8_t speed, int16_t pwm);
+
+#endif /* mimot_h */
diff --git a/digital/ai/src/twi_master/twi_master.c b/digital/ai/src/twi_master/twi_master.c
new file mode 100644
index 00000000..90977fd3
--- /dev/null
+++ b/digital/ai/src/twi_master/twi_master.c
@@ -0,0 +1,219 @@
+/* twi_master.c */
+/* ai - Robot Artificial Intelligence. {{{
+ *
+ * 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 "twi_master.h"
+
+#include "asserv.h"
+#include "mimot.h"
+
+#include "modules/twi/twi.h"
+#include "modules/utils/utils.h"
+#include "modules/utils/crc.h"
+
+/** Interval between retransmissions. */
+#define TWI_MASTER_RETRANSMIT_INTERVAL 10
+
+/** Index of the slave sequence number. */
+#define TWI_MASTER_STATUS_SEQ_INDEX 2
+
+/** Maximum command payload size. */
+#define TWI_MASTER_COMMAND_PAYLOAD_MAX 13
+
+/** Maximum status payload size. */
+#define TWI_MASTER_STATUS_PAYLOAD_MAX 15
+
+/** Maximum number of pending commands. */
+#define TWI_MASTER_PENDING_MAX 16
+
+/** Position of next free command. */
+#define TWI_MASTER_PENDING_TAIL \
+ ((twi_master.pending_head + twi_master.pending_nb) \
+ % TWI_MASTER_PENDING_MAX)
+
+/** Pending command structure. */
+struct twi_master_command_t
+{
+ /** Addressed slave index. */
+ uint8_t slave;
+ /** Command payload with space for CRC. */
+ uint8_t command[TWI_MASTER_COMMAND_PAYLOAD_MAX + 1];
+ /** Command length not including CRC. */
+ uint8_t length;
+};
+
+/** Global context. */
+struct twi_master_t
+{
+ /** Retransmission counter, retransmit when it reach zero. It is also set
+ * to zero after an acknowledge so that the next pending command is
+ * sent. */
+ uint8_t retransmit_counter;
+ /** Index of next pending command. */
+ uint8_t pending_head;
+ /** Number of pending commands. */
+ uint8_t pending_nb;
+ /** Table of pending commands. The next command is at pending_head.
+ * Next commands are stored in a circular buffer way. */
+ struct twi_master_command_t pending[TWI_MASTER_PENDING_MAX];
+} twi_master;
+
+/** Callback called when a slave status has been read.
+ * - status: status buffer (without CRC). */
+typedef void (*twi_master_slave_status_cb) (uint8_t *status);
+
+/** Information on a slave. */
+struct twi_master_slave_t
+{
+ /** Slave address. */
+ uint8_t address;
+ /** Last command sequence number. */
+ uint8_t seq;
+ /** Size of the status buffer not including CRC. */
+ uint8_t status_length;
+ /** Status callback. */
+ twi_master_slave_status_cb status_cb;
+};
+
+/** Information on all slaves. */
+static struct twi_master_slave_t twi_master_slaves[] = {
+ { ASSERV_TWI_ADDRESS, 0, ASSERV_STATUS_LENGTH, asserv_status_cb },
+ { MIMOT_TWI_ADDRESS, 0, MIMOT_STATUS_LENGTH, mimot_status_cb },
+};
+
+/** Send first pending message if available. */
+static void
+twi_master_send_head (void)
+{
+ if (twi_master.pending_nb)
+ {
+ struct twi_master_command_t *c =
+ &twi_master.pending[twi_master.pending_head];
+ /* Send command. */
+ twi_master_send (twi_master_slaves[c->slave].address, c->command,
+ c->length + 1);
+ twi_master_wait ();
+ /* Reset retransmission counter. */
+ twi_master.retransmit_counter = TWI_MASTER_RETRANSMIT_INTERVAL;
+ }
+}
+
+/** Update slave status, return non zero on success. */
+static uint8_t
+twi_master_update_status (uint8_t slave, uint8_t init)
+{
+ uint8_t buffer[TWI_MASTER_STATUS_PAYLOAD_MAX + 1];
+ /* Read status. */
+ twi_master_recv (twi_master_slaves[slave].address, buffer,
+ twi_master_slaves[slave].status_length + 1);
+ uint8_t ret = twi_master_wait ();
+ if (ret != twi_master_slaves[slave].status_length + 1)
+ return 0;
+ uint8_t crc = crc_compute (buffer + 1,
+ twi_master_slaves[slave].status_length);
+ if (crc != buffer[0])
+ return 0;
+ if (init)
+ twi_master_slaves[slave].seq =
+ buffer[1 + TWI_MASTER_STATUS_SEQ_INDEX];
+ /* Call user callback. */
+ twi_master_slaves[slave].status_cb (buffer + 1);
+ /* Update pending command list. */
+ if (twi_master.pending_nb)
+ {
+ struct twi_master_command_t *c =
+ &twi_master.pending[twi_master.pending_head];
+ if (slave == c->slave
+ && buffer[1 + TWI_MASTER_STATUS_SEQ_INDEX] == c->command[1])
+ {
+ /* Successfully acknowledged. */
+ twi_master.pending_nb--;
+ twi_master.pending_head = (twi_master.pending_head + 1)
+ % TWI_MASTER_PENDING_MAX;
+ /* Trigger next transmission. */
+ twi_master.retransmit_counter = 0;
+ }
+ }
+ /* Success. */
+ return 1;
+}
+
+void
+twi_master_init (void)
+{
+ uint8_t i;
+ /* Initialise hardware. */
+ twi_init (AC_IO_TWI_ADDRESS);
+ /* Get first status and reset sequence number. */
+ for (i = 0; i < UTILS_COUNT (twi_master_slaves); i++)
+ while (!twi_master_update_status (i, 1))
+ ;
+}
+
+uint8_t
+twi_master_sync (void)
+{
+ uint8_t i;
+ /* Update all slaves status. */
+ for (i = 0; i < UTILS_COUNT (twi_master_slaves); i++)
+ twi_master_update_status (i, 0);
+ /* If command is to be retransmitted (or transmitted for the first time
+ * after a acknowledge). */
+ if (twi_master.retransmit_counter == 0)
+ twi_master_send_head ();
+ else
+ twi_master.retransmit_counter--;
+ /* Synchronised if no pending command. */
+ return twi_master.pending_nb == 0;
+}
+
+uint8_t *
+twi_master_get_buffer (uint8_t slave)
+{
+ assert (twi_master.pending_nb < TWI_MASTER_PENDING_MAX);
+ struct twi_master_command_t *c =
+ &twi_master.pending[TWI_MASTER_PENDING_TAIL];
+ /* Store slave. */
+ c->slave = slave;
+ /* Skip CRC and sequence number. */
+ return &c->command[2];
+}
+
+void
+twi_master_send_buffer (uint8_t length)
+{
+ assert (length != 0);
+ struct twi_master_command_t *c =
+ &twi_master.pending[TWI_MASTER_PENDING_TAIL];
+ /* Fill sequence number, compute CRC, store length. */
+ c->command[1] = ++twi_master_slaves[c->slave].seq;
+ c->command[0] = crc_compute (&c->command[1], length + 1);
+ c->length = length + 1;
+ /* Add to the list of pending command. */
+ twi_master.pending_nb++;
+ /* Early transmission. */
+ if (twi_master.pending_nb == 1)
+ twi_master_send_head ();
+}
+
diff --git a/digital/ai/src/twi_master/twi_master.h b/digital/ai/src/twi_master/twi_master.h
new file mode 100644
index 00000000..e75dbfab
--- /dev/null
+++ b/digital/ai/src/twi_master/twi_master.h
@@ -0,0 +1,68 @@
+#ifndef twi_master_h
+#define twi_master_h
+/* twi_master.h */
+/* ai - Robot Artificial Intelligence. {{{
+ *
+ * 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.
+ *
+ * }}} */
+
+/**
+ * Handle communication as a master to several slaves.
+ *
+ * The communication protocol is always based on:
+ * - a message sent to the slave with a command to execute,
+ * - a status read from the slave containing the current slave state.
+ *
+ * The first byte of all messages is a CRC of the following bytes, called the
+ * message payload.
+ *
+ * The first byte of payload sent to slave is the command sequence number. It
+ * is used by the master to know if its command has been handled. The last
+ * handled command sequence number is available in the slave status (third
+ * byte).
+ *
+ * As long as the slave last command sequence number is not equal to the last
+ * sent sequence number, the master can not send any other command. If the
+ * slave do not acknowledge the command after a time out, the command is sent
+ * again.
+ *
+ * Several commands can be stored by this module, it will defer their
+ * transmission until the first command is acknowledged.
+ */
+
+/** Initialise module. */
+void
+twi_master_init (void);
+
+/** Synchronise all slaves, return non zero if synchronised. */
+uint8_t
+twi_master_sync (void);
+
+/** Get a buffer to send a command to the corresponding slave. */
+uint8_t *
+twi_master_get_buffer (uint8_t slave);
+
+/** Send previously got buffer with the given length. */
+void
+twi_master_send_buffer (uint8_t length);
+
+#endif /* twi_master_h */
diff --git a/digital/ai/src/utils/chrono.c b/digital/ai/src/utils/chrono.c
new file mode 100644
index 00000000..a1212e19
--- /dev/null
+++ b/digital/ai/src/utils/chrono.c
@@ -0,0 +1,150 @@
+/* chrono.c */
+/* ai - Robot Artificial Intelligence. {{{
+ *
+ * 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 "common.h"
+
+#include "timer.h"
+#include "asserv.h"
+#include "twi_master.h"
+
+#include "modules/utils/utils.h"
+#include "modules/host/mex.h"
+
+#include "chrono.h"
+
+/**
+ * Implementation notes.
+ * This module compute the number of tic of the main loop it should count
+ * before the match is over (chrono_init). Every tic of the main loop, it
+ * decrements the counter (chrono_update). When the counter is zero, the
+ * match is over (chrono_is_match_over, chrono_end_match).
+ */
+
+/** 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.
+ */
+#define CHRONO_LOOP_DURATION_MS 4
+
+/**
+ * Time to wait before resetting slaves board, in ms.
+ */
+#define CHRONO_WAIT_BEFORE_RESET_MS 1000
+
+/**
+ * Number of time to overflow before the end of the match.
+ */
+static uint32_t chrono_ov_count_;
+
+/**
+ * Status of the chrono module.
+ * Set to 0 if the module is disabled, otherwise set to a non 0 value.
+ */
+static uint8_t chrono_enabled_ = 0;
+
+
+void
+chrono_init (void)
+{
+ /* Enable chrono. */
+ chrono_enable ();
+ /* Set the overflow counter to the maximum of overflow before the end of
+ * the match. */
+ chrono_ov_count_ = CHRONO_MATCH_OVERFLOW_COUNT;
+}
+
+void
+chrono_update (void)
+{
+ /* Decrement overflow counter if it is possible. */
+ if (chrono_enabled_ && chrono_ov_count_)
+ chrono_ov_count_--;
+}
+
+uint8_t
+chrono_is_match_over (void)
+{
+ if (!chrono_enabled_ || chrono_ov_count_)
+ return 0;
+ else
+ return 1;
+}
+
+void
+chrono_enable (void)
+{
+ chrono_enabled_ = 1;
+}
+
+void
+chrono_disable (void)
+{
+ chrono_enabled_ = 0;
+}
+
+uint8_t
+chrono_enabled (void)
+{
+ return chrono_enabled_;
+}
+
+uint32_t
+chrono_remaining_time (void)
+{
+ return chrono_ov_count_ * TIMER_PERIOD_MS;
+}
+
+void
+chrono_end_match (uint8_t block)
+{
+ /* Make sure previous command has been acknowledged. If not, retransmit
+ * until acknowledged */
+ while (!twi_master_sync ())
+ utils_delay_ms (CHRONO_LOOP_DURATION_MS);
+
+ /* Make the bot stop moving */
+ asserv_stop_motor ();
+
+ /* Wait until complete */
+ while (!twi_master_sync ())
+ utils_delay_ms (CHRONO_LOOP_DURATION_MS);
+
+ /* Wait CHRONO_WAIT_BEFORE_RESET ms before reseting */
+ utils_delay_ms (CHRONO_WAIT_BEFORE_RESET_MS);
+ /* Reset the asserv board */
+ asserv_reset ();
+ /* Block indefinitely */
+ if (block)
+ while (42)
+#ifdef HOST
+ {
+ mex_node_wait ();
+ }
+#else
+ ;
+#endif
+}
diff --git a/digital/ai/src/utils/chrono.h b/digital/ai/src/utils/chrono.h
new file mode 100644
index 00000000..38bf3d61
--- /dev/null
+++ b/digital/ai/src/utils/chrono.h
@@ -0,0 +1,99 @@
+#ifndef chrono_h
+#define chrono_h
+/* chrono.h */
+/* ai - Robot Artificial Intelligence. {{{
+ *
+ * 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.
+ *
+ * }}} */
+
+/**
+ * @file Module to manage the chrono responsible to stop the bot after 90s.
+ *
+ * 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 CHRONO_MATCH_DURATION_MS.
+ */
+void
+chrono_init (void);
+
+/**
+ * Update chrono module.
+ * You must call this function every overflow of the main timer.
+ */
+void
+chrono_update (void);
+
+/**
+ * Enable chrono module.
+ * You should call this function when a match start.
+ */
+void
+chrono_enable (void);
+
+/**
+ * Disable chrono module.
+ */
+void
+chrono_disable (void);
+
+/**
+ * Is chrono module enabled?
+ * @return 0 if not enabled, other values otherwise.
+ */
+uint8_t
+chrono_enabled (void);
+
+/**
+ * Match over?
+ * @return
+ * - 0 if the match is not finished yet.
+ * - 1 if the match is over.
+ */
+uint8_t
+chrono_is_match_over (void);
+
+/**
+ * How much time remains before the end of the match.
+ * @return remaining time in ms.
+ */
+uint32_t
+chrono_remaining_time (void);
+
+/**
+ * End the match.
+ * This function is responsible of resetting the asserv board to stop the bot
+ * from moving and put the io board in a state where it will not do something.
+ * @param block blocking function until hardware reset?
+ */
+void
+chrono_end_match (uint8_t block);
+
+#endif /* chrono_h */
diff --git a/digital/ai/src/utils/timer.avr.c b/digital/ai/src/utils/timer.avr.c
new file mode 100644
index 00000000..63aa7bdc
--- /dev/null
+++ b/digital/ai/src/utils/timer.avr.c
@@ -0,0 +1,68 @@
+/* timer.avr.c */
+/* ai - Robot Artificial Intelligence. {{{
+ *
+ * Copyright (C) 2009 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 "common.h"
+
+#include "timer.h"
+
+#include "modules/utils/utils.h"
+#include "io.h"
+
+void
+timer_init (void)
+{
+ /* Configuration of the timer/counter 0:
+ * - top = 0xff,
+ * - prescaler = 256,
+ * -> Fov = F_io / (prescaler * (TOP + 1))
+ * -> Tov = 1 / Fov
+ * Note: if you change the TCCR0 register value, please also update
+ * 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
+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_reg & _BV (TOV0)))
+ /* We have not reached overflow. */
+ count_before_ov = 0;
+ /* Write 1 to clear overflow. */
+ TIFR_reg = _BV (TOV0);
+ return count_before_ov;
+}
+
diff --git a/digital/ai/src/utils/timer.h b/digital/ai/src/utils/timer.h
new file mode 100644
index 00000000..e085b86e
--- /dev/null
+++ b/digital/ai/src/utils/timer.h
@@ -0,0 +1,68 @@
+#ifndef timer_h
+#define timer_h
+/* timer.h */
+/* ai - Robot Artificial Intelligence. {{{
+ *
+ * Copyright (C) 2009 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.
+ *
+ * }}} */
+
+/**
+ * Tick timer, used to slow down the main loop to a given rate.
+ */
+
+/**
+ * Prescaler configured for timer/counter 0.
+ * If you want to change this value, you also need to change the TCCR0
+ * register.
+ */
+#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 TIMER_TC0_TOP 255
+
+/** Period of timer, ms. */
+#define TIMER_PERIOD_MS \
+ (1000.0 / (AC_FREQ / (TIMER_TC0_PRESCALER * (TIMER_TC0_TOP + 1))))
+
+/** Initialise timer. */
+void
+timer_init (void);
+
+/**
+ * Wait until the timer overflows.
+ * @return
+ * - 0 if we are on time (we have not reached overflow before calling this
+ * function).
+ * - 1 if we have already reached overflow.
+ * @warning if this function return 1, it means we are late and the main loop
+ * is lasting more than the time configured. Consequence, some important
+ * functions (like the chronometer for match duration) will not work
+ * correctly!
+ */
+uint8_t
+timer_wait (void);
+
+#endif /* timer_h */