summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2010-04-01 23:57:40 +0200
committerNicolas Schodet2010-04-01 23:57:40 +0200
commitcb9ddc2d8162ac2ec0d82579657b984335ec9aef (patch)
tree952c6193ffb4b880f71990628a830825cd9d4f6e /digital
parent80b3da91dac28a5895a8cb9f0477a7008365dd56 (diff)
digital/io/src: use one FSM with several active states, closes #83
Diffstat (limited to 'digital')
-rw-r--r--digital/avr/make/Makefile.fsm42
-rw-r--r--digital/io/src/Makefile5
-rw-r--r--digital/io/src/ai.conf10
-rw-r--r--digital/io/src/ai_init_cb.c (renamed from digital/io/src/init_cb.c)136
-rw-r--r--digital/io/src/ai_move_cb.c (renamed from digital/io/src/move_cb.c)101
-rw-r--r--digital/io/src/ai_top_cb.c (renamed from digital/io/src/top_cb.c)12
-rw-r--r--digital/io/src/fsm.c92
-rw-r--r--digital/io/src/fsm.conf8
-rw-r--r--digital/io/src/fsm.h50
-rw-r--r--digital/io/src/fsm_templates/template_fsm.c9
-rw-r--r--digital/io/src/fsm_templates/template_init_cb_skel.c11
-rw-r--r--digital/io/src/fsm_templates/template_move_cb_skel.c11
-rw-r--r--digital/io/src/fsm_templates/template_top_cb_skel.c (renamed from digital/io/src/fsm_templates/template_cb_skel.c)4
-rw-r--r--digital/io/src/init.fsm106
-rw-r--r--digital/io/src/main.c106
-rw-r--r--digital/io/src/main.h16
-rw-r--r--digital/io/src/move.c15
-rw-r--r--digital/io/src/move.fsm46
-rw-r--r--digital/io/src/move.h6
-rw-r--r--digital/io/src/top.fsm7
-rw-r--r--digital/io/src/trace.trc3
21 files changed, 371 insertions, 425 deletions
diff --git a/digital/avr/make/Makefile.fsm b/digital/avr/make/Makefile.fsm
index 4b8c9a79..a573b065 100644
--- a/digital/avr/make/Makefile.fsm
+++ b/digital/avr/make/Makefile.fsm
@@ -1,9 +1,10 @@
# Makefile.fsm - Makefile to build FSM.
# You need to define:
-# - FSM_SOURCES with the name of each of your FSM. For example, if you have
-# a example.fsm and check.fsm, you need to set FSM_SOURCES to "example
-# check".
+# - FSM with the FSM prefix name
+# - FSM_SOURCES with the name of each of your FSM files. For example, if
+# you have a example.fsm and check.fsm, you need to set FSM_SOURCES to
+# "example check". The first one is the one including the others.
# FSM build:
#
@@ -23,26 +24,25 @@
# available.
# If templates are modified, files should be rebuilt.
-FSM_TEMPLATES := template_cb.h template_cb_decl.h template_cb_impl.c \
- template_cb_skel.c template_fsm.c template_fsm.h
+FSM_TEMPLATES := template_fsm.h template_fsm.c template_cb.h \
+ template_cb_decl.h template_cb_impl.c \
+ $(FSM_SOURCES:%=template_%_cb_skel.c)
FSM_TEMPLATES := $(FSM_TEMPLATES:%=fsm_templates/%)
# Theses files are auto-generated, remove them on clean.
-EXTRA_CLEAN_FILES += $(FSM_SOURCES:%=%_fsm.h)
-EXTRA_CLEAN_FILES += $(FSM_SOURCES:%=%_fsm.c)
-EXTRA_CLEAN_FILES += $(FSM_SOURCES:%=%_cb.h)
-EXTRA_CLEAN_FILES += $(FSM_SOURCES:%=%_cb_skel.c)
-EXTRA_CLEAN_FILES += $(FSM_SOURCES:%=%.png)
+FSM_FILES = $(FSM)_fsm.h $(FSM)_fsm.c $(FSM)_cb.h \
+ $(FSM_SOURCES:%=$(FSM)_%_cb_skel.c)
+EXTRA_CLEAN_FILES += $(FSM_FILES) $(FSM).png
DFAGEN_DIR := $(BASE)/../../tools/dfagen
DFAGEN_DEPS := $(DFAGEN_DIR)/dfagen.py
DFAGEN := python $(DFAGEN_DIR)/dfagen.py
# Add source to build.
-$(PROGS)_SOURCES += $(FSM_SOURCES:%=%.c)
-$(PROGS)_SOURCES += $(FSM_SOURCES:%=%_fsm.c)
-$(PROGS)_SOURCES += $(FSM_SOURCES:%=%_cb.c)
-$(PROGS)_SOURCES += fsm.c
+$(PROGS)_SOURCES += $(FSM)_fsm.c \
+ $(FSM_SOURCES:%=$(FSM)_%_cb.c) \
+ $(FSM_SOURCES:%=%.c) \
+ fsm.c
# Include general Makefile for first rule to be default.
include $(BASE)/make/Makefile.gen
@@ -50,17 +50,17 @@ include $(BASE)/make/Makefile.gen
# Bootstrap, fsm should be generated before fsm.h is used.
fsm.c: fsm.h
main.c: fsm.h
-fsm.h: $(FSM_SOURCES:%=%_fsm.h)
+fsm.h: $(FSM)_fsm.h
$(DFAGEN_DIR)/dfagen.py: $(DFAGEN_DIR)/dfagen/parser.py
$(DFAGEN_DIR)/dfagen/parser.py: $(DFAGEN_DIR)/dfagen/parser.g
yapps $<
-%_fsm.h %_fsm.c %_cb.h %_cb_skel.c: %.fsm fsm.conf $(FSM_TEMPLATES) \
+$(FSM_FILES): $(FSM_SOURCES:%=%.fsm) $(FSM).conf $(FSM_TEMPLATES) \
$(DFAGEN_DEPS)
- $(DFAGEN) -o c -d $< -c fsm.conf -p $(<:%.fsm=%)
+ $(DFAGEN) -o c -d $< -c $(FSM).conf -p $(FSM)
-.PRECIOUS: $(FSM_SOURCES:%=%_cb.c)
+.PRECIOUS: $(FSM_SOURCES:%=$(FSM)_%_cb.c)
ifndef CB
%_cb.c: %_cb_skel.c
@@ -91,10 +91,10 @@ else
endif
-png: $(FSM_SOURCES:%=%.png)
+png: $(FSM).png
%.png: %.dot
dot -Tpng -o $@ $<
-%.dot: %.fsm $(DFAGEN_DEPS)
- $(DFAGEN) -o dot -d $< -p $(<:%.fsm=%)
+%.dot: $(FSM_SOURCES:%=%.fsm) $(DFAGEN_DEPS)
+ $(DFAGEN) -o dot -d $< -p $(FSM)
diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile
index 0ec3d6ce..9bd93afd 100644
--- a/digital/io/src/Makefile
+++ b/digital/io/src/Makefile
@@ -16,8 +16,9 @@ AVR_MCU = atmega128
OPTIMIZE = -O2
HOST_LIBS = -lm
-# FSMs.
-FSM_SOURCES := top move init
+# FSM.
+FSM = ai
+FSM_SOURCES := top init move
# Include FSM makefile.
include $(BASE)/make/Makefile.fsm
diff --git a/digital/io/src/ai.conf b/digital/io/src/ai.conf
new file mode 100644
index 00000000..53429b68
--- /dev/null
+++ b/digital/io/src/ai.conf
@@ -0,0 +1,10 @@
+[user]
+template-dir = fsm_templates
+
+[templates]
+template_fsm.h = %_fsm.h
+template_fsm.c = %_fsm.c
+template_cb.h = %_cb.h
+template_top_cb_skel.c = %_top_cb_skel.c
+template_init_cb_skel.c = %_init_cb_skel.c
+template_move_cb_skel.c = %_move_cb_skel.c
diff --git a/digital/io/src/init_cb.c b/digital/io/src/ai_init_cb.c
index 5668d37c..a1d61967 100644
--- a/digital/io/src/init_cb.c
+++ b/digital/io/src/ai_init_cb.c
@@ -21,10 +21,13 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
- * }}} */
+ * }}}
+ * init:
+ * place the bot in the start position for a match.
+ */
#include "common.h"
#include "fsm.h"
-#include "init_cb.h"
+#include "ai_cb.h"
#include "asserv.h"
#include "init.h"
#include "playground.h"
@@ -37,175 +40,175 @@
#include "modules/path/path.h"
/*
- * IDLE =start=>
- * => WAIT_FIRST_JACK_IN
+ * INIT_IDLE =start=>
+ * => INIT_WAIT_FIRST_JACK_IN
* do nothing.
*/
fsm_branch_t
-init__IDLE__start (void)
+ai__INIT_IDLE__start (void)
{
- return init_next (IDLE, start);
+ return ai_next (INIT_IDLE, start);
}
/*
- * WAIT_FIRST_JACK_IN =jack_inserted_into_bot=>
- * => WAIT_FIRST_JACK_OUT
+ * INIT_WAIT_FIRST_JACK_IN =jack_inserted_into_bot=>
+ * => INIT_WAIT_FIRST_JACK_OUT
* do nothing.
*/
fsm_branch_t
-init__WAIT_FIRST_JACK_IN__jack_inserted_into_bot (void)
+ai__INIT_WAIT_FIRST_JACK_IN__jack_inserted_into_bot (void)
{
- return init_next (WAIT_FIRST_JACK_IN, jack_inserted_into_bot);
+ return ai_next (INIT_WAIT_FIRST_JACK_IN, jack_inserted_into_bot);
}
/*
- * WAIT_FIRST_JACK_OUT =jack_removed_from_bot=>
- * => WAIT_SECOND_JACK_IN
+ * INIT_WAIT_FIRST_JACK_OUT =jack_removed_from_bot=>
+ * => INIT_WAIT_SECOND_JACK_IN
* start trace module.
* get and store the color of the bot.
*/
fsm_branch_t
-init__WAIT_FIRST_JACK_OUT__jack_removed_from_bot (void)
+ai__INIT_WAIT_FIRST_JACK_OUT__jack_removed_from_bot (void)
{
/* Initialize trace module (erase the flash). */
trace_init ();
/* Get the color. */
bot_color = switch_get_color ();
- return init_next (WAIT_FIRST_JACK_OUT, jack_removed_from_bot);
+ return ai_next (INIT_WAIT_FIRST_JACK_OUT, jack_removed_from_bot);
}
/*
- * WAIT_SECOND_JACK_IN =jack_inserted_into_bot=>
- * => WAIT_FOR_HANDS_OUT
+ * INIT_WAIT_SECOND_JACK_IN =jack_inserted_into_bot=>
+ * => INIT_WAIT_FOR_HANDS_OUT
* do nothing.
*/
fsm_branch_t
-init__WAIT_SECOND_JACK_IN__jack_inserted_into_bot (void)
+ai__INIT_WAIT_SECOND_JACK_IN__jack_inserted_into_bot (void)
{
- return init_next (WAIT_SECOND_JACK_IN, jack_inserted_into_bot);
+ return ai_next (INIT_WAIT_SECOND_JACK_IN, jack_inserted_into_bot);
}
/*
- * WAIT_FOR_HANDS_OUT =state_timeout=>
- * => GO_TO_THE_WALL
+ * INIT_WAIT_FOR_HANDS_OUT =state_timeout=>
+ * => INIT_GO_TO_THE_WALL
* fuck the wall in front.
*/
fsm_branch_t
-init__WAIT_FOR_HANDS_OUT__state_timeout (void)
+ai__INIT_WAIT_FOR_HANDS_OUT__state_timeout (void)
{
/* Go to the wall, no backward. */
asserv_go_to_the_wall (0);
- return init_next (WAIT_FOR_HANDS_OUT, state_timeout);
+ return ai_next (INIT_WAIT_FOR_HANDS_OUT, state_timeout);
}
/*
- * GO_TO_THE_WALL =bot_move_succeed=>
- * => SET_Y_POSITION
+ * INIT_GO_TO_THE_WALL =bot_move_succeed=>
+ * => INIT_SET_Y_POSITION
* reset the Y position of the bot.
*/
fsm_branch_t
-init__GO_TO_THE_WALL__bot_move_succeed (void)
+ai__INIT_GO_TO_THE_WALL__bot_move_succeed (void)
{
/* We are against the border of absys Y set to PG_LENGTH. */
asserv_set_y_position (PG_LENGTH - (BOT_LENGTH / 2));
- return init_next (GO_TO_THE_WALL, bot_move_succeed);
+ return ai_next (INIT_GO_TO_THE_WALL, bot_move_succeed);
}
/*
- * SET_Y_POSITION =asserv_last_cmd_ack=>
- * => SET_ANGULAR_POSITION
+ * INIT_SET_Y_POSITION =asserv_last_cmd_ack=>
+ * => INIT_SET_ANGULAR_POSITION
* reset the angular position of the bot.
*/
fsm_branch_t
-init__SET_Y_POSITION__asserv_last_cmd_ack (void)
+ai__INIT_SET_Y_POSITION__asserv_last_cmd_ack (void)
{
/* We are facing top border. */
asserv_set_angle_position (90 * BOT_ANGLE_DEGREE);
- return init_next (SET_Y_POSITION, asserv_last_cmd_ack);
+ return ai_next (INIT_SET_Y_POSITION, asserv_last_cmd_ack);
}
/*
- * SET_ANGULAR_POSITION =asserv_last_cmd_ack=>
- * => GO_AWAY_FROM_THE_WALL
+ * INIT_SET_ANGULAR_POSITION =asserv_last_cmd_ack=>
+ * => INIT_GO_AWAY_FROM_THE_WALL
* move away from the wall (linear move).
*/
fsm_branch_t
-init__SET_ANGULAR_POSITION__asserv_last_cmd_ack (void)
+ai__INIT_SET_ANGULAR_POSITION__asserv_last_cmd_ack (void)
{
/* Move away from the border. */
asserv_move_linearly (- INIT_DIST);
- return init_next (SET_ANGULAR_POSITION, asserv_last_cmd_ack);
+ return ai_next (INIT_SET_ANGULAR_POSITION, asserv_last_cmd_ack);
}
/*
- * GO_AWAY_FROM_THE_WALL =bot_move_succeed=>
- * => FACE_OTHER_WALL
+ * INIT_GO_AWAY_FROM_THE_WALL =bot_move_succeed=>
+ * => INIT_FACE_OTHER_WALL
* turn to face the other wall.
*/
fsm_branch_t
-init__GO_AWAY_FROM_THE_WALL__bot_move_succeed (void)
+ai__INIT_GO_AWAY_FROM_THE_WALL__bot_move_succeed (void)
{
/* Face the other wall. */
asserv_goto_angle (PG_A_VALUE_COMPUTING (180 * BOT_ANGLE_DEGREE));
- return init_next (GO_AWAY_FROM_THE_WALL, bot_move_succeed);
+ return ai_next (INIT_GO_AWAY_FROM_THE_WALL, bot_move_succeed);
}
/*
- * FACE_OTHER_WALL =bot_move_succeed=>
- * => WAIT_AFTER_ROTATION
+ * INIT_FACE_OTHER_WALL =bot_move_succeed=>
+ * => INIT_WAIT_AFTER_ROTATION
* nothing to do.
*/
fsm_branch_t
-init__FACE_OTHER_WALL__bot_move_succeed (void)
+ai__INIT_FACE_OTHER_WALL__bot_move_succeed (void)
{
- return init_next (FACE_OTHER_WALL, bot_move_succeed);
+ return ai_next (INIT_FACE_OTHER_WALL, bot_move_succeed);
}
/*
- * WAIT_AFTER_ROTATION =state_timeout=>
- * => GO_TO_THE_WALL_AGAIN
+ * INIT_WAIT_AFTER_ROTATION =state_timeout=>
+ * => INIT_GO_TO_THE_WALL_AGAIN
* fuck the wall in front.
*/
fsm_branch_t
-init__WAIT_AFTER_ROTATION__state_timeout (void)
+ai__INIT_WAIT_AFTER_ROTATION__state_timeout (void)
{
/* Go to the wall, no backward. */
asserv_go_to_the_wall (0);
- return init_next (WAIT_AFTER_ROTATION, state_timeout);
+ return ai_next (INIT_WAIT_AFTER_ROTATION, state_timeout);
}
/*
- * GO_TO_THE_WALL_AGAIN =bot_move_succeed=>
- * => SET_X_POSITION
+ * INIT_GO_TO_THE_WALL_AGAIN =bot_move_succeed=>
+ * => INIT_SET_X_POSITION
* reset the X position of the bot.
*/
fsm_branch_t
-init__GO_TO_THE_WALL_AGAIN__bot_move_succeed (void)
+ai__INIT_GO_TO_THE_WALL_AGAIN__bot_move_succeed (void)
{
asserv_set_x_position (PG_X_VALUE_COMPUTING (BOT_LENGTH / 2));
- return init_next (GO_TO_THE_WALL_AGAIN, bot_move_succeed);
+ return ai_next (INIT_GO_TO_THE_WALL_AGAIN, bot_move_succeed);
}
/*
- * SET_X_POSITION =asserv_last_cmd_ack=>
- * => GO_AWAY_FROM_THE_WALL_AGAIN
+ * INIT_SET_X_POSITION =asserv_last_cmd_ack=>
+ * => INIT_GO_AWAY_FROM_THE_WALL_AGAIN
* move away from the wall (linear move).
*/
fsm_branch_t
-init__SET_X_POSITION__asserv_last_cmd_ack (void)
+ai__INIT_SET_X_POSITION__asserv_last_cmd_ack (void)
{
/* Move away from the border. */
asserv_move_linearly (- INIT_DIST);
- return init_next (SET_X_POSITION, asserv_last_cmd_ack);
+ return ai_next (INIT_SET_X_POSITION, asserv_last_cmd_ack);
}
/*
- * GO_AWAY_FROM_THE_WALL_AGAIN =bot_move_succeed=>
- * => GO_TO_START_POSITION
+ * INIT_GO_AWAY_FROM_THE_WALL_AGAIN =bot_move_succeed=>
+ * => INIT_GO_TO_START_POSITION
* go to the start position with a go to movement.
*/
fsm_branch_t
-init__GO_AWAY_FROM_THE_WALL_AGAIN__bot_move_succeed (void)
+ai__INIT_GO_AWAY_FROM_THE_WALL_AGAIN__bot_move_succeed (void)
{
/* Move away from the border. */
asserv_goto_xya (PG_X_VALUE_COMPUTING (PG_START_ZONE_WIDTH
@@ -213,33 +216,34 @@ init__GO_AWAY_FROM_THE_WALL_AGAIN__bot_move_succeed (void)
- 50),
PG_LENGTH - PG_START_ZONE_LENGTH + BOT_LENGTH / 2 + 50,
PG_A_VALUE_COMPUTING (0 * BOT_ANGLE_DEGREE), 0);
- return init_next (GO_AWAY_FROM_THE_WALL_AGAIN, bot_move_succeed);
+ return ai_next (INIT_GO_AWAY_FROM_THE_WALL_AGAIN, bot_move_succeed);
}
/*
- * GO_TO_START_POSITION =bot_move_succeed=>
- * => WAIT_SECOND_JACK_OUT
+ * INIT_GO_TO_START_POSITION =bot_move_succeed=>
+ * => INIT_WAIT_SECOND_JACK_OUT
* nothing to do, the bot is at the start position.
*/
fsm_branch_t
-init__GO_TO_START_POSITION__bot_move_succeed (void)
+ai__INIT_GO_TO_START_POSITION__bot_move_succeed (void)
{
- return init_next (GO_TO_START_POSITION, bot_move_succeed);
+ return ai_next (INIT_GO_TO_START_POSITION, bot_move_succeed);
}
/*
- * WAIT_SECOND_JACK_OUT =jack_removed_from_bot=>
- * => IDLE
+ * INIT_WAIT_SECOND_JACK_OUT =jack_removed_from_bot=>
+ * => INIT_IDLE
* tell other FSM the match begins.
+ * start the chrono.
*/
fsm_branch_t
-init__WAIT_SECOND_JACK_OUT__jack_removed_from_bot (void)
+ai__INIT_WAIT_SECOND_JACK_OUT__jack_removed_from_bot (void)
{
/* Set the flag to transmit to other FSM. */
init_match_is_started = 1;
/* Start the chrono. */
chrono_init ();
- return init_next (WAIT_SECOND_JACK_OUT, jack_removed_from_bot);
+ return ai_next (INIT_WAIT_SECOND_JACK_OUT, jack_removed_from_bot);
}
diff --git a/digital/io/src/move_cb.c b/digital/io/src/ai_move_cb.c
index 900842b9..f130f139 100644
--- a/digital/io/src/move_cb.c
+++ b/digital/io/src/ai_move_cb.c
@@ -21,10 +21,13 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
- * }}} */
+ * }}}
+ * move:
+ * move with avoid obstacle.
+ */
#include "common.h"
#include "fsm.h"
-#include "move_cb.h"
+#include "ai_cb.h"
#include "asserv.h"
#include "playground.h"
@@ -228,142 +231,142 @@ move_obstacle_here (void)
}
/*
- * IDLE =start=>
- * => MOVING
+ * MOVE_IDLE =move_start=>
+ * => MOVE_MOVING
* ask the asserv to go to the computed position.
*/
fsm_branch_t
-move__IDLE__start (void)
+ai__MOVE_IDLE__move_start (void)
{
/* ask the asserv to go to the computed position. */
move_get_next_position (&move_data.intermediate);
- return move_next (IDLE, start);
+ return ai_next (MOVE_IDLE, move_start);
}
/*
- * MOVING =bot_move_succeed=>
- * we_are_at_final_position => IDLE
+ * MOVE_MOVING =bot_move_succeed=>
+ * we_are_at_final_position => MOVE_IDLE
* post an event for the top FSM to tell it we have finished.
- * position_intermediary => MOVING
+ * position_intermediary => MOVE_MOVING
* get next position computed by the path module.
* if next position is the final, use a goto_xya.
* otherwise go to the next intermediate position with goto.
- * no_intermediate_path_found => IDLE
+ * no_intermediate_path_found => MOVE_IDLE
* post an event for the top FSM to generate a failure.
*/
fsm_branch_t
-move__MOVING__bot_move_succeed (void)
+ai__MOVE_MOVING__bot_move_succeed (void)
{
uint8_t ret = move_get_next_position (&move_data.intermediate);
if (ret == 2)
{
/* Post an event for the top FSM to tell it we have finished. */
- main_post_event_for_top_fsm = TOP_EVENT_move_fsm_succeed;
- return move_next_branch (MOVING, bot_move_succeed, we_are_at_final_position);
+ main_post_event_for_top_fsm = AI_EVENT_move_fsm_succeed;
+ return ai_next_branch (MOVE_MOVING, bot_move_succeed, we_are_at_final_position);
}
else if (ret == 1)
{
/* Nothing to do. */
- return move_next_branch (MOVING, bot_move_succeed, position_intermediary);
+ return ai_next_branch (MOVE_MOVING, bot_move_succeed, position_intermediary);
}
else
{
/* Post an event for the top FSM to generate a failure. */
- main_post_event_for_top_fsm = TOP_EVENT_move_fsm_failed;
- return move_next_branch (MOVING, bot_move_succeed, no_intermediate_path_found);
+ main_post_event_for_top_fsm = AI_EVENT_move_fsm_failed;
+ return ai_next_branch (MOVE_MOVING, bot_move_succeed, no_intermediate_path_found);
}
}
/*
- * MOVING =bot_move_failed=>
- * => MOVING_BACKWARD_TO_TURN_FREELY
+ * MOVE_MOVING =bot_move_failed=>
+ * => MOVE_MOVING_BACKWARD_TO_TURN_FREELY
* compute the obstacle position.
* move backward to turn freely.
*/
fsm_branch_t
-move__MOVING__bot_move_failed (void)
+ai__MOVE_MOVING__bot_move_failed (void)
{
/* Compute the obstacle position. */
move_obstacle_here ();
/* Move backward to turn freely. */
asserv_move_linearly (asserv_get_last_moving_direction () == 1 ?
- 300 : 300);
- return move_next (MOVING, bot_move_failed);
+ return ai_next (MOVE_MOVING, bot_move_failed);
}
/*
- * MOVING =obstacle_in_front=>
- * => WAIT_FOR_CLEAR_PATH
+ * MOVE_MOVING =obstacle_in_front=>
+ * => MOVE_WAIT_FOR_CLEAR_PATH
* stop the bot.
*/
fsm_branch_t
-move__MOVING__obstacle_in_front (void)
+ai__MOVE_MOVING__obstacle_in_front (void)
{
/* Stop the bot. */
asserv_stop_motor ();
- return move_next (MOVING, obstacle_in_front);
+ return ai_next (MOVE_MOVING, obstacle_in_front);
}
/*
- * MOVING_BACKWARD_TO_TURN_FREELY =bot_move_succeed=>
- * intermediate_path_found => MOVING
+ * MOVE_MOVING_BACKWARD_TO_TURN_FREELY =bot_move_succeed=>
+ * intermediate_path_found => MOVE_MOVING
* get next intermediate position from path module.
- * no_intermediate_path_found => IDLE
+ * no_intermediate_path_found => MOVE_IDLE
* post an event for the top FSM to generate a failure.
*/
fsm_branch_t
-move__MOVING_BACKWARD_TO_TURN_FREELY__bot_move_succeed (void)
+ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_succeed (void)
{
uint8_t ret = move_get_next_position (&move_data.intermediate);
if (ret == 1)
{
/* Nothing to do. */
- return move_next_branch (MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, intermediate_path_found);
+ return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, intermediate_path_found);
}
else
{
/* Post an event for the top FSM to generate a failure. */
- main_post_event_for_top_fsm = TOP_EVENT_move_fsm_failed;
- return move_next_branch (MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, no_intermediate_path_found);
+ main_post_event_for_top_fsm = AI_EVENT_move_fsm_failed;
+ return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, no_intermediate_path_found);
}
}
/*
- * MOVING_BACKWARD_TO_TURN_FREELY =bot_move_failed=>
- * intermediate_path_found => MOVING
+ * MOVE_MOVING_BACKWARD_TO_TURN_FREELY =bot_move_failed=>
+ * intermediate_path_found => MOVE_MOVING
* get next intermediate position from path module
- * no_intermediate_path_found => WAIT_FOR_CLEAR_PATH
+ * no_intermediate_path_found => MOVE_WAIT_FOR_CLEAR_PATH
* nothing to do.
*/
fsm_branch_t
-move__MOVING_BACKWARD_TO_TURN_FREELY__bot_move_failed (void)
+ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_failed (void)
{
uint8_t ret = move_get_next_position (&move_data.intermediate);
if (ret == 1)
{
/* Nothing to do. */
- return move_next_branch (MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, intermediate_path_found);
+ return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, intermediate_path_found);
}
else
{
/* Nothing to do. */
- return move_next_branch (MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_intermediate_path_found);
+ return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_intermediate_path_found);
}
}
/*
- * WAIT_FOR_CLEAR_PATH =state_timeout=>
- * no_more_obstacle_or_next_position => MOVING
+ * MOVE_WAIT_FOR_CLEAR_PATH =state_timeout=>
+ * no_more_obstacle_or_next_position => MOVE_MOVING
* get next position computed by the path module.
* if next position is the final, use a goto_xya.
* otherwise go to the next intermediate position with goto.
- * obstacle_and_no_intermediate_path_found_and_try_again => WAIT_FOR_CLEAR_PATH
+ * obstacle_and_no_intermediate_path_found_and_try_again => MOVE_WAIT_FOR_CLEAR_PATH
* decrement counter.
- * obstacle_and_no_intermediate_path_found_and_no_try_again => IDLE
+ * obstacle_and_no_intermediate_path_found_and_no_try_again => MOVE_IDLE
* post an event for the top FSM to generate a failure.
*/
fsm_branch_t
-move__WAIT_FOR_CLEAR_PATH__state_timeout (void)
+ai__MOVE_WAIT_FOR_CLEAR_PATH__state_timeout (void)
{
if (sharp_path_obstrued (asserv_get_last_moving_direction ()))
move_obstacle_here ();
@@ -371,8 +374,8 @@ move__WAIT_FOR_CLEAR_PATH__state_timeout (void)
if (ret == 1)
{
/* Go to position. */
- return move_next_branch (WAIT_FOR_CLEAR_PATH, state_timeout,
- no_more_obstacle_or_next_position);
+ return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout,
+ no_more_obstacle_or_next_position);
}
else
{
@@ -380,13 +383,13 @@ move__WAIT_FOR_CLEAR_PATH__state_timeout (void)
if (--move_data.try_again_counter == 0)
{
/* Post an event for the top FSM to generate a failure. */
- main_post_event_for_top_fsm = TOP_EVENT_move_fsm_failed;
- return move_next_branch (WAIT_FOR_CLEAR_PATH, state_timeout,
- obstacle_and_no_intermediate_path_found_and_no_try_again);
+ main_post_event_for_top_fsm = AI_EVENT_move_fsm_failed;
+ return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout,
+ obstacle_and_no_intermediate_path_found_and_no_try_again);
}
else
- return move_next_branch (WAIT_FOR_CLEAR_PATH, state_timeout,
- obstacle_and_no_intermediate_path_found_and_try_again);
+ return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout,
+ obstacle_and_no_intermediate_path_found_and_try_again);
}
}
diff --git a/digital/io/src/top_cb.c b/digital/io/src/ai_top_cb.c
index bf24c7ee..5fbe703f 100644
--- a/digital/io/src/top_cb.c
+++ b/digital/io/src/ai_top_cb.c
@@ -1,4 +1,4 @@
-/* top_cb.c */
+/* ai_top_cb.c */
/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
*
* Copyright (C) 2010 Nicolas Schodet
@@ -26,7 +26,7 @@
*/
#include "common.h"
#include "fsm.h"
-#include "top_cb.h"
+#include "ai_cb.h"
#include "top.h"
#include "move.h"
#include "chrono.h"
@@ -44,9 +44,9 @@
* nothing to do.
*/
fsm_branch_t
-top__IDLE__start (void)
+ai__IDLE__start (void)
{
- return top_next (IDLE, start);
+ return ai_next (IDLE, start);
}
/*
@@ -55,8 +55,8 @@ top__IDLE__start (void)
* the match start, place something interresting here
*/
fsm_branch_t
-top__WAIT_INIT_TO_FINISH__init_match_is_started (void)
+ai__WAIT_INIT_TO_FINISH__init_match_is_started (void)
{
- return top_next (WAIT_INIT_TO_FINISH, init_match_is_started);
+ return ai_next (WAIT_INIT_TO_FINISH, init_match_is_started);
}
diff --git a/digital/io/src/fsm.c b/digital/io/src/fsm.c
index 763f192d..53e4877d 100644
--- a/digital/io/src/fsm.c
+++ b/digital/io/src/fsm.c
@@ -25,65 +25,97 @@
#include "common.h"
#include "fsm.h"
+#include "modules/trace/trace.h"
+#include "trace_event.h"
+
#ifdef HOST
# include <stdio.h>
#endif
-/** Reset a FSM. */
void
fsm_init (fsm_t *fsm)
{
+ uint8_t i;
assert (fsm);
- fsm->state_current = fsm->state_init;
-}
-
-/** Handle state timeout, return 1 if a event was handled. */
-uint8_t
-fsm_handle_timeout (fsm_t *fsm)
-{
- assert (fsm);
- /* If there is a timeout for this state. */
- if (fsm->state_timeout != 0xffff)
- {
- if (fsm->state_timeout)
- fsm->state_timeout--;
- else
- {
- /* Timeout expired, generate corresponding event. */
- return fsm_handle_event (fsm, fsm->state_timeout_event);
- }
- }
- return 0;
+ for (i = 0; i < FSM_ACTIVE_STATES_MAX; i++)
+ fsm->states_active[i] = fsm->states_init[i];
+ for (i = 0; i < fsm->active_states_nb; i++)
+ fsm->states_timeout[i] =
+ fsm->state_timeout_table[fsm->states_init[i]];
}
-/** Handle an event on the given FSM. */
-uint8_t
-fsm_handle_event (fsm_t *fsm, u8 event)
+/** Handle an event on the given FSM on a single active state, return 1 if the
+ * event was handled. */
+static uint8_t
+fsm_handle_event_single (fsm_t *fsm, uint8_t active, uint8_t event)
{
assert (fsm);
+ assert (active < fsm->active_states_nb);
assert (event < fsm->events_nb);
/* Lookup transition. */
+ uint8_t old_state = fsm->states_active[active];
fsm_transition_t tr = fsm->transition_table[
- fsm->state_current * fsm->events_nb + event];
+ old_state * fsm->events_nb + event];
/* Ignore unhandled events. */
if (tr)
{
/* Execute transition. */
+ uint8_t new_state;
fsm_branch_t br = tr ();
/* Change state. */
#ifdef HOST
- assert (((br >> 16) & 0xff) == fsm->state_current);
+ assert (((br >> 16) & 0xff) == fsm->states_active[active]);
assert (((br >> 8) & 0xff) == event);
fprintf (stderr, "%s %s =%s=> %s\n", fsm->name,
- fsm->states_names[fsm->state_current],
+ fsm->states_names[fsm->states_active[active]],
fsm->events_names[event], fsm->states_names[br & 0xff]);
- fsm->state_current = br & 0xff;
+ new_state = br & 0xff;
#else
- fsm->state_current = br;
+ new_state = br;
#endif
- fsm->state_timeout = fsm->state_timeout_table[fsm->state_current];
+ TRACE (TRACE_FSM__HANDLE_EVENT, (u8) old_state, (u8) event,
+ (u8) new_state);
+ fsm->states_active[active] = new_state;
+ fsm->states_timeout[active] = fsm->state_timeout_table[new_state];
return 1;
}
return 0;
}
+uint8_t
+fsm_handle_event (fsm_t *fsm, uint8_t event)
+{
+ uint8_t i, handled = 0;
+ assert (fsm);
+ assert (event < fsm->events_nb);
+ for (i = 0; i < fsm->active_states_nb; i++)
+ {
+ /* Handle the event for this active state. */
+ handled += fsm_handle_event_single (fsm, i, event);
+ }
+ return handled;
+}
+
+uint8_t
+fsm_handle_timeout (fsm_t *fsm)
+{
+ uint8_t i, handled = 0;
+ assert (fsm);
+ for (i = 0; i < fsm->active_states_nb; i++)
+ {
+ /* If there is a timeout for this state. */
+ if (fsm->states_timeout[i] != 0xffff)
+ {
+ if (fsm->states_timeout[i])
+ fsm->states_timeout[i]--;
+ else
+ {
+ /* Timeout expired, generate corresponding event. */
+ handled += fsm_handle_event_single (fsm, i,
+ fsm->state_timeout_event);
+ }
+ }
+ }
+ return handled;
+}
+
diff --git a/digital/io/src/fsm.conf b/digital/io/src/fsm.conf
deleted file mode 100644
index 6dce1c15..00000000
--- a/digital/io/src/fsm.conf
+++ /dev/null
@@ -1,8 +0,0 @@
-[user]
-template-dir = fsm_templates
-
-[templates]
-template_fsm.h = %_fsm.h
-template_fsm.c = %_fsm.c
-template_cb.h = %_cb.h
-template_cb_skel.c = %_cb_skel.c
diff --git a/digital/io/src/fsm.h b/digital/io/src/fsm.h
index 57e7ae30..a90d1e83 100644
--- a/digital/io/src/fsm.h
+++ b/digital/io/src/fsm.h
@@ -26,27 +26,19 @@
* }}} */
/**
- * The io board includes several concurrent FSM. The code to handle events is
- * generic, but as events are not compatibles (they do not share the same
- * identifier), events should be generated for each FSM separately.
+ * The io board includes an FSM with several concurrent active states.
*
* The main loop tests for each possible event and generate the corresponding
* FSM events. For example:
*
* if (asserv_move_cmd_status () == success)
- * {
- * fsm_handle_event (&top_fsm, TOP_EVENT_position_reached);
- * fsm_handle_event (&getsamples_fsm, GETSAMPLES_EVENT_position_reached);
- * }
+ * fsm_handle_event (&ai_fsm, AI_EVENT_position_reached);
*
* Any unhandled event will be ignored.
*
* To start a sub machine from the top FSM, call the sub machine start
- * function (for example getsamples_start), which will set parameters, reset
- * its fsm, then trigger its start event.
- *
- * To abandon a FSM, reset it using fsm_init or it will continue to run
- * concurrently.
+ * function (for example move_start), which will set parameters, then trigger
+ * its start event.
*
* The sub machine is expected to generate an event for the top machine to
* make it continue.
@@ -63,21 +55,26 @@ typedef u8 fsm_branch_t;
/** Transition function. */
typedef fsm_branch_t (*fsm_transition_t) (void);
-/** Context of a FSM or sub-FSM. */
+/** Maximum number of active states. */
+#define FSM_ACTIVE_STATES_MAX 3
+
+/** Context of an FSM. */
struct fsm_t
{
/** Transition table. */
const fsm_transition_t *transition_table;
/** Number of events, used to index the right transition. */
u8 events_nb;
- /** Initial state. */
- u8 state_init;
- /** Current active state. */
- u8 state_current;
+ /** Initial states. */
+ u8 states_init[FSM_ACTIVE_STATES_MAX];
+ /** Current active states. */
+ u8 states_active[FSM_ACTIVE_STATES_MAX];
+ /** Nomber of active states. */
+ u8 active_states_nb;
/** State timeout table. */
const u16 *state_timeout_table;
- /** Current state timeout if not 0xffff. */
- u16 state_timeout;
+ /** Current active states timeout if not 0xffff. */
+ u16 states_timeout[FSM_ACTIVE_STATES_MAX];
/** Event to generate on state timeout. */
u8 state_timeout_event;
#ifdef HOST
@@ -95,20 +92,15 @@ typedef struct fsm_t fsm_t;
void
fsm_init (fsm_t *fsm);
-/** Handle state timeout, return 1 if a event was handled. */
+/** Handle state timeout, return non-zero if at least an event was handled. */
uint8_t
fsm_handle_timeout (fsm_t *fsm);
-/** Handle an event on the given FSM.
- * @return
- * - 0 if this FSM does not handle this event ;
- * - 1 if this FSM handles this event in its current state.
- */
+/** Handle an event on the given FSM, return non-zero if at least an event was
+ * handled. */
uint8_t
-fsm_handle_event (fsm_t *fsm, u8 event);
+fsm_handle_event (fsm_t *fsm, uint8_t event);
-#include "move_fsm.h"
-#include "top_fsm.h"
-#include "init_fsm.h"
+#include "ai_fsm.h"
#endif /* fsm_h */
diff --git a/digital/io/src/fsm_templates/template_fsm.c b/digital/io/src/fsm_templates/template_fsm.c
index 87367189..efa1b0d3 100644
--- a/digital/io/src/fsm_templates/template_fsm.c
+++ b/digital/io/src/fsm_templates/template_fsm.c
@@ -38,10 +38,13 @@ static const char *
fsm_t %(prefix)s_fsm = {
&%(prefix)s_transition_table[0][0],
%(PREFIX)s_EVENT_NB,
- %(PREFIX)s_STATE_%(initial)s,
- %(PREFIX)s_STATE_%(initial)s,
+ {
+ %(initials)s},
+ {
+ %(initials)s},
+ %(initials_nb)s,
&%(prefix)s_state_timeout_table[0],
- 0,
+ { 0 },
%(PREFIX)s_EVENT_state_timeout,
#ifdef HOST
"%(name)s",
diff --git a/digital/io/src/fsm_templates/template_init_cb_skel.c b/digital/io/src/fsm_templates/template_init_cb_skel.c
new file mode 100644
index 00000000..fea9270e
--- /dev/null
+++ b/digital/io/src/fsm_templates/template_init_cb_skel.c
@@ -0,0 +1,11 @@
+/*
+ * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT!
+ *
+ * Skeleton for %(name)s init callbacks implementation.
+ *
+%(*comments)s */
+#include "common.h"
+#include "fsm.h"
+#include "%(prefix)s_cb.h"
+
+%(states_template,template_cb_impl.c,origin=init)s
diff --git a/digital/io/src/fsm_templates/template_move_cb_skel.c b/digital/io/src/fsm_templates/template_move_cb_skel.c
new file mode 100644
index 00000000..533ffbfd
--- /dev/null
+++ b/digital/io/src/fsm_templates/template_move_cb_skel.c
@@ -0,0 +1,11 @@
+/*
+ * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT!
+ *
+ * Skeleton for %(name)s move callbacks implementation.
+ *
+%(*comments)s */
+#include "common.h"
+#include "fsm.h"
+#include "%(prefix)s_cb.h"
+
+%(states_template,template_cb_impl.c,origin=move)s
diff --git a/digital/io/src/fsm_templates/template_cb_skel.c b/digital/io/src/fsm_templates/template_top_cb_skel.c
index bf0a0639..8895dc64 100644
--- a/digital/io/src/fsm_templates/template_cb_skel.c
+++ b/digital/io/src/fsm_templates/template_top_cb_skel.c
@@ -1,11 +1,11 @@
/*
* THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT!
*
- * Skeleton for %(name)s callbacks implementation.
+ * Skeleton for %(name)s top callbacks implementation.
*
%(*comments)s */
#include "common.h"
#include "fsm.h"
#include "%(prefix)s_cb.h"
-%(states_template,template_cb_impl.c)s
+%(states_template,template_cb_impl.c,origin=top)s
diff --git a/digital/io/src/init.fsm b/digital/io/src/init.fsm
index 89ec26e6..86c08610 100644
--- a/digital/io/src/init.fsm
+++ b/digital/io/src/init.fsm
@@ -11,116 +11,108 @@ init
place the bot in the start position for a match.
States:
- IDLE
+ *INIT_IDLE
waiting for the beginning of the top FSM.
- WAIT_FIRST_JACK_IN
+ INIT_WAIT_FIRST_JACK_IN
wait for the jack to be inserted into the bot for the first time.
- WAIT_FIRST_JACK_OUT
+ INIT_WAIT_FIRST_JACK_OUT
wait the jack to be removed from the bot for the first time.
- WAIT_SECOND_JACK_IN
+ INIT_WAIT_SECOND_JACK_IN
wait for the jack to be inserted into the bot for the second time.
- WAIT_FOR_HANDS_OUT[timeout=450]
+ INIT_WAIT_FOR_HANDS_OUT[timeout=450]
just wait for operator hands to be removed from the jack.
- GO_TO_THE_WALL
+ INIT_GO_TO_THE_WALL
go to the wall for the first time.
- SET_Y_POSITION
+ INIT_SET_Y_POSITION
reset the Y position of the bot.
- SET_ANGULAR_POSITION
+ INIT_SET_ANGULAR_POSITION
reset the angular position of the bot.
- GO_AWAY_FROM_THE_WALL
+ INIT_GO_AWAY_FROM_THE_WALL
go backward from the wall for a specific distance.
- FACE_OTHER_WALL
+ INIT_FACE_OTHER_WALL
make the bot face the other wall.
- WAIT_AFTER_ROTATION [timeout=100]
+ INIT_WAIT_AFTER_ROTATION [timeout=100]
make sure rotation is really finished before continuing.
- GO_TO_THE_WALL_AGAIN
+ INIT_GO_TO_THE_WALL_AGAIN
go to the wall for the second time.
- SET_X_POSITION
+ INIT_SET_X_POSITION
reset the X position of the bot.
- GO_AWAY_FROM_THE_WALL_AGAIN
+ INIT_GO_AWAY_FROM_THE_WALL_AGAIN
go backward from the wall for a specific distance.
- GO_TO_START_POSITION
+ INIT_GO_TO_START_POSITION
go to the start position.
- WAIT_SECOND_JACK_OUT
+ INIT_WAIT_SECOND_JACK_OUT
wait the jack to be removed from the bot for the second time.
Events:
- start
- initialize the FSM.
jack_inserted_into_bot
the jack is inserted into the bot.
jack_removed_from_bot
the jack is removed from the bot.
- bot_move_succeed
- the move is finished.
- state_timeout
- timeout.
- asserv_last_cmd_ack
- last command sent to the asserv board has been acquitted.
-
-IDLE:
- start -> WAIT_FIRST_JACK_IN
+
+INIT_IDLE:
+ start -> INIT_WAIT_FIRST_JACK_IN
do nothing.
-WAIT_FIRST_JACK_IN:
- jack_inserted_into_bot -> WAIT_FIRST_JACK_OUT
+INIT_WAIT_FIRST_JACK_IN:
+ jack_inserted_into_bot -> INIT_WAIT_FIRST_JACK_OUT
do nothing.
-WAIT_FIRST_JACK_OUT:
- jack_removed_from_bot -> WAIT_SECOND_JACK_IN
+INIT_WAIT_FIRST_JACK_OUT:
+ jack_removed_from_bot -> INIT_WAIT_SECOND_JACK_IN
start trace module.
get and store the color of the bot.
-WAIT_SECOND_JACK_IN:
- jack_inserted_into_bot -> WAIT_FOR_HANDS_OUT
+INIT_WAIT_SECOND_JACK_IN:
+ jack_inserted_into_bot -> INIT_WAIT_FOR_HANDS_OUT
do nothing.
-WAIT_FOR_HANDS_OUT:
- state_timeout -> GO_TO_THE_WALL
+INIT_WAIT_FOR_HANDS_OUT:
+ state_timeout -> INIT_GO_TO_THE_WALL
fuck the wall in front.
-GO_TO_THE_WALL:
- bot_move_succeed -> SET_Y_POSITION
+INIT_GO_TO_THE_WALL:
+ bot_move_succeed -> INIT_SET_Y_POSITION
reset the Y position of the bot.
-SET_Y_POSITION:
- asserv_last_cmd_ack -> SET_ANGULAR_POSITION
+INIT_SET_Y_POSITION:
+ asserv_last_cmd_ack -> INIT_SET_ANGULAR_POSITION
reset the angular position of the bot.
-SET_ANGULAR_POSITION:
- asserv_last_cmd_ack -> GO_AWAY_FROM_THE_WALL
+INIT_SET_ANGULAR_POSITION:
+ asserv_last_cmd_ack -> INIT_GO_AWAY_FROM_THE_WALL
move away from the wall (linear move).
-GO_AWAY_FROM_THE_WALL:
- bot_move_succeed -> FACE_OTHER_WALL
+INIT_GO_AWAY_FROM_THE_WALL:
+ bot_move_succeed -> INIT_FACE_OTHER_WALL
turn to face the other wall.
-FACE_OTHER_WALL:
- bot_move_succeed -> WAIT_AFTER_ROTATION
+INIT_FACE_OTHER_WALL:
+ bot_move_succeed -> INIT_WAIT_AFTER_ROTATION
nothing to do.
-WAIT_AFTER_ROTATION:
- state_timeout -> GO_TO_THE_WALL_AGAIN
+INIT_WAIT_AFTER_ROTATION:
+ state_timeout -> INIT_GO_TO_THE_WALL_AGAIN
fuck the wall in front.
-GO_TO_THE_WALL_AGAIN:
- bot_move_succeed -> SET_X_POSITION
+INIT_GO_TO_THE_WALL_AGAIN:
+ bot_move_succeed -> INIT_SET_X_POSITION
reset the X position of the bot.
-SET_X_POSITION:
- asserv_last_cmd_ack -> GO_AWAY_FROM_THE_WALL_AGAIN
+INIT_SET_X_POSITION:
+ asserv_last_cmd_ack -> INIT_GO_AWAY_FROM_THE_WALL_AGAIN
move away from the wall (linear move).
-GO_AWAY_FROM_THE_WALL_AGAIN:
- bot_move_succeed -> GO_TO_START_POSITION
+INIT_GO_AWAY_FROM_THE_WALL_AGAIN:
+ bot_move_succeed -> INIT_GO_TO_START_POSITION
go to the start position with a go to movement.
-GO_TO_START_POSITION:
- bot_move_succeed -> WAIT_SECOND_JACK_OUT
+INIT_GO_TO_START_POSITION:
+ bot_move_succeed -> INIT_WAIT_SECOND_JACK_OUT
nothing to do, the bot is at the start position.
-WAIT_SECOND_JACK_OUT:
- jack_removed_from_bot -> IDLE
+INIT_WAIT_SECOND_JACK_OUT:
+ jack_removed_from_bot -> INIT_IDLE
tell other FSM the match begins.
start the chrono.
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index 8d23b107..ecc115af 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -81,17 +81,6 @@ uint8_t main_post_event_for_top_fsm = 0xFF;
uint16_t main_sharp_ignore_event;
/**
- * Post an event for the main loop to wake up the move FSM in a certain count
- * of cycles.
- */
-uint16_t main_move_wait_cycle;
-
-/**
- * The same for init FSM
- */
-uint16_t main_init_wait_cycle;
-
-/**
* Sharps stats counters.
*/
uint8_t main_stats_sharps, main_stats_sharps_cpt;
@@ -108,23 +97,6 @@ static uint8_t main_stats_asserv_, main_stats_asserv_cpt_;
static uint8_t main_stats_timer_;
/**
- * Get short FSM name.
- */
-char
-main_short_fsm_name (fsm_t *fsm)
-{
- /* Dirty hack. */
- char id = '?';
- if (fsm == &top_fsm)
- id = 'T';
- else if (fsm == &init_fsm)
- id = 'I';
- else if (fsm == &move_fsm)
- id = 'M';
- return id;
-}
-
-/**
* Main events management.
* This function is responsible to get all events and send them to the
* different FSM that want its.
@@ -132,29 +104,18 @@ main_short_fsm_name (fsm_t *fsm)
void
main_event_to_fsm (void)
{
-#define FSM_HANDLE_EVENT(fsm,event) \
- do { if (fsm_handle_event (fsm,event)) \
- { \
- TRACE (TRACE_FSM__HANDLE_EVENT, main_short_fsm_name (fsm), (u8) event); \
- return; \
- } \
- } while (0)
+ /* If an event is handled, stop generating any other event, because a
+ * transition may have invalidated the current robot state. */
+#define FSM_HANDLE_EVENT(fsm, event) \
+ do { if (fsm_handle_event ((fsm), (event))) return; } while (0)
#define FSM_HANDLE_TIMEOUT(fsm) \
- do { if (fsm_handle_timeout (fsm)) \
- { \
- TRACE (TRACE_FSM__HANDLE_TIMEOUT, main_short_fsm_name (fsm)); \
- return; \
- } \
- } while (0)
+ do { if (fsm_handle_timeout (fsm)) return; } while (0)
/* Update FSM timeouts. */
- FSM_HANDLE_TIMEOUT (&move_fsm);
- FSM_HANDLE_TIMEOUT (&top_fsm);
- FSM_HANDLE_TIMEOUT (&init_fsm);
+ FSM_HANDLE_TIMEOUT (&ai_fsm);
/* If we have entering this function, last command of the asserv board has
* been aquited. */
- FSM_HANDLE_EVENT (&init_fsm, INIT_EVENT_asserv_last_cmd_ack);
- FSM_HANDLE_EVENT (&top_fsm, TOP_EVENT_asserv_last_cmd_ack);
+ FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_asserv_last_cmd_ack);
asserv_status_e
move_status = none,
@@ -168,39 +129,19 @@ main_event_to_fsm (void)
/* Check commands move status. */
if (move_status == success)
- {
- /* Pass it to all the FSM that need it. */
- FSM_HANDLE_EVENT (&move_fsm,
- MOVE_EVENT_bot_move_succeed);
- FSM_HANDLE_EVENT (&init_fsm,
- INIT_EVENT_bot_move_succeed);
- FSM_HANDLE_EVENT (&top_fsm,
- TOP_EVENT_bot_move_succeed);
- }
+ FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_bot_move_succeed);
else if (move_status == failure)
- {
- /* Move failed. */
- FSM_HANDLE_EVENT (&move_fsm,
- MOVE_EVENT_bot_move_failed);
- FSM_HANDLE_EVENT (&top_fsm,
- TOP_EVENT_bot_move_failed);
- }
+ FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_bot_move_failed);
/* Jack */
- if(switch_get_jack())
- {
- FSM_HANDLE_EVENT (&init_fsm,
- INIT_EVENT_jack_removed_from_bot);
- }
+ if (switch_get_jack ())
+ FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_jack_removed_from_bot);
else
- {
- FSM_HANDLE_EVENT (&init_fsm,
- INIT_EVENT_jack_inserted_into_bot);
- }
+ FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_jack_inserted_into_bot);
if (init_match_is_started)
{
- FSM_HANDLE_EVENT (&top_fsm, TOP_EVENT_init_match_is_started);
+ FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_init_match_is_started);
/* This must be done in the last part of this block. */
init_match_is_started = 0;
@@ -217,7 +158,7 @@ main_event_to_fsm (void)
/* Reset */
main_post_event_for_top_fsm = 0xFF;
/* Post the event */
- FSM_HANDLE_EVENT (&top_fsm, save_event);
+ FSM_HANDLE_EVENT (&ai_fsm, save_event);
}
/* Sharps event for move FSM */
/* Get the current direction of the bot */
@@ -228,8 +169,7 @@ main_event_to_fsm (void)
if (sharp_path_obstrued (moving_direction))
{
/* Generate an event for move FSM */
- FSM_HANDLE_EVENT (&move_fsm,
- MOVE_EVENT_obstacle_in_front);
+ FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_obstacle_in_front);
}
}
/* TODO: Check other sensors */
@@ -258,15 +198,12 @@ main_init (void)
/* Path module */
path_init (PG_BORDER_DISTANCE, PG_BORDER_DISTANCE,
PG_WIDTH - PG_BORDER_DISTANCE, PG_LENGTH - PG_BORDER_DISTANCE);
- /* Top initialization. */
+ /* Top. */
top_init ();
- /* Init all FSM (except move FSM) */
- fsm_init(&top_fsm);
- fsm_init(&init_fsm);
- /* Start all FSM (except move and top FSM) */
- /* FIXME: who sould start top? init?. */
- fsm_handle_event (&init_fsm, INIT_EVENT_start);
- fsm_handle_event (&top_fsm, TOP_EVENT_start);
+ /* Init FSM. */
+ fsm_init (&ai_fsm);
+ /* Start FSM. */
+ fsm_handle_event (&ai_fsm, AI_EVENT_start);
/* Sharp module */
sharp_init ();
/* PWM module */
@@ -341,9 +278,6 @@ main_loop (void)
/* Update the ignore sharp event flag */
if (main_sharp_ignore_event)
main_sharp_ignore_event--;
- /* Update wait flag for move FSM */
- if (main_move_wait_cycle)
- main_move_wait_cycle--;
/* Update sharps */
sharp_update ();
diff --git a/digital/io/src/main.h b/digital/io/src/main.h
index 92793fcc..8791d5ed 100644
--- a/digital/io/src/main.h
+++ b/digital/io/src/main.h
@@ -28,7 +28,7 @@
/**
* Post a event to the top FSM in the next iteration of main loop.
* You just need to set variable to the value of the event you want to post to
- * the top FSM and add one to it (because some event could have a zero value).
+ * the top FSM.
* It will be posted and cleared in the next main loop iteration.
*/
extern uint8_t main_post_event_for_top_fsm;
@@ -40,18 +40,4 @@ extern uint8_t main_post_event_for_top_fsm;
*/
extern uint16_t main_sharp_ignore_event;
-/**
- * Post an event for the main loop to wake up the move FSM in a certain count
- * of cycles.
- * This flag is used by the move FSM to wait a certain amount of time before
- * checking on the sharps again.
- */
-extern uint16_t main_move_wait_cycle;
-
-/**
- * Post an event for the init loop to wait a certain amount of time before
- * make any move
- */
-extern uint16_t main_init_wait_cycle;
-
#endif /* main_h */
diff --git a/digital/io/src/move.c b/digital/io/src/move.c
index 181e4545..eec8799d 100644
--- a/digital/io/src/move.c
+++ b/digital/io/src/move.c
@@ -45,20 +45,7 @@ move_start (asserv_position_t position, uint8_t backward)
move_data.final_move = 0;
/* Reset move FSM flags */
main_sharp_ignore_event = 0;
- main_move_wait_cycle = 0;
/* Start the FSM. */
- fsm_init (&move_fsm);
- fsm_handle_event (&move_fsm, MOVE_EVENT_start);
+ fsm_handle_event (&ai_fsm, AI_EVENT_move_start);
}
-void
-move_stop (void)
-{
- /* This is a dirty hack. */
- /* Re-init move FSM. */
- fsm_init (&move_fsm);
- /* Ask the asserv to stop the motor. */
- asserv_stop_motor ();
- /* Generate the event, move FSM stopped. */
- main_post_event_for_top_fsm = TOP_EVENT_move_fsm_stopped;
-}
diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm
index 50f104e2..c278c748 100644
--- a/digital/io/src/move.fsm
+++ b/digital/io/src/move.fsm
@@ -6,62 +6,56 @@ move
move with avoid obstacle.
States:
- IDLE
+ *MOVE_IDLE
waiting for the beginning of the move FSM.
- MOVING
+ MOVE_MOVING
moving to a position (intermediate or final).
- MOVING_BACKWARD_TO_TURN_FREELY
+ MOVE_MOVING_BACKWARD_TO_TURN_FREELY
moving backward to go away from what is blocking the bot.
- WAIT_FOR_CLEAR_PATH [timeout=255]
+ MOVE_WAIT_FOR_CLEAR_PATH [timeout=255]
waiting for obstacle to disappear.
Events:
- start
+ move_start
initialize the FSM and start the movement directly.
- bot_move_failed
- the bot movement failed (blocked by something for example).
- bot_move_succeed
- the bot has finished to moved and arrived at the desired position.
obstacle_in_front
the bot has seen something (with the sharps) (front is the same when going backward).
- state_timeout
- timeout.
-IDLE:
- start -> MOVING
+MOVE_IDLE:
+ move_start -> MOVE_MOVING
ask the asserv to go to the computed position.
-MOVING:
- bot_move_succeed: we_are_at_final_position -> IDLE
+MOVE_MOVING:
+ bot_move_succeed: we_are_at_final_position -> MOVE_IDLE
post an event for the top FSM to tell it we have finished.
bot_move_succeed: position_intermediary -> .
get next position computed by the path module.
if next position is the final, use a goto_xya.
otherwise go to the next intermediate position with goto.
- bot_move_succeed: no_intermediate_path_found -> IDLE
+ bot_move_succeed: no_intermediate_path_found -> MOVE_IDLE
post an event for the top FSM to generate a failure.
- bot_move_failed -> MOVING_BACKWARD_TO_TURN_FREELY
+ bot_move_failed -> MOVE_MOVING_BACKWARD_TO_TURN_FREELY
compute the obstacle position.
move backward to turn freely.
- obstacle_in_front -> WAIT_FOR_CLEAR_PATH
+ obstacle_in_front -> MOVE_WAIT_FOR_CLEAR_PATH
stop the bot.
-MOVING_BACKWARD_TO_TURN_FREELY:
- bot_move_succeed: intermediate_path_found -> MOVING
+MOVE_MOVING_BACKWARD_TO_TURN_FREELY:
+ bot_move_succeed: intermediate_path_found -> MOVE_MOVING
get next intermediate position from path module.
- bot_move_succeed: no_intermediate_path_found -> IDLE
+ bot_move_succeed: no_intermediate_path_found -> MOVE_IDLE
post an event for the top FSM to generate a failure.
- bot_move_failed: intermediate_path_found -> MOVING
+ bot_move_failed: intermediate_path_found -> MOVE_MOVING
get next intermediate position from path module
- bot_move_failed: no_intermediate_path_found -> WAIT_FOR_CLEAR_PATH
+ bot_move_failed: no_intermediate_path_found -> MOVE_WAIT_FOR_CLEAR_PATH
nothing to do.
-WAIT_FOR_CLEAR_PATH:
- state_timeout: no_more_obstacle_or_next_position -> MOVING
+MOVE_WAIT_FOR_CLEAR_PATH:
+ state_timeout: no_more_obstacle_or_next_position -> MOVE_MOVING
get next position computed by the path module.
if next position is the final, use a goto_xya.
otherwise go to the next intermediate position with goto.
state_timeout: obstacle_and_no_intermediate_path_found_and_try_again -> .
decrement counter.
- state_timeout: obstacle_and_no_intermediate_path_found_and_no_try_again -> IDLE
+ state_timeout: obstacle_and_no_intermediate_path_found_and_no_try_again -> MOVE_IDLE
post an event for the top FSM to generate a failure.
diff --git a/digital/io/src/move.h b/digital/io/src/move.h
index 48d7a3ea..6a53725f 100644
--- a/digital/io/src/move.h
+++ b/digital/io/src/move.h
@@ -73,10 +73,4 @@ extern struct move_data_t move_data;
void
move_start (asserv_position_t position, uint8_t backward);
-/**
- * Stop the move FSM and the bot.
- */
-void
-move_stop (void);
-
#endif /* move_h */
diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm
index 8203dfdb..dde79a51 100644
--- a/digital/io/src/top.fsm
+++ b/digital/io/src/top.fsm
@@ -3,7 +3,7 @@ top
Main FSM calling other FSM.
States:
- IDLE
+ *IDLE
waiting for the beginning of the top FSM.
WAIT_INIT_TO_FINISH
waiting for the end of init.
@@ -15,8 +15,6 @@ Events:
when the move FSM returns with success.
move_fsm_failed
when the move FSM returns with an error.
- move_fsm_stopped
- when the move FSM has stop the bot (as asked).
bot_move_succeed
when the bot has succeed its asserv movement.
bot_move_failed
@@ -28,6 +26,9 @@ Events:
asserv_last_cmd_ack
last command sent to the asserv board has been acquitted.
+import init.fsm
+import move.fsm
+
IDLE:
start -> WAIT_INIT_TO_FINISH
nothing to do.
diff --git a/digital/io/src/trace.trc b/digital/io/src/trace.trc
index 226c388b..2d3cfb90 100644
--- a/digital/io/src/trace.trc
+++ b/digital/io/src/trace.trc
@@ -2,8 +2,7 @@ EVENT asserv__retransmit seq 1 "Asserv[0x%02x]: retransmiting."
EVENT asserv__send seq 1 cmd 1 "Asserv[0x%02x]: sending: !%c."
EVENT asserv__last_status_ack seq 1 status 1 "Asserv[0x%02x]: status: %x."
EVENT main_timer__late "MainTimer: we are late."
-EVENT fsm__handle_event fsm 1 event 1 "FSM[%c]: handle event %i."
-EVENT fsm__handle_timeout fsm 1 "FSM[%c]: handle timeout."
+EVENT fsm__handle_event from 1 event 1 to 1 "FSM: %i =%i=> %i."
EVENT move__go_to xs 2 ys 2 as 2 xd 2 yd 2 ad 2 "Move: (%d, %d, %x) -> (%d, %d, %x)."
EVENT move__obstacle x 2 y 2 "Move: obstacle a (%d, %d)."
EVENT move__sharp s 2 s 2 s 2 s 2 s 2 "Sharps values: %x %x %x %x %x."