summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorJérôme Jutteau2011-05-02 23:32:51 +0200
committerNicolas Schodet2011-05-02 23:32:51 +0200
commit2376aeb614d69d7050209f858818bfa7d36be398 (patch)
treededa5a2e0b95187c9fc295bf3f6c5230eb684b99 /digital
parent8a6cac0a41be34d36122ab5271295b0ee83fddac (diff)
digital/io: adapt FSM to new FSM system (AngFSM)
Diffstat (limited to 'digital')
-rw-r--r--digital/io/src/Makefile15
-rw-r--r--digital/io/src/ai.conf12
-rw-r--r--digital/io/src/ai_hola_cb.c165
-rw-r--r--digital/io/src/ai_init_cb.c213
-rw-r--r--digital/io/src/ai_loader_cb.c636
-rw-r--r--digital/io/src/ai_move_cb.c648
-rw-r--r--digital/io/src/ai_top_cb.c434
-rw-r--r--digital/io/src/fsm.c137
-rw-r--r--digital/io/src/fsm.h648
-rw-r--r--digital/io/src/fsm.host.c1571
-rw-r--r--digital/io/src/fsm_templates/template_cb.h10
-rw-r--r--digital/io/src/fsm_templates/template_cb_decl.h6
-rw-r--r--digital/io/src/fsm_templates/template_cb_impl.c9
-rw-r--r--digital/io/src/fsm_templates/template_fsm.c55
-rw-r--r--digital/io/src/fsm_templates/template_fsm.h52
-rw-r--r--digital/io/src/fsm_templates/template_hola_cb_skel.c11
-rw-r--r--digital/io/src/fsm_templates/template_init_cb_skel.c11
-rw-r--r--digital/io/src/fsm_templates/template_loader_cb_skel.c11
-rw-r--r--digital/io/src/fsm_templates/template_move_cb_skel.c11
-rw-r--r--digital/io/src/fsm_templates/template_top_cb_skel.c11
-rw-r--r--digital/io/src/hola.c115
-rw-r--r--digital/io/src/hola.fsm59
-rw-r--r--digital/io/src/init.c206
-rw-r--r--digital/io/src/init.fsm106
-rw-r--r--digital/io/src/loader.c613
-rw-r--r--digital/io/src/loader.fsm227
-rw-r--r--digital/io/src/main.c79
-rw-r--r--digital/io/src/move.c605
-rw-r--r--digital/io/src/move.fsm137
-rw-r--r--digital/io/src/top.c359
-rw-r--r--digital/io/src/top.fsm162
31 files changed, 4060 insertions, 3274 deletions
diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile
index ceb7c206..8d1d70b8 100644
--- a/digital/io/src/Makefile
+++ b/digital/io/src/Makefile
@@ -7,7 +7,8 @@ io_SOURCES = main.c servo.avr.c eeprom.avr.c pwm.c \
switch.avr.c chrono.c main_timer.avr.c servo_pos.c \
twi_master.c asserv.c mimot.c \
simu.host.c contact.c usdist.c radar.c \
- path.c food.c events.host.c
+ path.c food.c events.host.c \
+ fsm.host.c init.c move.c top.c hola.c loader.c fsm_AI_gen.avr.c
# Modules needed for IO.
MODULES = proto uart twi utils adc math/fixed math/geometry path/astar \
trace flash spi
@@ -24,11 +25,13 @@ vpath %.c $(AI_MODULES:%=../../ai/src/%)
vpath %.h $(AI_MODULES:%=../../ai/src/%)
INCLUDES += -I. $(AI_MODULES:%=-I../../ai/src/%)
-# FSM.
-FSM = ai
-FSM_SOURCES := top init move loader hola
-# Include FSM makefile.
-include $(BASE)/make/Makefile.fsm
+include $(BASE)/make/Makefile.gen
+
+#FSM generation
+obj/main.avr.o: fsm_AI_gen.h
+fsm_AI_gen.avr.c fsm_AI_gen.h: host
+ ./io.host --gen
+ mv fsm_AI_gen.c fsm_AI_gen.avr.c
events.h events.host.c: trace.trc
PYTHONPATH=$(BASE)/../../host python $(BASE)/../../tools/trace/trace.py -t create -i $< -o
diff --git a/digital/io/src/ai.conf b/digital/io/src/ai.conf
deleted file mode 100644
index ce7f6374..00000000
--- a/digital/io/src/ai.conf
+++ /dev/null
@@ -1,12 +0,0 @@
-[user]
-template-dir = fsm_templates
-
-[templates]
-template_fsm.h = %_fsm.h
-template_fsm.c = %_fsm.c
-template_cb.h = %_cb.h
-template_top_cb_skel.c = %_top_cb_skel.c
-template_init_cb_skel.c = %_init_cb_skel.c
-template_move_cb_skel.c = %_move_cb_skel.c
-template_loader_cb_skel.c = %_loader_cb_skel.c
-template_hola_cb_skel.c = %_hola_cb_skel.c
diff --git a/digital/io/src/ai_hola_cb.c b/digital/io/src/ai_hola_cb.c
deleted file mode 100644
index 6b76b058..00000000
--- a/digital/io/src/ai_hola_cb.c
+++ /dev/null
@@ -1,165 +0,0 @@
-/* ai_hola_cb.c */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
- *
- * Copyright (C) 2010 Nicolas Schodet
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}} */
-#include "common.h"
-#include "fsm.h"
-#include "ai_cb.h"
-#include "asserv.h"
-#include "mimot.h"
-#include "bot.h"
-#include "playground.h"
-
-#include "modules/math/fixed/fixed.h"
-
-uint32_t hola_angle;
-int16_t hola_timeout;
-
-/*
- * HOLA_IDLE =start=>
- * => HOLA_WAIT_HOLA
- */
-fsm_branch_t
-ai__HOLA_IDLE__start (void)
-{
- return ai_next (HOLA_IDLE, start);
-}
-
-/*
- * HOLA_WAIT_HOLA =hola_start=>
- * => HOLA_WAIT_JACK_IN
- */
-fsm_branch_t
-ai__HOLA_WAIT_HOLA__hola_start (void)
-{
- return ai_next (HOLA_WAIT_HOLA, hola_start);
-}
-
-/*
- * HOLA_WAIT_JACK_IN =jack_inserted_into_bot=>
- * => HOLA_WAIT_JACK_OUT
- */
-fsm_branch_t
-ai__HOLA_WAIT_JACK_IN__jack_inserted_into_bot (void)
-{
- return ai_next (HOLA_WAIT_JACK_IN, jack_inserted_into_bot);
-}
-
-/*
- * HOLA_WAIT_JACK_OUT =jack_removed_from_bot=>
- * => HOLA_ROUNDS
- */
-fsm_branch_t
-ai__HOLA_WAIT_JACK_OUT__jack_removed_from_bot (void)
-{
- return ai_next (HOLA_WAIT_JACK_OUT, jack_removed_from_bot);
-}
-
-/*
- * HOLA_ROUNDS =asserv_last_cmd_ack=>
- * no_timeout => HOLA_ROUNDS
- * move clamp rounds
- * timeout => HOLA_UP
- * move up
- * rotate
- */
-fsm_branch_t
-ai__HOLA_ROUNDS__asserv_last_cmd_ack (void)
-{
- if (++hola_timeout < 150)
- {
- int32_t cos = fixed_cos_f824 (hola_angle) + 0x1000000;
- int32_t sin = fixed_sin_f824 (hola_angle) + 0x1000000;
- int32_t dx = BOT_CLAMP_OPEN_STEP
- + fixed_mul_f824 (BOT_CLAMP_STROKE_STEP / 3 / 2, cos);
- int32_t dy = BOT_ELEVATOR_ZERO_SPEED
- + fixed_mul_f824 (BOT_ELEVATOR_REST_STEP / 3 / 2, sin);
- mimot_move_motor0_absolute (dx, BOT_CLAMP_SPEED);
- mimot_move_motor1_absolute (dx, BOT_CLAMP_SPEED);
- asserv_move_motor0_absolute (dy, BOT_ELEVATOR_SPEED);
- hola_angle += 0x1000000 / (225 / 12);
- return ai_next_branch (HOLA_ROUNDS, asserv_last_cmd_ack, no_timeout);
- }
- else
- {
- asserv_set_speed (0x10, 0x1c, 0x10, 0x1c);
- asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED / 3);
- asserv_move_angularly (POSITION_A_DEG (174));
- return ai_next_branch (HOLA_ROUNDS, asserv_last_cmd_ack, timeout);
- }
-}
-
-/*
- * HOLA_UP =bot_move_succeed=>
- * => HOLA_CENTER
- * rotate to center
- */
-fsm_branch_t
-ai__HOLA_UP__bot_move_succeed (void)
-{
- asserv_set_speed (0x10, 0x20, 0x10, 0x20);
- asserv_move_angularly (POSITION_A_DEG (-90));
- asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP / 2, BOT_ELEVATOR_SPEED);
- return ai_next (HOLA_UP, bot_move_succeed);
-}
-
-/*
- * HOLA_CENTER =bot_move_succeed=>
- * => HOLA_CLAP_CLOSE
- * close clamp
- */
-fsm_branch_t
-ai__HOLA_CENTER__bot_move_succeed (void)
-{
- mimot_motor0_clamp (BOT_CLAMP_ZERO_SPEED, 0);
- mimot_motor1_clamp (BOT_CLAMP_ZERO_SPEED, 0);
- return ai_next (HOLA_CENTER, bot_move_succeed);
-}
-
-/*
- * HOLA_CLAP_CLOSE =clamp_succeed=>
- * => HOLA_CLAP_OPEN
- * open clamp
- */
-fsm_branch_t
-ai__HOLA_CLAP_CLOSE__clamp_succeed (void)
-{
- mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- return ai_next (HOLA_CLAP_CLOSE, clamp_succeed);
-}
-
-/*
- * HOLA_CLAP_OPEN =clamp_succeed=>
- * => HOLA_CLAP_CLOSE
- * close clamp
- */
-fsm_branch_t
-ai__HOLA_CLAP_OPEN__clamp_succeed (void)
-{
- mimot_motor0_clamp (BOT_CLAMP_SPEED, 0);
- mimot_motor1_clamp (BOT_CLAMP_SPEED, 0);
- return ai_next (HOLA_CLAP_OPEN, clamp_succeed);
-}
-
-
diff --git a/digital/io/src/ai_init_cb.c b/digital/io/src/ai_init_cb.c
deleted file mode 100644
index 444b867b..00000000
--- a/digital/io/src/ai_init_cb.c
+++ /dev/null
@@ -1,213 +0,0 @@
-/* init_cb.c - init FSM callbacks. */
-/* {{{
- *
- * Copyright (C) 2009 Nicolas Haller
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}}
- * init:
- * place the bot in the start position for a match.
- */
-#include "common.h"
-#include "fsm.h"
-#include "ai_cb.h"
-#include "asserv.h"
-#include "init.h"
-#include "playground.h"
-#include "main.h"
-#include "bot.h"
-#include "switch.h"
-#include "chrono.h"
-#include "playground.h"
-#include "modules/trace/trace.h"
-#include "modules/path/path.h"
-
-/*
- * INIT_IDLE =start=>
- * => INIT_WAIT_FIRST_JACK_IN
- * do nothing.
- */
-fsm_branch_t
-ai__INIT_IDLE__start (void)
-{
- return ai_next (INIT_IDLE, start);
-}
-
-/*
- * INIT_WAIT_FIRST_JACK_IN =jack_inserted_into_bot=>
- * => INIT_WAIT_FIRST_JACK_OUT
- * do nothing.
- */
-fsm_branch_t
-ai__INIT_WAIT_FIRST_JACK_IN__jack_inserted_into_bot (void)
-{
- return ai_next (INIT_WAIT_FIRST_JACK_IN, jack_inserted_into_bot);
-}
-
-/*
- * INIT_WAIT_FIRST_JACK_OUT =jack_removed_from_bot=>
- * => INIT_WAIT_SECOND_JACK_IN
- * start trace module.
- * get and store the color of the bot.
- */
-fsm_branch_t
-ai__INIT_WAIT_FIRST_JACK_OUT__jack_removed_from_bot (void)
-{
- /* Initialize trace module (erase the flash). */
- trace_init ();
- /* Get the color. */
- bot_color = switch_get_color ();
- return ai_next (INIT_WAIT_FIRST_JACK_OUT, jack_removed_from_bot);
-}
-
-/*
- * INIT_WAIT_SECOND_JACK_IN =jack_inserted_into_bot=>
- * => INIT_WAIT_FOR_HANDS_OUT
- * do nothing.
- */
-fsm_branch_t
-ai__INIT_WAIT_SECOND_JACK_IN__jack_inserted_into_bot (void)
-{
- return ai_next (INIT_WAIT_SECOND_JACK_IN, jack_inserted_into_bot);
-}
-
-/*
- * INIT_WAIT_FOR_HANDS_OUT =state_timeout=>
- * => INIT_GO_TO_THE_WALL
- * fuck the wall in front.
- */
-fsm_branch_t
-ai__INIT_WAIT_FOR_HANDS_OUT__state_timeout (void)
-{
- /* Go to the wall, no backward. */
- asserv_go_to_the_wall (0);
- return ai_next (INIT_WAIT_FOR_HANDS_OUT, state_timeout);
-}
-
-/*
- * INIT_GO_TO_THE_WALL =bot_move_succeed=>
- * => INIT_GO_AWAY_FROM_THE_WALL
- * reset the Y position of the bot.
- * reset the angular position of the bot.
- * move away from the wall (linear move).
- */
-fsm_branch_t
-ai__INIT_GO_TO_THE_WALL__bot_move_succeed (void)
-{
- asserv_set_y_position (PG_Y (PG_LENGTH - BOT_SIZE_FRONT));
- asserv_set_angle_position (PG_A_DEG (90));
- /* Move away from the border. */
- asserv_move_linearly (- INIT_DIST);
- return ai_next (INIT_GO_TO_THE_WALL, bot_move_succeed);
-}
-
-/*
- * INIT_GO_AWAY_FROM_THE_WALL =bot_move_succeed=>
- * => INIT_FACE_OTHER_WALL
- * turn to face the other wall.
- */
-fsm_branch_t
-ai__INIT_GO_AWAY_FROM_THE_WALL__bot_move_succeed (void)
-{
- /* Face the other wall. */
- asserv_goto_angle (PG_A_DEG (180));
- return ai_next (INIT_GO_AWAY_FROM_THE_WALL, bot_move_succeed);
-}
-
-/*
- * INIT_FACE_OTHER_WALL =bot_move_succeed=>
- * => INIT_WAIT_AFTER_ROTATION
- * nothing to do.
- */
-fsm_branch_t
-ai__INIT_FACE_OTHER_WALL__bot_move_succeed (void)
-{
- return ai_next (INIT_FACE_OTHER_WALL, bot_move_succeed);
-}
-
-/*
- * INIT_WAIT_AFTER_ROTATION =state_timeout=>
- * => INIT_GO_TO_THE_WALL_AGAIN
- * fuck the wall in front.
- */
-fsm_branch_t
-ai__INIT_WAIT_AFTER_ROTATION__state_timeout (void)
-{
- /* Go to the wall, no backward. */
- asserv_go_to_the_wall (0);
- return ai_next (INIT_WAIT_AFTER_ROTATION, state_timeout);
-}
-
-/*
- * INIT_GO_TO_THE_WALL_AGAIN =bot_move_succeed=>
- * => INIT_GO_AWAY_FROM_THE_WALL_AGAIN
- * reset the X position of the bot.
- * move away from the wall (linear move).
- */
-fsm_branch_t
-ai__INIT_GO_TO_THE_WALL_AGAIN__bot_move_succeed (void)
-{
- asserv_set_x_position (PG_X (BOT_SIZE_FRONT));
- /* Move away from the border. */
- asserv_move_linearly (- INIT_DIST);
- return ai_next (INIT_GO_TO_THE_WALL_AGAIN, bot_move_succeed);
-}
-
-/*
- * INIT_GO_AWAY_FROM_THE_WALL_AGAIN =bot_move_succeed=>
- * => INIT_GO_TO_START_POSITION
- * go to the start position with a go to movement.
- */
-fsm_branch_t
-ai__INIT_GO_AWAY_FROM_THE_WALL_AGAIN__bot_move_succeed (void)
-{
- /* Move away from the border. */
- asserv_goto_xya (BOT_INIT_XYA, 0);
- return ai_next (INIT_GO_AWAY_FROM_THE_WALL_AGAIN, bot_move_succeed);
-}
-
-/*
- * INIT_GO_TO_START_POSITION =bot_move_succeed=>
- * => INIT_WAIT_SECOND_JACK_OUT
- * nothing to do, the bot is at the start position.
- */
-fsm_branch_t
-ai__INIT_GO_TO_START_POSITION__bot_move_succeed (void)
-{
- return ai_next (INIT_GO_TO_START_POSITION, bot_move_succeed);
-}
-
-/*
- * INIT_WAIT_SECOND_JACK_OUT =jack_removed_from_bot=>
- * => INIT_IDLE
- * tell other FSM the match begins.
- * start the chrono.
- */
-fsm_branch_t
-ai__INIT_WAIT_SECOND_JACK_OUT__jack_removed_from_bot (void)
-{
- /* Set the flag to transmit to other FSM. */
- init_match_is_started = 1;
- /* Start the chrono. */
- chrono_init ();
- return ai_next (INIT_WAIT_SECOND_JACK_OUT, jack_removed_from_bot);
-}
-
-
diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c
deleted file mode 100644
index fdb628a9..00000000
--- a/digital/io/src/ai_loader_cb.c
+++ /dev/null
@@ -1,636 +0,0 @@
-/* ai_loader_cb.c */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
- *
- * Copyright (C) 2010 Nicolas Schodet
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}} */
-#include "common.h"
-#include "fsm.h"
-#include "ai_cb.h"
-#include "asserv.h"
-#include "mimot.h"
-#include "bot.h"
-#include "main.h"
-#include "food.h"
-#include "loader.h"
-
-/*
- * LOADER_IDLE =start=>
- * => LOADER_WAIT_JACK_IN
- */
-fsm_branch_t
-ai__LOADER_IDLE__start (void)
-{
- return ai_next (LOADER_IDLE, start);
-}
-
-/*
- * LOADER_WAIT_JACK_IN =jack_inserted_into_bot=>
- * => LOADER_WAIT_JACK_OUT
- */
-fsm_branch_t
-ai__LOADER_WAIT_JACK_IN__jack_inserted_into_bot (void)
-{
- return ai_next (LOADER_WAIT_JACK_IN, jack_inserted_into_bot);
-}
-
-/*
- * LOADER_WAIT_JACK_IN =hola_start=>
- * => LOADER_IDLE
- */
-fsm_branch_t
-ai__LOADER_WAIT_JACK_IN__hola_start (void)
-{
- return ai_next (LOADER_WAIT_JACK_IN, hola_start);
-}
-
-/*
- * LOADER_WAIT_JACK_OUT =jack_removed_from_bot=>
- * => LOADER_INIT_ELEVATOR_ZERO
- * find elevator zero
- * close clamp
- * find gate zero
- */
-fsm_branch_t
-ai__LOADER_WAIT_JACK_OUT__jack_removed_from_bot (void)
-{
- asserv_motor0_zero_position (-BOT_ELEVATOR_ZERO_SPEED);
- mimot_motor0_clamp (BOT_CLAMP_ZERO_SPEED, 0);
- mimot_motor1_clamp (BOT_CLAMP_ZERO_SPEED, 0);
- asserv_motor1_zero_position (-BOT_GATE_SPEED);
- return ai_next (LOADER_WAIT_JACK_OUT, jack_removed_from_bot);
-}
-
-/*
- * LOADER_INIT_ELEVATOR_ZERO =elevator_succeed=>
- * => LOADER_INIT_CLAMP_CLOSE
- */
-fsm_branch_t
-ai__LOADER_INIT_ELEVATOR_ZERO__elevator_succeed (void)
-{
- return ai_next (LOADER_INIT_ELEVATOR_ZERO, elevator_succeed);
-}
-
-/*
- * LOADER_INIT_CLAMP_CLOSE =clamp_succeed=>
- * => LOADER_INIT_CLAMP_ZERO
- * move elevator up
- * find clamp zero
- */
-fsm_branch_t
-ai__LOADER_INIT_CLAMP_CLOSE__clamp_succeed (void)
-{
- asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP,
- BOT_ELEVATOR_ZERO_SPEED);
- mimot_motor0_zero_position (-BOT_CLAMP_ZERO_SPEED);
- mimot_motor1_zero_position (-BOT_CLAMP_ZERO_SPEED);
- return ai_next (LOADER_INIT_CLAMP_CLOSE, clamp_succeed);
-}
-
-/*
- * LOADER_INIT_CLAMP_ZERO =clamp_succeed=>
- * => LOADER_INIT_GATE_ZERO
- */
-fsm_branch_t
-ai__LOADER_INIT_CLAMP_ZERO__clamp_succeed (void)
-{
- return ai_next (LOADER_INIT_CLAMP_ZERO, clamp_succeed);
-}
-
-/*
- * LOADER_INIT_GATE_ZERO =gate_succeed=>
- * => LOADER_INIT_ELEVATOR_UP
- */
-fsm_branch_t
-ai__LOADER_INIT_GATE_ZERO__gate_succeed (void)
-{
- return ai_next (LOADER_INIT_GATE_ZERO, gate_succeed);
-}
-
-/*
- * LOADER_INIT_ELEVATOR_UP =elevator_succeed=>
- * => LOADER_INIT_GATE_WAIT
- */
-fsm_branch_t
-ai__LOADER_INIT_ELEVATOR_UP__elevator_succeed (void)
-{
- return ai_next (LOADER_INIT_ELEVATOR_UP, elevator_succeed);
-}
-
-/*
- * LOADER_INIT_ELEVATOR_UP =elevator_failed=>
- * => LOADER_IDLE
- * initialisation failure
- */
-fsm_branch_t
-ai__LOADER_INIT_ELEVATOR_UP__elevator_failed (void)
-{
- /* Move so that the operator notice it. */
- asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP / 3,
- BOT_ELEVATOR_ZERO_SPEED);
- return ai_next (LOADER_INIT_ELEVATOR_UP, elevator_failed);
-}
-
-/*
- * LOADER_INIT_GATE_WAIT =state_timeout=>
- * => LOADER_INIT_GATE_CLOSE
- * close gate
- */
-fsm_branch_t
-ai__LOADER_INIT_GATE_WAIT__state_timeout (void)
-{
- asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED);
- return ai_next (LOADER_INIT_GATE_WAIT, state_timeout);
-}
-
-/*
- * LOADER_INIT_GATE_CLOSE =gate_succeed=>
- * => LOADER_UP
- */
-fsm_branch_t
-ai__LOADER_INIT_GATE_CLOSE__gate_succeed (void)
-{
- return ai_next (LOADER_INIT_GATE_CLOSE, gate_succeed);
-}
-
-/*
- * LOADER_UP =loader_down=>
- * => LOADER_DOWNING
- * move down
- */
-fsm_branch_t
-ai__LOADER_UP__loader_down (void)
-{
- asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED);
- return ai_next (LOADER_UP, loader_down);
-}
-
-/*
- * LOADER_DOWN =loader_up=>
- * => LOADER_UPING
- * move up
- */
-fsm_branch_t
-ai__LOADER_DOWN__loader_up (void)
-{
- asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED);
- return ai_next (LOADER_DOWN, loader_up);
-}
-
-/*
- * LOADER_DOWN =loader_element=>
- * => LOADER_LOAD_CLOSING
- * clamp
- */
-fsm_branch_t
-ai__LOADER_DOWN__loader_element (void)
-{
- mimot_motor0_clamp (BOT_CLAMP_SPEED, BOT_CLAMP_PWM);
- mimot_motor1_clamp (BOT_CLAMP_SPEED, BOT_CLAMP_PWM);
- return ai_next (LOADER_DOWN, loader_element);
-}
-
-/*
- * LOADER_UPING =elevator_succeed=>
- * => LOADER_UP
- * post loader_uped event
- */
-fsm_branch_t
-ai__LOADER_UPING__elevator_succeed (void)
-{
- main_post_event (AI_EVENT_loader_uped);
- return ai_next (LOADER_UPING, elevator_succeed);
-}
-
-/*
- * LOADER_UPING =elevator_failed=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_UPING__elevator_failed (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_UPING, elevator_failed);
-}
-
-/*
- * LOADER_UPING =loader_down=>
- * => LOADER_DOWNING
- * move down
- */
-fsm_branch_t
-ai__LOADER_UPING__loader_down (void)
-{
- asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED);
- return ai_next (LOADER_UPING, loader_down);
-}
-
-/*
- * LOADER_DOWNING =elevator_succeed=>
- * => LOADER_DOWN
- * release elevator motor
- * post loader_downed event
- */
-fsm_branch_t
-ai__LOADER_DOWNING__elevator_succeed (void)
-{
- asserv_motor0_free ();
- main_post_event (AI_EVENT_loader_downed);
- return ai_next (LOADER_DOWNING, elevator_succeed);
-}
-
-/*
- * LOADER_DOWNING =elevator_failed=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_DOWNING__elevator_failed (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_DOWNING, elevator_failed);
-}
-
-/*
- * LOADER_DOWNING =loader_element=>
- * => LOADER_ERROR
- * emergency stop
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_DOWNING__loader_element (void)
-{
- asserv_motor0_free ();
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_DOWNING, loader_element);
-}
-
-/*
- * LOADER_DOWNING =loader_up=>
- * => LOADER_UPING
- * move up
- */
-fsm_branch_t
-ai__LOADER_DOWNING__loader_up (void)
-{
- asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED);
- return ai_next (LOADER_DOWNING, loader_up);
-}
-
-/*
- * LOADER_ERROR =loader_down=>
- * => LOADER_ERROR_DOWNING
- * move down
- */
-fsm_branch_t
-ai__LOADER_ERROR__loader_down (void)
-{
- asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED);
- return ai_next (LOADER_ERROR, loader_down);
-}
-
-/*
- * LOADER_ERROR =loader_up=>
- * => LOADER_ERROR_UPING
- * move up
- */
-fsm_branch_t
-ai__LOADER_ERROR__loader_up (void)
-{
- asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP, BOT_ELEVATOR_SPEED);
- return ai_next (LOADER_ERROR, loader_up);
-}
-
-/*
- * LOADER_ERROR_DOWNING =elevator_succeed=>
- * => LOADER_ERROR_DOWNING_OPEN
- * release elevator motor
- * open clamp
- */
-fsm_branch_t
-ai__LOADER_ERROR_DOWNING__elevator_succeed (void)
-{
- asserv_motor0_free ();
- mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- return ai_next (LOADER_ERROR_DOWNING, elevator_succeed);
-}
-
-/*
- * LOADER_ERROR_DOWNING =elevator_failed=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_ERROR_DOWNING__elevator_failed (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_ERROR_DOWNING, elevator_failed);
-}
-
-/*
- * LOADER_ERROR_DOWNING =state_timeout=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_ERROR_DOWNING__state_timeout (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_ERROR_DOWNING, state_timeout);
-}
-
-/*
- * LOADER_ERROR_DOWNING_OPEN =clamp_succeed=>
- * => LOADER_DOWN
- * post loader_downed event
- */
-fsm_branch_t
-ai__LOADER_ERROR_DOWNING_OPEN__clamp_succeed (void)
-{
- main_post_event (AI_EVENT_loader_downed);
- return ai_next (LOADER_ERROR_DOWNING_OPEN, clamp_succeed);
-}
-
-/*
- * LOADER_ERROR_DOWNING_OPEN =clamp_failed=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_ERROR_DOWNING_OPEN__clamp_failed (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_ERROR_DOWNING_OPEN, clamp_failed);
-}
-
-/*
- * LOADER_ERROR_DOWNING_OPEN =state_timeout=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_ERROR_DOWNING_OPEN__state_timeout (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_ERROR_DOWNING_OPEN, state_timeout);
-}
-
-/*
- * LOADER_ERROR_UPING =elevator_succeed=>
- * => LOADER_ERROR_UPING_OPEN
- * open clamp
- */
-fsm_branch_t
-ai__LOADER_ERROR_UPING__elevator_succeed (void)
-{
- mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- return ai_next (LOADER_ERROR_UPING, elevator_succeed);
-}
-
-/*
- * LOADER_ERROR_UPING =elevator_failed=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_ERROR_UPING__elevator_failed (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_ERROR_UPING, elevator_failed);
-}
-
-/*
- * LOADER_ERROR_UPING =state_timeout=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_ERROR_UPING__state_timeout (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_ERROR_UPING, state_timeout);
-}
-
-/*
- * LOADER_ERROR_UPING_OPEN =clamp_succeed=>
- * => LOADER_UP
- * post loader_uped event
- */
-fsm_branch_t
-ai__LOADER_ERROR_UPING_OPEN__clamp_succeed (void)
-{
- main_post_event (AI_EVENT_loader_uped);
- return ai_next (LOADER_ERROR_UPING_OPEN, clamp_succeed);
-}
-
-/*
- * LOADER_ERROR_UPING_OPEN =clamp_failed=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_ERROR_UPING_OPEN__clamp_failed (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_ERROR_UPING_OPEN, clamp_failed);
-}
-
-/*
- * LOADER_ERROR_UPING_OPEN =state_timeout=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_ERROR_UPING_OPEN__state_timeout (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_ERROR_UPING_OPEN, state_timeout);
-}
-
-/*
- * LOADER_LOAD_CLOSING =clamp_succeed=>
- * full => LOADER_LOAD_UPING
- * move up
- * count one element
- * empty => LOADER_LOAD_EMPTY_OPEN
- * open clamp
- */
-fsm_branch_t
-ai__LOADER_LOAD_CLOSING__clamp_succeed (void)
-{
- /* Measure load using clamp position. */
- uint16_t tickness = BOT_CLAMP_WIDTH_STEP
- - mimot_get_motor0_position ()
- - mimot_get_motor1_position ();
- if (tickness > BOT_CLAMP_EMPTY_STEP)
- {
- asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP,
- BOT_ELEVATOR_SPEED);
- position_t robot_position;
- asserv_get_position (&robot_position);
- food_taken (robot_position);
- loader_elements++;
- return ai_next_branch (LOADER_LOAD_CLOSING, clamp_succeed, full);
- }
- else
- {
- mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- return ai_next_branch (LOADER_LOAD_CLOSING, clamp_succeed, empty);
- }
-}
-
-/*
- * LOADER_LOAD_UPING =elevator_unload_position=>
- * => LOADER_LOAD_UNLOADING
- * open clamp
- */
-fsm_branch_t
-ai__LOADER_LOAD_UPING__elevator_unload_position (void)
-{
- mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- return ai_next (LOADER_LOAD_UPING, elevator_unload_position);
-}
-
-/*
- * LOADER_LOAD_UPING =elevator_failed=>
- * => LOADER_ERROR
- * post loader_errored or loader_black event
- * remove one element
- * open clamp
- */
-fsm_branch_t
-ai__LOADER_LOAD_UPING__elevator_failed (void)
-{
- if (asserv_get_motor0_position () < BOT_ELEVATOR_BLACK_THRESHOLD_STEP)
- {
- if (loader_elements)
- loader_elements--;
- main_post_event (AI_EVENT_loader_black);
- }
- else
- main_post_event (AI_EVENT_loader_errored);
- mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- return ai_next (LOADER_LOAD_UPING, elevator_failed);
-}
-
-/*
- * LOADER_LOAD_UNLOADING =elevator_succeed=>
- * => LOADER_LOAD_UNLOADING_OPEN
- */
-fsm_branch_t
-ai__LOADER_LOAD_UNLOADING__elevator_succeed (void)
-{
- return ai_next (LOADER_LOAD_UNLOADING, elevator_succeed);
-}
-
-/*
- * LOADER_LOAD_UNLOADING =elevator_failed=>
- * => LOADER_ERROR
- * post loader_errored event
- * open clamp
- */
-fsm_branch_t
-ai__LOADER_LOAD_UNLOADING__elevator_failed (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- return ai_next (LOADER_LOAD_UNLOADING, elevator_failed);
-}
-
-/*
- * LOADER_LOAD_UNLOADING_OPEN =clamp_succeed=>
- * down => LOADER_DOWNING
- * move down
- * up => LOADER_UPING
- * move up
- */
-fsm_branch_t
-ai__LOADER_LOAD_UNLOADING_OPEN__clamp_succeed (void)
-{
- if (loader_want_up)
- {
- asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP,
- BOT_ELEVATOR_SPEED);
- return ai_next_branch (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed, up);
- }
- else
- {
- asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP,
- BOT_ELEVATOR_SPEED);
- return ai_next_branch (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed, down);
- }
-}
-
-/*
- * LOADER_LOAD_UNLOADING_OPEN =clamp_failed=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_LOAD_UNLOADING_OPEN__clamp_failed (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_LOAD_UNLOADING_OPEN, clamp_failed);
-}
-
-/*
- * LOADER_LOAD_EMPTY_OPEN =clamp_succeed=>
- * down => LOADER_DOWN
- * post loader_downed event
- * up => LOADER_UPING
- * move up
- */
-fsm_branch_t
-ai__LOADER_LOAD_EMPTY_OPEN__clamp_succeed (void)
-{
- if (loader_want_up)
- {
- asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP,
- BOT_ELEVATOR_SPEED);
- return ai_next_branch (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, up);
- }
- else
- {
- main_post_event (AI_EVENT_loader_downed);
- return ai_next_branch (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, down);
- }
-}
-
-/*
- * LOADER_LOAD_EMPTY_OPEN =clamp_failed=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_LOAD_EMPTY_OPEN__clamp_failed (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_LOAD_EMPTY_OPEN, clamp_failed);
-}
-
-
diff --git a/digital/io/src/ai_move_cb.c b/digital/io/src/ai_move_cb.c
deleted file mode 100644
index 32cdd221..00000000
--- a/digital/io/src/ai_move_cb.c
+++ /dev/null
@@ -1,648 +0,0 @@
-/* move_cb.c */
-/* {{{
- *
- * Copyright (C) 2008 Nélio Laranjeiro
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}}
- * move:
- * move with avoid obstacle.
- */
-#include "common.h"
-#include "fsm.h"
-#include "ai_cb.h"
-
-#include "asserv.h"
-#include "playground.h"
-#include "move.h"
-#include "bot.h"
-#include "radar.h"
-#include "events.h"
-#include "loader.h"
-
-#include "main.h" /* main_post_event_for_top_fsm */
-#include "modules/math/fixed/fixed.h" /* fixed_* */
-#include "modules/path/path.h"
-#include "modules/utils/utils.h"
-#include "modules/trace/trace.h"
-
-#include "debug.host.h"
-
-#include <math.h>
-
-#define MOVE_LOADER_UNBLOCKING_DISTANCE 70
-
-/** Used to define a grid. If robot is outside the grid, be careful. */
-#define MOVE_GRID_X 450
-#define MOVE_GRID_X_ORIGIN 150
-#define MOVE_GRID_Y 250
-#define MOVE_GRID_Y_ORIGIN 128
-#define MOVE_GRID_MARGIN 50
-
-/** Go to current step, low level function. */
-static void
-move_go (void)
-{
- vect_t dst = move_data.step;
- /* Modify final point if requested. */
- if (move_data.final_move && move_data.shorten)
- {
- /* Compute a vector from destination to robot with lenght
- * 'shorten'. */
- position_t robot_position;
- asserv_get_position (&robot_position);
- vect_t v = robot_position.v;
- vect_sub (&v, &move_data.step);
- int16_t d = vect_norm (&v);
- if (d > move_data.shorten)
- {
- vect_scale_f824 (&v, 0x1000000 / d * move_data.shorten);
- vect_translate (&dst, &v);
- }
- }
- if (move_data.step_with_angle)
- asserv_goto_xya (dst.x, dst.y, move_data.step_angle,
- move_data.step_backward);
- else
- asserv_goto (dst.x, dst.y, move_data.step_backward);
-}
-
-/** Go or rotate toward position, returns 1 for linear move, 2 for angular
- * move. */
-static uint8_t
-move_go_or_rotate (vect_t dst, uint16_t angle, uint8_t with_angle,
- uint8_t backward)
-{
- position_t robot_position;
- /* Remember step. */
- move_data.step = dst;
- move_data.step_angle = angle;
- move_data.step_with_angle = with_angle;
- move_data.step_backward = backward;
- /* Compute angle to destination. */
- asserv_get_position (&robot_position);
- vect_t v = dst; vect_sub (&v, &robot_position.v);
- uint16_t dst_angle = atan2 (v.y, v.x) * ((1l << 16) / (2 * M_PI));
- if (backward & ASSERV_BACKWARD)
- dst_angle += 0x8000;
- if ((backward & ASSERV_REVERT_OK)
- && (dst_angle ^ robot_position.a) & 0x8000)
- dst_angle += 0x8000;
- int16_t diff = dst_angle - robot_position.a;
- /* Move or rotate. */
- if (UTILS_ABS (diff) < 0x1000)
- {
- loader_down ();
- move_go ();
- return 1;
- }
- else
- {
- loader_up ();
- asserv_goto_angle (dst_angle);
- return 2;
- }
-}
-
-/** Go to next position computed by path module, to be called by
- * move_path_init and move_path_next. Returns 1 for linear move, 2 for angular
- * move. */
-static uint8_t
-move_go_to_next (vect_t dst)
-{
- uint8_t r;
- /* If it is not the last position. */
- if (dst.x != move_data.final.v.x || dst.y != move_data.final.v.y)
- {
- /* Not final position. */
- move_data.final_move = 0;
- /* Goto without angle. */
- r = move_go_or_rotate (dst, 0, 0, move_data.backward_movement_allowed
- | (move_data.slow ? ASSERV_REVERT_OK : 0));
- }
- else
- {
- /* Final position. */
- move_data.final_move = 1;
- /* Goto with angle if requested. */
- r = move_go_or_rotate (dst, move_data.final.a, move_data.with_angle,
- move_data.backward_movement_allowed);
- }
- TRACE (TRACE_MOVE__GO_TO, dst.x, dst.y);
- /* Next time, do not use slow. */
- move_data.slow = 0;
- return r;
-}
-
-/** Update and go to first position, return non zero if a path is found, 1 for
- * linear move, 2 for angular move. */
-static uint8_t
-move_path_init (void)
-{
- uint8_t found;
- vect_t dst;
- /* Get the current position */
- position_t current_pos;
- asserv_get_position (&current_pos);
- /* Give the current position of the bot to the path module */
- path_endpoints (current_pos.v, move_data.final.v);
- /* Update the path module */
- move_data.slow = 0;
- path_update ();
- found = path_get_next (&dst);
- /* If not found, try to escape. */
- if (!found)
- {
- move_data.slow = 1;
- path_escape (8);
- path_update ();
- found = path_get_next (&dst);
- }
- /* If not on the grid, slow down. */
- int16_t mx = current_pos.v.x % MOVE_GRID_X;
- int16_t my = current_pos.v.y % MOVE_GRID_Y;
- if (mx < MOVE_GRID_X_ORIGIN - MOVE_GRID_MARGIN
- || mx > MOVE_GRID_X_ORIGIN + MOVE_GRID_MARGIN
- || my < MOVE_GRID_Y_ORIGIN - MOVE_GRID_MARGIN
- || my > MOVE_GRID_Y_ORIGIN + MOVE_GRID_MARGIN
- )
- move_data.slow = 1;
- /* If found, go. */
- if (found)
- {
- return move_go_to_next (dst);
- }
- else
- {
- /* Error, not final move. */
- move_data.final_move = 0;
- return 0;
- }
-}
-
-/** Go to next position in path. Returns 1 for linear move, 2 for angular
- * move. */
-static uint8_t
-move_path_next (void)
-{
- vect_t dst;
- path_get_next (&dst);
- return move_go_to_next (dst);
-}
-
-/*
- * MOVE_IDLE =move_start=>
- * path_found_rotate => MOVE_ROTATING
- * rotate towards next position.
- * path_found => MOVE_MOVING
- * move to next position.
- * no_path_found => MOVE_IDLE
- * post failure event.
- */
-fsm_branch_t
-ai__MOVE_IDLE__move_start (void)
-{
- uint8_t next = move_path_init ();
- if (next)
- {
- if (next == 2)
- return ai_next_branch (MOVE_IDLE, move_start, path_found_rotate);
- else
- return ai_next_branch (MOVE_IDLE, move_start, path_found);
- }
- else
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_IDLE, move_start, no_path_found);
- }
-}
-
-/*
- * MOVE_ROTATING =bot_move_succeed=>
- * => MOVE_MOVING
- * move to next position.
- */
-fsm_branch_t
-ai__MOVE_ROTATING__bot_move_succeed (void)
-{
- loader_down ();
- move_go ();
- return ai_next (MOVE_ROTATING, bot_move_succeed);
-}
-
-/*
- * MOVE_ROTATING =bot_move_failed=>
- * => MOVE_MOVING
- * move to next position.
- */
-fsm_branch_t
-ai__MOVE_ROTATING__bot_move_failed (void)
-{
- ai__MOVE_ROTATING__bot_move_succeed ();
- return ai_next (MOVE_ROTATING, bot_move_failed);
-}
-
-/*
- * MOVE_ROTATING =state_timeout=>
- * => MOVE_MOVING
- * move to next position.
- */
-fsm_branch_t
-ai__MOVE_ROTATING__state_timeout (void)
-{
- ai__MOVE_ROTATING__bot_move_succeed ();
- return ai_next (MOVE_ROTATING, state_timeout);
-}
-
-/*
- * MOVE_ROTATING =loader_errored=>
- * => MOVE_LOADER_UNBLOCKING_UPING
- * move backward
- * loader up
- */
-fsm_branch_t
-ai__MOVE_ROTATING__loader_errored (void)
-{
- asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
- loader_up ();
- return ai_next (MOVE_ROTATING, loader_errored);
-}
-
-/*
- * MOVE_MOVING =bot_move_succeed=>
- * done => MOVE_IDLE
- * post success event.
- * path_found_rotate => MOVE_ROTATING
- * rotate towards next position.
- * path_found => MOVE_MOVING
- * move to next position.
- * no_path_found => MOVE_IDLE
- * post failure event.
- */
-fsm_branch_t
-ai__MOVE_MOVING__bot_move_succeed (void)
-{
- if (move_data.final_move)
- {
- main_post_event (AI_EVENT_move_fsm_succeed);
- return ai_next_branch (MOVE_MOVING, bot_move_succeed, done);
- }
- else
- {
- uint8_t next = move_path_next ();
- if (next == 2)
- return ai_next_branch (MOVE_MOVING, bot_move_succeed, path_found_rotate);
- else
- return ai_next_branch (MOVE_MOVING, bot_move_succeed, path_found);
- }
- //return ai_next_branch (MOVE_MOVING, bot_move_succeed, no_path_found);
-}
-
-/*
- * MOVE_MOVING =bot_move_failed=>
- * => MOVE_MOVING_BACKWARD_TO_TURN_FREELY
- * reset final_move.
- * move backward to turn freely.
- */
-fsm_branch_t
-ai__MOVE_MOVING__bot_move_failed (void)
-{
- move_data.final_move = 0;
- /* Assume there is an obstacle in front of the robot. */
- position_t robot_pos;
- asserv_get_position (&robot_pos);
- vect_t obstacle_pos;
- int16_t dist = asserv_get_last_moving_direction () == 1
- ? BOT_SIZE_FRONT + MOVE_REAL_OBSTACLE_RADIUS
- : -(BOT_SIZE_BACK + MOVE_REAL_OBSTACLE_RADIUS);
- vect_from_polar_uf016 (&obstacle_pos, dist, robot_pos.a);
- vect_translate (&obstacle_pos, &robot_pos.v);
- path_obstacle (0, obstacle_pos, MOVE_OBSTACLE_RADIUS, 0,
- MOVE_OBSTACLE_VALIDITY);
- /* Move backward to turn freely. */
- asserv_move_linearly (asserv_get_last_moving_direction () == 1 ?
- - 300 : 300);
- return ai_next (MOVE_MOVING, bot_move_failed);
-}
-
-/*
- * MOVE_MOVING =state_timeout=>
- * => MOVE_MOVING_BACKWARD_TO_TURN_FREELY
- * reset final_move.
- * move backward to turn freely.
- */
-fsm_branch_t
-ai__MOVE_MOVING__state_timeout (void)
-{
- ai__MOVE_MOVING__bot_move_failed ();
- return ai_next (MOVE_MOVING, state_timeout);
-}
-
-/*
- * MOVE_MOVING =obstacle_in_front=>
- * tryagain => MOVE_WAIT_FOR_CLEAR_PATH
- * reset final_move.
- * stop the bot.
- * tryout => MOVE_IDLE
- * stop the bot.
- * post failure event.
- */
-fsm_branch_t
-ai__MOVE_MOVING__obstacle_in_front (void)
-{
- move_data.final_move = 0;
- asserv_stop_motor ();
- if (--move_data.try_again_counter == 0)
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_MOVING, obstacle_in_front, tryout);
- }
- else
- return ai_next_branch (MOVE_MOVING, obstacle_in_front, tryagain);
-}
-
-/*
- * MOVE_MOVING =loader_errored=>
- * => MOVE_LOADER_UNBLOCKING_UPING
- * move backward
- * loader up
- */
-fsm_branch_t
-ai__MOVE_MOVING__loader_errored (void)
-{
- asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
- loader_up ();
- return ai_next (MOVE_MOVING, loader_errored);
-}
-
-/*
- * MOVE_MOVING_BACKWARD_TO_TURN_FREELY =bot_move_succeed=>
- * tryout => MOVE_IDLE
- * post failure event.
- * path_found_rotate => MOVE_ROTATING
- * rotate towards next position.
- * path_found => MOVE_MOVING
- * move to next position.
- * no_path_found => MOVE_IDLE
- * post failure event.
- */
-fsm_branch_t
-ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_succeed (void)
-{
- if (--move_data.try_again_counter == 0)
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, tryout);
- }
- else
- {
- uint8_t next = move_path_init ();
- if (next)
- {
- if (next == 2)
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, path_found_rotate);
- else
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, path_found);
- }
- else
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, no_path_found);
- }
- }
-}
-
-/*
- * MOVE_MOVING_BACKWARD_TO_TURN_FREELY =bot_move_failed=>
- * tryout => MOVE_IDLE
- * post failure event.
- * path_found_rotate => MOVE_ROTATING
- * rotate towards next position.
- * path_found => MOVE_MOVING
- * move to next position.
- * no_path_found_tryagain => MOVE_WAIT_FOR_CLEAR_PATH
- * nothing to do.
- * no_path_found_tryout => MOVE_IDLE
- * post failure event.
- */
-fsm_branch_t
-ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_failed (void)
-{
- if (--move_data.try_again_counter == 0)
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, tryout);
- }
- else
- {
- uint8_t next = move_path_init ();
- if (next)
- {
- if (next == 2)
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, path_found_rotate);
- else
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, path_found);
- }
- else
- {
- if (--move_data.try_again_counter == 0)
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_path_found_tryout);
- }
- else
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_path_found_tryagain);
- }
- }
-}
-
-/*
- * MOVE_WAIT_FOR_CLEAR_PATH =state_timeout=>
- * path_found_rotate => MOVE_ROTATING
- * rotate towards next position.
- * path_found => MOVE_MOVING
- * move to next position.
- * no_path_found_tryagain => MOVE_WAIT_FOR_CLEAR_PATH
- * decrement counter.
- * no_path_found_tryout => MOVE_IDLE
- * post failure.
- */
-fsm_branch_t
-ai__MOVE_WAIT_FOR_CLEAR_PATH__state_timeout (void)
-{
- /* Try to move. */
- uint8_t next = move_path_init ();
- if (next)
- {
- if (next == 2)
- return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, path_found_rotate);
- else
- return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, path_found);
- }
- else
- {
- /* Error, no new position, should we try again? */
- if (--move_data.try_again_counter == 0)
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, no_path_found_tryout);
- }
- else
- return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, no_path_found_tryagain);
- }
-}
-
-/*
- * MOVE_LOADER_UNBLOCKING_UPING =bot_move_succeed=>
- * => MOVE_LOADER_UNBLOCKING_DOWNING
- * loader down
- * reset unblocking retry counter
- */
-fsm_branch_t
-ai__MOVE_LOADER_UNBLOCKING_UPING__bot_move_succeed (void)
-{
- loader_down ();
- move_data.loader_unblocking_retry = 2;
- return ai_next (MOVE_LOADER_UNBLOCKING_UPING, bot_move_succeed);
-}
-
-/*
- * MOVE_LOADER_UNBLOCKING_UPING =bot_move_failed=>
- * => MOVE_LOADER_UNBLOCKING_DOWNING
- * loader down
- * reset unblocking retry counter
- */
-fsm_branch_t
-ai__MOVE_LOADER_UNBLOCKING_UPING__bot_move_failed (void)
-{
- loader_down ();
- move_data.loader_unblocking_retry = 2;
- return ai_next (MOVE_LOADER_UNBLOCKING_UPING, bot_move_failed);
-}
-
-/*
- * MOVE_LOADER_UNBLOCKING_DOWNING =loader_downed=>
- * path_found_rotate => MOVE_ROTATING
- * rotate towards next position.
- * path_found => MOVE_MOVING
- * move to next position.
- * no_path_found => MOVE_IDLE
- * post failure.
- */
-fsm_branch_t
-ai__MOVE_LOADER_UNBLOCKING_DOWNING__loader_downed (void)
-{
- /* Try to move. */
- uint8_t next = move_path_init ();
- if (next)
- {
- if (next == 2)
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, path_found_rotate);
- else
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, path_found);
- }
- else
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, no_path_found);
- }
-}
-
-/*
- * MOVE_LOADER_UNBLOCKING_DOWNING =loader_errored=>
- * tryagain => MOVE_LOADER_UNBLOCKING_UPING
- * move backward
- * loader up
- * tryout_path_found_rotate => MOVE_ROTATING
- * rotate towards next position.
- * tryout_path_found => MOVE_ROTATING
- * move to next position.
- * tryout_no_path_found => MOVE_IDLE
- * post failure.
- */
-fsm_branch_t
-ai__MOVE_LOADER_UNBLOCKING_DOWNING__loader_errored (void)
-{
- if (--move_data.loader_unblocking_retry)
- {
- asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
- loader_up ();
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryagain);
- }
- else
- {
- /* Try to move. */
- uint8_t next = move_path_init ();
- if (next)
- {
- if (next == 2)
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_path_found_rotate);
- else
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_path_found);
- }
- else
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_no_path_found);
- }
- }
-}
-
-/*
- * MOVE_LOADER_UNBLOCKING_DOWNING =state_timeout=>
- * tryagain => MOVE_LOADER_UNBLOCKING_UPING
- * move backward
- * loader up
- * tryout_path_found_rotate => MOVE_ROTATING
- * rotate towards next position.
- * tryout_path_found => MOVE_ROTATING
- * move to next position.
- * tryout_no_path_found => MOVE_IDLE
- * post failure.
- */
-fsm_branch_t
-ai__MOVE_LOADER_UNBLOCKING_DOWNING__state_timeout (void)
-{
- if (--move_data.loader_unblocking_retry)
- {
- asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
- loader_up ();
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, state_timeout, tryagain);
- }
- else
- {
- /* Try to move. */
- uint8_t next = move_path_init ();
- if (next)
- {
- if (next == 2)
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, state_timeout, tryout_path_found_rotate);
- else
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, state_timeout, tryout_path_found);
- }
- else
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, state_timeout, tryout_no_path_found);
- }
- }
-}
-
-
diff --git a/digital/io/src/ai_top_cb.c b/digital/io/src/ai_top_cb.c
deleted file mode 100644
index ed1ae038..00000000
--- a/digital/io/src/ai_top_cb.c
+++ /dev/null
@@ -1,434 +0,0 @@
-/* ai_top_cb.c */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
- *
- * Copyright (C) 2010 Nicolas Schodet
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}}
- * Main FSM calling other FSM.
- */
-#include "common.h"
-#include "fsm.h"
-#include "ai_cb.h"
-#include "top.h"
-#include "move.h"
-#include "chrono.h"
-#include "playground.h"
-#include "asserv.h"
-#include "loader.h"
-#include "food.h"
-
-/*
- * IDLE =start=>
- * => WAIT_INIT_TO_FINISH
- * nothing to do.
- */
-fsm_branch_t
-ai__IDLE__start (void)
-{
- return ai_next (IDLE, start);
-}
-
-/*
- * WAIT_INIT_TO_FINISH =init_match_is_started=>
- * => FIRST_GO_BEGIN_OF_LINE_FAST
- * the match start
- * set fast speed
- * go to first sequence begin of line
- */
-fsm_branch_t
-ai__WAIT_INIT_TO_FINISH__init_match_is_started (void)
-{
- asserv_set_speed (BOT_MOVE_FAST);
- move_start_noangle (PG_VECT (375, 1503), 0, 0);
- return ai_next (WAIT_INIT_TO_FINISH, init_match_is_started);
-}
-
-/*
- * FIRST_GO_BEGIN_OF_LINE_FAST =move_fsm_succeed=>
- * => FIRST_GO_END_OF_LINE_FAST
- * go to end of line
- * loader down
- */
-fsm_branch_t
-ai__FIRST_GO_BEGIN_OF_LINE_FAST__move_fsm_succeed (void)
-{
- move_start_noangle (PG_VECT (2625, 253), 0, 0);
- loader_down ();
- return ai_next (FIRST_GO_BEGIN_OF_LINE_FAST, move_fsm_succeed);
-}
-
-/*
- * FIRST_GO_BEGIN_OF_LINE_FAST =move_fsm_failed=>
- * => FIRST_GO_BEGIN_OF_LINE_FAST
- * set slow speed
- * retry
- */
-fsm_branch_t
-ai__FIRST_GO_BEGIN_OF_LINE_FAST__move_fsm_failed (void)
-{
- asserv_set_speed (BOT_MOVE_SLOW);
- move_start_noangle (PG_VECT (375, 1503), 0, 0);
- return ai_next (FIRST_GO_BEGIN_OF_LINE_FAST, move_fsm_failed);
-}
-
-/*
- * FIRST_GO_END_OF_LINE_FAST =in_field=>
- * => FIRST_GO_END_OF_LINE_SLOW
- * set slow speed
- */
-fsm_branch_t
-ai__FIRST_GO_END_OF_LINE_FAST__in_field (void)
-{
- asserv_set_speed (BOT_MOVE_SLOW);
- return ai_next (FIRST_GO_END_OF_LINE_FAST, in_field);
-}
-
-/*
- * FIRST_GO_END_OF_LINE_FAST =move_fsm_succeed=>
- * => UNLOAD_LOADER_UP
- * set slow speed
- * move loader up
- */
-fsm_branch_t
-ai__FIRST_GO_END_OF_LINE_FAST__move_fsm_succeed (void)
-{
- asserv_set_speed (BOT_MOVE_SLOW);
- ai__FIRST_GO_END_OF_LINE_SLOW__move_fsm_succeed ();
- return ai_next (FIRST_GO_END_OF_LINE_FAST, move_fsm_succeed);
-}
-
-/*
- * FIRST_GO_END_OF_LINE_FAST =move_fsm_failed=>
- * => FIRST_GO_END_OF_LINE_SLOW
- * set slow speed
- * retry
- */
-fsm_branch_t
-ai__FIRST_GO_END_OF_LINE_FAST__move_fsm_failed (void)
-{
- asserv_set_speed (BOT_MOVE_SLOW);
- ai__FIRST_GO_END_OF_LINE_SLOW__move_fsm_failed ();
- return ai_next (FIRST_GO_END_OF_LINE_FAST, move_fsm_failed);
-}
-
-/*
- * FIRST_GO_END_OF_LINE_SLOW =move_fsm_succeed=>
- * => UNLOAD_LOADER_UP
- * move loader up
- */
-fsm_branch_t
-ai__FIRST_GO_END_OF_LINE_SLOW__move_fsm_succeed (void)
-{
- loader_up ();
- return ai_next (FIRST_GO_END_OF_LINE_SLOW, move_fsm_succeed);
-}
-
-/*
- * FIRST_GO_END_OF_LINE_SLOW =move_fsm_failed=>
- * => FIRST_GO_END_OF_LINE_SLOW
- * retry
- */
-fsm_branch_t
-ai__FIRST_GO_END_OF_LINE_SLOW__move_fsm_failed (void)
-{
- move_start_noangle (PG_VECT (2625, 253), 0, 0);
- return ai_next (FIRST_GO_END_OF_LINE_SLOW, move_fsm_failed);
-}
-
-/*
- * UNLOAD =move_fsm_succeed=>
- * => UNLOAD_LOADER_UP
- * move loader up
- */
-fsm_branch_t
-ai__UNLOAD__move_fsm_succeed (void)
-{
- loader_up ();
- return ai_next (UNLOAD, move_fsm_succeed);
-}
-
-/*
- * UNLOAD =move_fsm_failed=>
- * => UNLOAD
- * retry
- */
-fsm_branch_t
-ai__UNLOAD__move_fsm_failed (void)
-{
- move_start_noangle (PG_VECT (2625, 253), 0, 0);
- return ai_next (UNLOAD, move_fsm_failed);
-}
-
-/*
- * UNLOAD_LOADER_UP =loader_uped=>
- * => UNLOAD_FACE_BIN
- * turn toward bin
- */
-fsm_branch_t
-ai__UNLOAD_LOADER_UP__loader_uped (void)
-{
- asserv_goto_angle (PG_A_DEG (90));
- return ai_next (UNLOAD_LOADER_UP, loader_uped);
-}
-
-/*
- * UNLOAD_LOADER_UP =loader_errored=>
- * => UNLOAD_FACE_BIN
- * turn toward bin
- */
-fsm_branch_t
-ai__UNLOAD_LOADER_UP__loader_errored (void)
-{
- asserv_goto_angle (PG_A_DEG (90));
- return ai_next (UNLOAD_LOADER_UP, loader_errored);
-}
-
-/*
- * UNLOAD_FACE_BIN =bot_move_succeed=>
- * => UNLOAD_BACK_BIN
- * go backward to bin
- */
-fsm_branch_t
-ai__UNLOAD_FACE_BIN__bot_move_succeed (void)
-{
- asserv_move_linearly (-(128 + 250 / 2 - BOT_SIZE_BACK - 50));
- return ai_next (UNLOAD_FACE_BIN, bot_move_succeed);
-}
-
-/*
- * UNLOAD_FACE_BIN =bot_move_failed=>
- * => UNLOAD_FACE_BIN_UNBLOCK
- * move backward
- */
-fsm_branch_t
-ai__UNLOAD_FACE_BIN__bot_move_failed (void)
-{
- asserv_move_linearly (-40);
- return ai_next (UNLOAD_FACE_BIN, bot_move_failed);
-}
-
-/*
- * UNLOAD_FACE_BIN_UNBLOCK =bot_move_succeed=>
- * => UNLOAD_FACE_BIN
- * turn toward bin
- */
-fsm_branch_t
-ai__UNLOAD_FACE_BIN_UNBLOCK__bot_move_succeed (void)
-{
- asserv_goto_angle (PG_A_DEG (90));
- return ai_next (UNLOAD_FACE_BIN_UNBLOCK, bot_move_succeed);
-}
-
-/*
- * UNLOAD_FACE_BIN_UNBLOCK =bot_move_failed=>
- * => UNLOAD_FACE_BIN
- * turn toward bin
- */
-fsm_branch_t
-ai__UNLOAD_FACE_BIN_UNBLOCK__bot_move_failed (void)
-{
- asserv_goto_angle (PG_A_DEG (90));
- return ai_next (UNLOAD_FACE_BIN_UNBLOCK, bot_move_failed);
-}
-
-/*
- * UNLOAD_BACK_BIN =bot_move_succeed=>
- * => UNLOAD_UNLOAD
- * unload
- */
-fsm_branch_t
-ai__UNLOAD_BACK_BIN__bot_move_succeed (void)
-{
- asserv_move_motor1_absolute (0, BOT_GATE_SPEED);
- return ai_next (UNLOAD_BACK_BIN, bot_move_succeed);
-}
-
-/*
- * UNLOAD_BACK_BIN =bot_move_failed=>
- * => UNLOAD_UNLOAD
- * unload
- */
-fsm_branch_t
-ai__UNLOAD_BACK_BIN__bot_move_failed (void)
-{
- ai__UNLOAD_BACK_BIN__bot_move_succeed ();
- return ai_next (UNLOAD_BACK_BIN, bot_move_failed);
-}
-
-/*
- * UNLOAD_UNLOAD =state_timeout=>
- * => COLLECT
- * close gate
- * loader down
- * choose best food to collect
- */
-fsm_branch_t
-ai__UNLOAD_UNLOAD__state_timeout (void)
-{
- loader_elements = 0;
- asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED);
- loader_down ();
- top_collect (1);
- return ai_next (UNLOAD_UNLOAD, state_timeout);
-}
-
-/*
- * COLLECT =move_fsm_succeed=>
- * unload => UNLOAD
- * collect => COLLECT
- * slow_motion => COLLECT_SLOW_MOTION
- * post loader_element event
- */
-fsm_branch_t
-ai__COLLECT__move_fsm_succeed (void)
-{
- int16_t slow_motion = food_slow_motion (top_food);
- if (slow_motion)
- {
- fsm_handle_event (&ai_fsm, AI_EVENT_loader_element);
- return ai_next_branch (COLLECT, move_fsm_succeed, slow_motion);
- }
- else if (top_collect (0))
- return ai_next_branch (COLLECT, move_fsm_succeed, collect);
- else
- return ai_next_branch (COLLECT, move_fsm_succeed, unload);
-}
-
-/*
- * COLLECT =move_fsm_failed=>
- * unload => UNLOAD
- * collect => COLLECT
- * cancel this element
- */
-fsm_branch_t
-ai__COLLECT__move_fsm_failed (void)
-{
- if (top_collect (0))
- {
- food_cancel (top_food);
- return ai_next_branch (COLLECT, move_fsm_failed, collect);
- }
- else
- return ai_next_branch (COLLECT, move_fsm_failed, unload);
-}
-
-/*
- * COLLECT =loader_black=>
- * => COLLECT_BLACK
- * move backward
- * mark as black
- */
-fsm_branch_t
-ai__COLLECT__loader_black (void)
-{
- asserv_move_linearly (-90);
- food_black (top_food);
- return ai_next (COLLECT, loader_black);
-}
-
-/*
- * COLLECT_SLOW_MOTION =loader_downed=>
- * unload => UNLOAD
- * collect => COLLECT
- * collect
- */
-fsm_branch_t
-ai__COLLECT_SLOW_MOTION__loader_downed (void)
-{
- if (top_collect (0))
- return ai_next_branch (COLLECT_SLOW_MOTION, loader_downed, collect);
- else
- return ai_next_branch (COLLECT_SLOW_MOTION, loader_downed, unload);
-}
-
-/*
- * COLLECT_SLOW_MOTION =loader_errored=>
- * unload => UNLOAD
- * collect => COLLECT
- * same as above
- */
-fsm_branch_t
-ai__COLLECT_SLOW_MOTION__loader_errored (void)
-{
- if (top_collect (0))
- return ai_next_branch (COLLECT_SLOW_MOTION, loader_errored, collect);
- else
- return ai_next_branch (COLLECT_SLOW_MOTION, loader_errored, unload);
-}
-
-/*
- * COLLECT_SLOW_MOTION =state_timeout=>
- * unload => UNLOAD
- * collect => COLLECT
- * same as above
- */
-fsm_branch_t
-ai__COLLECT_SLOW_MOTION__state_timeout (void)
-{
- if (top_collect (0))
- return ai_next_branch (COLLECT_SLOW_MOTION, state_timeout, collect);
- else
- return ai_next_branch (COLLECT_SLOW_MOTION, state_timeout, unload);
-}
-
-/*
- * COLLECT_SLOW_MOTION =loader_black=>
- * => COLLECT_BLACK
- * move backward
- * mark as black
- */
-fsm_branch_t
-ai__COLLECT_SLOW_MOTION__loader_black (void)
-{
- asserv_move_linearly (-90);
- food_black (top_food);
- return ai_next (COLLECT_SLOW_MOTION, loader_black);
-}
-
-/*
- * COLLECT_BLACK =bot_move_succeed=>
- * unload => UNLOAD
- * collect => COLLECT
- */
-fsm_branch_t
-ai__COLLECT_BLACK__bot_move_succeed (void)
-{
- if (top_collect (0))
- return ai_next_branch (COLLECT_BLACK, bot_move_succeed, collect);
- else
- return ai_next_branch (COLLECT_BLACK, bot_move_succeed, unload);
-}
-
-/*
- * COLLECT_BLACK =bot_move_failed=>
- * unload => UNLOAD
- * collect => COLLECT
- */
-fsm_branch_t
-ai__COLLECT_BLACK__bot_move_failed (void)
-{
- if (top_collect (0))
- return ai_next_branch (COLLECT_BLACK, bot_move_failed, collect);
- else
- return ai_next_branch (COLLECT_BLACK, bot_move_failed, unload);
-}
-
-
diff --git a/digital/io/src/fsm.c b/digital/io/src/fsm.c
deleted file mode 100644
index 45eeb5ba..00000000
--- a/digital/io/src/fsm.c
+++ /dev/null
@@ -1,137 +0,0 @@
-/* fsm.c - Finite State Machine code. */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
- *
- * Copyright (C) 2008 Nicolas Schodet
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}} */
-#include "common.h"
-#include "fsm.h"
-
-#include "modules/trace/trace.h"
-#include "modules/proto/proto.h"
-#include "events.h"
-
-#ifdef HOST
-# include <stdio.h>
-#endif
-
-void
-fsm_init (fsm_t *fsm)
-{
- uint8_t i;
- assert (fsm);
- for (i = 0; i < FSM_ACTIVE_STATES_MAX; i++)
- fsm->states_active[i] = fsm->states_init[i];
- for (i = 0; i < fsm->active_states_nb; i++)
- fsm->states_timeout[i] =
- fsm->state_timeout_table[fsm->states_init[i]];
-}
-
-/** Handle an event on the given FSM on a single active state, return 1 if the
- * event was handled. */
-static uint8_t
-fsm_handle_event_single (fsm_t *fsm, uint8_t active, uint8_t event)
-{
- assert (fsm);
- assert (active < fsm->active_states_nb);
- assert (event < fsm->events_nb);
- /* Lookup transition. */
- uint8_t old_state = fsm->states_active[active];
- fsm_transition_t tr = FSM_TRANSITION (fsm, old_state, event);
- /* Ignore unhandled events. */
- if (tr)
- {
- /* Execute transition. */
- uint8_t new_state;
- fsm_branch_t br = tr ();
- /* Change state. */
-#ifdef HOST
- assert (((br >> 16) & 0xff) == fsm->states_active[active]);
- assert (((br >> 8) & 0xff) == event);
- fprintf (stderr, "%s %s =%s=> %s\n", fsm->name,
- fsm->states_names[fsm->states_active[active]],
- fsm->events_names[event], fsm->states_names[br & 0xff]);
- new_state = br & 0xff;
-#else
- new_state = br;
- proto_send3b ('f', old_state, event, new_state);
-#endif
- TRACE (TRACE_FSM__HANDLE_EVENT, (u8) old_state, (u8) event,
- (u8) new_state);
- fsm->states_active[active] = new_state;
- fsm->states_timeout[active] = fsm->state_timeout_table[new_state];
- return 1;
- }
- return 0;
-}
-
-uint8_t
-fsm_handle_event (fsm_t *fsm, uint8_t event)
-{
- uint8_t i, handled = 0;
- assert (fsm);
- assert (event < fsm->events_nb);
- for (i = 0; i < fsm->active_states_nb; i++)
- {
- /* Handle the event for this active state. */
- handled += fsm_handle_event_single (fsm, i, event);
- }
- return handled;
-}
-
-uint8_t
-fsm_handle_timeout (fsm_t *fsm)
-{
- uint8_t i, handled = 0;
- assert (fsm);
- for (i = 0; i < fsm->active_states_nb; i++)
- {
- /* If there is a timeout for this state. */
- if (fsm->states_timeout[i] != 0xffff)
- {
- if (fsm->states_timeout[i])
- fsm->states_timeout[i]--;
- else
- {
- /* Timeout expired, generate corresponding event. */
- handled += fsm_handle_event_single (fsm, i,
- fsm->state_timeout_event);
- }
- }
- }
- return handled;
-}
-
-uint8_t
-fsm_can_handle_event (fsm_t *fsm, uint8_t event)
-{
- uint8_t i;
- assert (fsm);
- assert (event < fsm->events_nb);
- for (i = 0; i < fsm->active_states_nb; i++)
- {
- uint8_t state = fsm->states_active[i];
- if (FSM_TRANSITION (fsm, state, event))
- return 1;
- }
- return 0;
-}
-
diff --git a/digital/io/src/fsm.h b/digital/io/src/fsm.h
index 2d0e1933..6b376d30 100644
--- a/digital/io/src/fsm.h
+++ b/digital/io/src/fsm.h
@@ -1,126 +1,566 @@
-#ifndef fsm_h
-#define fsm_h
-/* fsm.h - Finite State Machine code. */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
- *
- * Copyright (C) 2008 Nicolas Schodet
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}} */
+/*
+ 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/
-#ifndef HOST
+ This file is part of AngFSM.
-# include <avr/pgmspace.h>
+ 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.
-#else
+ 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/>.
+ */
-# define PROGMEM
-# define pgm_read_word(addr) (*(addr))
+#include <inttypes.h>
+#ifndef _FSM_GEN_
+#define _FSM_GEN_
+
+#ifndef FSM_NAME
+#define FSM_NAME FSM
#endif
-/**
- * The io board includes an FSM with several concurrent active states.
- *
- * The main loop tests for each possible event and generate the corresponding
- * FSM events. For example:
- *
- * if (asserv_move_cmd_status () == success)
- * fsm_handle_event (&ai_fsm, AI_EVENT_position_reached);
- *
- * Any unhandled event will be ignored.
- *
- * To start a sub machine from the top FSM, call the sub machine start
- * function (for example move_start), which will set parameters, then trigger
- * its start event.
+#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
*
- * The sub machine is expected to generate an event for the top machine to
- * make it continue.
+ * 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;
-#ifdef HOST
-/** On host, check branches. */
-typedef u32 fsm_branch_t;
-#else
-/** On AVR, no check. */
-typedef u8 fsm_branch_t;
-#endif
+/* 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;
-/** Transition function. */
-typedef fsm_branch_t (*fsm_transition_t) (void);
+/* Pointer to a transition function. */
+typedef fsm_build_state_t* (*fsm_build_run_strans_func_t)(void);
-/** Maximum number of active states. */
-#define FSM_ACTIVE_STATES_MAX 5
+/* 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;
-/** Context of an FSM. */
-struct fsm_t
+/* Timeout structure. */
+typedef struct fsm_build_timeout_t
{
- /** Transition table. */
- const fsm_transition_t *transition_table;
- /** Number of events, used to index the right transition. */
- u8 events_nb;
- /** Initial states. */
- u8 states_init[FSM_ACTIVE_STATES_MAX];
- /** Current active states. */
- u8 states_active[FSM_ACTIVE_STATES_MAX];
- /** Nomber of active states. */
- u8 active_states_nb;
- /** State timeout table. */
- const u16 *state_timeout_table;
- /** Current active states timeout if not 0xffff. */
- u16 states_timeout[FSM_ACTIVE_STATES_MAX];
- /** Event to generate on state timeout. */
- u8 state_timeout_event;
-#ifdef HOST
- /** FSM name. */
- const char *name;
- /** States names. */
- const char * const *states_names;
- /** Events names. */
- const char * const *events_names;
-#endif
-};
-typedef struct fsm_t fsm_t;
+ 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 FSM_TRANSITION(fsm, state, event) \
- ((fsm_transition_t) pgm_read_word ( \
- &(fsm)->transition_table[(state) * (fsm)->events_nb + (event)]))
+/** 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);\
+}
-/** Reset a FSM. */
+/**
+ * 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_init (fsm_t *fsm);
+fsm_build_init (fsm_build_t *fsm, char *name);
-/** Handle state timeout, return non-zero if at least an event was handled. */
-uint8_t
-fsm_handle_timeout (fsm_t *fsm);
+/** 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 an event on the given FSM, return non-zero if at least an event was
- * handled. */
-uint8_t
-fsm_handle_event (fsm_t *fsm, uint8_t event);
+/**
+ * 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);
-/** Return non zero if event can be handled. This can be used if event
- * generation requires expensive computations. */
-uint8_t
-fsm_can_handle_event (fsm_t *fsm, uint8_t event);
+/** 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);
-#include "ai_fsm.h"
+#endif /* Architectures */
-#endif /* fsm_h */
+#endif /* #ifdef _FSM_GEN_ */
diff --git a/digital/io/src/fsm.host.c b/digital/io/src/fsm.host.c
new file mode 100644
index 00000000..bf8b761d
--- /dev/null
+++ b/digital/io/src/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/io/src/fsm_templates/template_cb.h b/digital/io/src/fsm_templates/template_cb.h
deleted file mode 100644
index 4f4deeec..00000000
--- a/digital/io/src/fsm_templates/template_cb.h
+++ /dev/null
@@ -1,10 +0,0 @@
-#ifndef %(prefix)s_cb_h
-#define %(prefix)s_cb_h
-/*
- * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT!
- *
- * %(name)s callbacks declaration.
- *
-%(*comments)s */
-
-%(states_template,template_cb_decl.h)s#endif /* %(prefix)s_cb_h */
diff --git a/digital/io/src/fsm_templates/template_cb_decl.h b/digital/io/src/fsm_templates/template_cb_decl.h
deleted file mode 100644
index fd72d678..00000000
--- a/digital/io/src/fsm_templates/template_cb_decl.h
+++ /dev/null
@@ -1,6 +0,0 @@
-/*
- * %(state)s =%(event)s=>
-%(*branches_to)s */
-fsm_branch_t
-%(prefix)s__%(state)s__%(event)s (void);
-
diff --git a/digital/io/src/fsm_templates/template_cb_impl.c b/digital/io/src/fsm_templates/template_cb_impl.c
deleted file mode 100644
index 91ff4fbb..00000000
--- a/digital/io/src/fsm_templates/template_cb_impl.c
+++ /dev/null
@@ -1,9 +0,0 @@
-/*
- * %(state)s =%(event)s=>
-%(*branches_to)s */
-fsm_branch_t
-%(prefix)s__%(state)s__%(event)s (void)
-{
-%(returns)s
-}
-
diff --git a/digital/io/src/fsm_templates/template_fsm.c b/digital/io/src/fsm_templates/template_fsm.c
deleted file mode 100644
index 03ce5abf..00000000
--- a/digital/io/src/fsm_templates/template_fsm.c
+++ /dev/null
@@ -1,55 +0,0 @@
-/*
- * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT!
- *
- * %(name)s
- *
-%(*comments)s */
-#include "common.h"
-#include "fsm.h"
-#include "%(prefix)s_cb.h"
-
-#define NULL ((void *)0L)
-
-/* %(name)s transition table. */
-static const fsm_transition_t PROGMEM
-%(prefix)s_transition_table[%(PREFIX)s_STATE_NB][%(PREFIX)s_EVENT_NB] = {
-%(transition_table)s};
-
-/* %(name)s state timeout table. */
-static const uint16_t
-%(prefix)s_state_timeout_table[%(PREFIX)s_STATE_NB] = {
-%(states,%(@timeout|0xffff)s)s};
-
-#ifdef HOST
-
-/* %(name)s state names. */
-static const char *
-%(prefix)s_states_names[] = {
-%(states_names)s};
-
-/* %(name)s event names. */
-static const char *
-%(prefix)s_events_names[] = {
-%(events_names)s};
-
-#endif /* HOST */
-
-/* %(name)s context. */
-fsm_t %(prefix)s_fsm = {
- &%(prefix)s_transition_table[0][0],
- %(PREFIX)s_EVENT_NB,
- {
- %(initials)s},
- {
- %(initials)s},
- %(initials_nb)s,
- &%(prefix)s_state_timeout_table[0],
- { 0 },
- %(PREFIX)s_EVENT_state_timeout,
-#ifdef HOST
- "%(name)s",
- %(prefix)s_states_names,
- %(prefix)s_events_names,
-#endif
-};
-
diff --git a/digital/io/src/fsm_templates/template_fsm.h b/digital/io/src/fsm_templates/template_fsm.h
deleted file mode 100644
index 9ac98895..00000000
--- a/digital/io/src/fsm_templates/template_fsm.h
+++ /dev/null
@@ -1,52 +0,0 @@
-#ifndef %(prefix)s_fsm_h
-#define %(prefix)s_fsm_h
-/*
- * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT!
- *
- * %(name)s
- *
-%(*comments)s */
-
-/* %(name)s states. */
-enum %(prefix)s_state_t
-{
-%(states)s %(PREFIX)s_STATE_NB
-};
-
-/* %(name)s events. */
-enum %(prefix)s_event_t
-{
-%(events)s %(PREFIX)s_EVENT_NB
-};
-
-/* This macro enables checks for branches used in the wrong state/event
- * combination. */
-#ifdef HOST
-# define _BRANCH(state, event, to) \
- ((%(PREFIX)s_STATE_ ## state) << 16 \
- | (%(PREFIX)s_EVENT_ ## event) << 8 \
- | (%(PREFIX)s_STATE_ ## to))
-#else
-# define _BRANCH(state, event, to) \
- ((%(PREFIX)s_STATE_ ## to))
-#endif
-
-/* %(name)s branches. */
-enum %(prefix)s_branch_t
-{
-%(branches)s};
-
-#undef _BRANCH
-
-/* Value to return to follow the only branch. */
-#define %(prefix)s_next(state, event) \
- %(PREFIX)s_BRANCH__ ## state ## __ ## event ## __
-
-/* Value to return to follow a given branch. */
-#define %(prefix)s_next_branch(state, event, branch) \
- %(PREFIX)s_BRANCH__ ## state ## __ ## event ## __ ## branch
-
-/* %(name)s context. */
-extern fsm_t %(prefix)s_fsm;
-
-#endif /* %(prefix)s_fsm_h */
diff --git a/digital/io/src/fsm_templates/template_hola_cb_skel.c b/digital/io/src/fsm_templates/template_hola_cb_skel.c
deleted file mode 100644
index 980718da..00000000
--- a/digital/io/src/fsm_templates/template_hola_cb_skel.c
+++ /dev/null
@@ -1,11 +0,0 @@
-/*
- * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT!
- *
- * Skeleton for %(name)s hola callbacks implementation.
- *
-%(*comments)s */
-#include "common.h"
-#include "fsm.h"
-#include "%(prefix)s_cb.h"
-
-%(states_template,template_cb_impl.c,origin=hola)s
diff --git a/digital/io/src/fsm_templates/template_init_cb_skel.c b/digital/io/src/fsm_templates/template_init_cb_skel.c
deleted file mode 100644
index fea9270e..00000000
--- a/digital/io/src/fsm_templates/template_init_cb_skel.c
+++ /dev/null
@@ -1,11 +0,0 @@
-/*
- * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT!
- *
- * Skeleton for %(name)s init callbacks implementation.
- *
-%(*comments)s */
-#include "common.h"
-#include "fsm.h"
-#include "%(prefix)s_cb.h"
-
-%(states_template,template_cb_impl.c,origin=init)s
diff --git a/digital/io/src/fsm_templates/template_loader_cb_skel.c b/digital/io/src/fsm_templates/template_loader_cb_skel.c
deleted file mode 100644
index 06398952..00000000
--- a/digital/io/src/fsm_templates/template_loader_cb_skel.c
+++ /dev/null
@@ -1,11 +0,0 @@
-/*
- * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT!
- *
- * Skeleton for %(name)s loader callbacks implementation.
- *
-%(*comments)s */
-#include "common.h"
-#include "fsm.h"
-#include "%(prefix)s_cb.h"
-
-%(states_template,template_cb_impl.c,origin=loader)s
diff --git a/digital/io/src/fsm_templates/template_move_cb_skel.c b/digital/io/src/fsm_templates/template_move_cb_skel.c
deleted file mode 100644
index 533ffbfd..00000000
--- a/digital/io/src/fsm_templates/template_move_cb_skel.c
+++ /dev/null
@@ -1,11 +0,0 @@
-/*
- * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT!
- *
- * Skeleton for %(name)s move callbacks implementation.
- *
-%(*comments)s */
-#include "common.h"
-#include "fsm.h"
-#include "%(prefix)s_cb.h"
-
-%(states_template,template_cb_impl.c,origin=move)s
diff --git a/digital/io/src/fsm_templates/template_top_cb_skel.c b/digital/io/src/fsm_templates/template_top_cb_skel.c
deleted file mode 100644
index 8895dc64..00000000
--- a/digital/io/src/fsm_templates/template_top_cb_skel.c
+++ /dev/null
@@ -1,11 +0,0 @@
-/*
- * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT!
- *
- * Skeleton for %(name)s top callbacks implementation.
- *
-%(*comments)s */
-#include "common.h"
-#include "fsm.h"
-#include "%(prefix)s_cb.h"
-
-%(states_template,template_cb_impl.c,origin=top)s
diff --git a/digital/io/src/hola.c b/digital/io/src/hola.c
index 1fd15f18..c7d07e21 100644
--- a/digital/io/src/hola.c
+++ b/digital/io/src/hola.c
@@ -23,3 +23,118 @@
*
* }}} */
+#define FSM_NAME AI
+
+#include "common.h"
+#include "fsm.h"
+#include "asserv.h"
+#include "mimot.h"
+#include "bot.h"
+#include "playground.h"
+
+#include "modules/math/fixed/fixed.h"
+
+uint32_t hola_angle;
+int16_t hola_timeout;
+
+FSM_STATES (
+ /* waiting start */
+ HOLA_IDLE,
+ /* wait hola signal */
+ HOLA_WAIT_HOLA,
+ /* wait until the jack is inserted for the first time */
+ HOLA_WAIT_JACK_IN,
+ /* wait until the jack is removed to initialise robot */
+ HOLA_WAIT_JACK_OUT,
+ /* moving clamps in rounds */
+ HOLA_ROUNDS,
+ /* moving clamp up */
+ HOLA_UP,
+ HOLA_CENTER,
+ HOLA_CLAP_CLOSE,
+ HOLA_CLAP_OPEN)
+
+FSM_EVENTS (hola_start)
+
+FSM_START_WITH (HOLA_IDLE)
+
+FSM_TRANS (HOLA_IDLE, start, HOLA_WAIT_HOLA)
+{
+ return FSM_NEXT (HOLA_IDLE, start);
+}
+
+FSM_TRANS (HOLA_WAIT_HOLA, hola_start, HOLA_WAIT_JACK_IN)
+{
+ return FSM_NEXT (HOLA_WAIT_HOLA, hola_start);
+}
+
+FSM_TRANS (HOLA_WAIT_JACK_IN, jack_inserted_into_bot, HOLA_WAIT_JACK_OUT)
+{
+ return FSM_NEXT (HOLA_WAIT_JACK_IN, jack_inserted_into_bot);
+}
+
+FSM_TRANS (HOLA_WAIT_JACK_OUT, jack_removed_from_bot, HOLA_ROUNDS)
+{
+ return FSM_NEXT (HOLA_WAIT_JACK_OUT, jack_removed_from_bot);
+}
+
+FSM_TRANS (HOLA_ROUNDS, asserv_last_cmd_ack,
+ /* move clamp rounds */
+ no_timeout, HOLA_ROUNDS,
+ /* move up, rotate */
+ timeout, HOLA_UP)
+{
+ if (++hola_timeout < 150)
+ {
+ int32_t cos = fixed_cos_f824 (hola_angle) + 0x1000000;
+ int32_t sin = fixed_sin_f824 (hola_angle) + 0x1000000;
+ int32_t dx = BOT_CLAMP_OPEN_STEP
+ + fixed_mul_f824 (BOT_CLAMP_STROKE_STEP / 3 / 2, cos);
+ int32_t dy = BOT_ELEVATOR_ZERO_SPEED
+ + fixed_mul_f824 (BOT_ELEVATOR_REST_STEP / 3 / 2, sin);
+ mimot_move_motor0_absolute (dx, BOT_CLAMP_SPEED);
+ mimot_move_motor1_absolute (dx, BOT_CLAMP_SPEED);
+ asserv_move_motor0_absolute (dy, BOT_ELEVATOR_SPEED);
+ hola_angle += 0x1000000 / (225 / 12);
+ return FSM_NEXT (HOLA_ROUNDS, asserv_last_cmd_ack, no_timeout);
+ }
+ else
+ {
+ asserv_set_speed (0x10, 0x1c, 0x10, 0x1c);
+ asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED / 3);
+ asserv_move_angularly (POSITION_A_DEG (174));
+ return FSM_NEXT (HOLA_ROUNDS, asserv_last_cmd_ack, timeout);
+ }
+}
+
+FSM_TRANS (HOLA_UP, bot_move_succeed, HOLA_CENTER)
+{
+ asserv_set_speed (0x10, 0x20, 0x10, 0x20);
+ asserv_move_angularly (POSITION_A_DEG (-90));
+ asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP / 2, BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (HOLA_UP, bot_move_succeed);
+}
+
+FSM_TRANS (HOLA_CENTER, bot_move_succeed, HOLA_CLAP_CLOSE)
+{
+ mimot_motor0_clamp (BOT_CLAMP_ZERO_SPEED, 0);
+ mimot_motor1_clamp (BOT_CLAMP_ZERO_SPEED, 0);
+ return FSM_NEXT (HOLA_CENTER, bot_move_succeed);
+}
+
+FSM_TRANS (HOLA_CLAP_CLOSE, clamp_succeed, HOLA_CLAP_OPEN)
+{
+ mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ return FSM_NEXT (HOLA_CLAP_CLOSE, clamp_succeed);
+}
+
+/*
+ * close clamp
+ */
+FSM_TRANS (HOLA_CLAP_OPEN, clamp_succeed, HOLA_CLAP_CLOSE)
+{
+ mimot_motor0_clamp (BOT_CLAMP_SPEED, 0);
+ mimot_motor1_clamp (BOT_CLAMP_SPEED, 0);
+ return FSM_NEXT (HOLA_CLAP_OPEN, clamp_succeed);
+}
diff --git a/digital/io/src/hola.fsm b/digital/io/src/hola.fsm
deleted file mode 100644
index ca219491..00000000
--- a/digital/io/src/hola.fsm
+++ /dev/null
@@ -1,59 +0,0 @@
-# Hola FSM
-hola
- entertain people
-
-States:
- *HOLA_IDLE
- waiting start
- HOLA_WAIT_HOLA
- wait hola signal
- HOLA_WAIT_JACK_IN
- wait until the jack is inserted for the first time
- HOLA_WAIT_JACK_OUT
- wait until the jack is removed to initialise robot
- HOLA_ROUNDS
- moving clamps in rounds
- HOLA_UP
- moving clamp up
- HOLA_CENTER
- HOLA_CLAP_CLOSE
- HOLA_CLAP_OPEN
-
-Events:
- hola_start
- start hola
-
-HOLA_IDLE:
- start -> HOLA_WAIT_HOLA
-
-HOLA_WAIT_HOLA:
- hola_start -> HOLA_WAIT_JACK_IN
-
-HOLA_WAIT_JACK_IN:
- jack_inserted_into_bot -> HOLA_WAIT_JACK_OUT
-
-HOLA_WAIT_JACK_OUT:
- jack_removed_from_bot -> HOLA_ROUNDS
-
-HOLA_ROUNDS:
- asserv_last_cmd_ack: no_timeout -> .
- move clamp rounds
- asserv_last_cmd_ack: timeout -> HOLA_UP
- move up
- rotate
-
-HOLA_UP:
- bot_move_succeed -> HOLA_CENTER
- rotate to center
-
-HOLA_CENTER:
- bot_move_succeed -> HOLA_CLAP_CLOSE
- close clamp
-
-HOLA_CLAP_CLOSE:
- clamp_succeed -> HOLA_CLAP_OPEN
- open clamp
-
-HOLA_CLAP_OPEN:
- clamp_succeed -> HOLA_CLAP_CLOSE
- close clamp
diff --git a/digital/io/src/init.c b/digital/io/src/init.c
index 1d0d8d82..4cb2bcd9 100644
--- a/digital/io/src/init.c
+++ b/digital/io/src/init.c
@@ -22,8 +22,212 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
-#include "common.h"
+#define FSM_NAME AI
+#include "common.h"
+#include "fsm.h"
+#include "asserv.h"
#include "init.h"
+#include "playground.h"
+#include "main.h"
+#include "bot.h"
+#include "switch.h"
+#include "chrono.h"
+#include "playground.h"
+#include "modules/trace/trace.h"
+#include "modules/path/path.h"
uint8_t init_match_is_started = 0;
+
+FSM_STATES (
+ /* waiting for the beginning of the top FSM. */
+ INIT_IDLE,
+ /* wait for the jack to be inserted into the bot for the first time. */
+ INIT_WAIT_FIRST_JACK_IN,
+ /* wait the jack to be removed from the bot for the first time. */
+ INIT_WAIT_FIRST_JACK_OUT,
+ /* wait for the jack to be inserted into the bot for the second time. */
+ INIT_WAIT_SECOND_JACK_IN,
+ /* just wait for operator hands to be removed from the jack. */
+ INIT_WAIT_FOR_HANDS_OUT,
+ /* go to the wall for the first time. */
+ INIT_GO_TO_THE_WALL,
+ /* reset the Y position of the bot.
+ * reset the angular position of the bot.
+ * go backward from the wall for a specific distance. */
+ INIT_GO_AWAY_FROM_THE_WALL,
+ /* make the bot face the other wall. */
+ INIT_FACE_OTHER_WALL,
+ /* make sure rotation is really finished before continuing. */
+ INIT_WAIT_AFTER_ROTATION,
+ /* go to the wall for the second time. */
+ INIT_GO_TO_THE_WALL_AGAIN,
+ /* reset the X position of the bot.
+ * go backward from the wall for a specific distance. */
+ INIT_GO_AWAY_FROM_THE_WALL_AGAIN,
+ /* go to the start position. */
+ INIT_GO_TO_START_POSITION,
+ /* wait the jack to be removed from the bot for the second time. */
+ INIT_WAIT_SECOND_JACK_OUT)
+
+FSM_EVENTS (
+ /* the jack is inserted into the bot. */
+ jack_inserted_into_bot,
+ /* the jack is removed from the bot. */
+ jack_removed_from_bot)
+
+FSM_START_WITH (INIT_IDLE)
+
+/*
+ * do nothing.
+ */
+FSM_TRANS (INIT_IDLE, start, INIT_WAIT_FIRST_JACK_IN)
+{
+ return FSM_NEXT (INIT_IDLE, start);
+}
+
+/*
+ * do nothing.
+ */
+FSM_TRANS (INIT_WAIT_FIRST_JACK_IN,
+ jack_inserted_into_bot,
+ INIT_WAIT_FIRST_JACK_OUT)
+{
+ return FSM_NEXT (INIT_WAIT_FIRST_JACK_IN, jack_inserted_into_bot);
+}
+
+/*
+ * start trace module.
+ * get and store the color of the bot.
+ */
+FSM_TRANS (INIT_WAIT_FIRST_JACK_OUT,
+ jack_removed_from_bot,
+ INIT_WAIT_SECOND_JACK_IN)
+{
+ /* Initialize trace module (erase the flash). */
+ trace_init ();
+ /* Get the color. */
+ bot_color = switch_get_color ();
+ return FSM_NEXT (INIT_WAIT_FIRST_JACK_OUT, jack_removed_from_bot);
+}
+
+/*
+ * do nothing.
+ */
+FSM_TRANS (INIT_WAIT_SECOND_JACK_IN,
+ jack_inserted_into_bot,
+ INIT_WAIT_FOR_HANDS_OUT)
+{
+ return FSM_NEXT (INIT_WAIT_SECOND_JACK_IN, jack_inserted_into_bot);
+}
+
+/*
+ * fuck the wall in front.
+ */
+FSM_TRANS_TIMEOUT (INIT_WAIT_FOR_HANDS_OUT, 225,
+ INIT_GO_TO_THE_WALL)
+{
+ /* Go to the wall, no backward. */
+ asserv_go_to_the_wall (0);
+ return FSM_NEXT_TIMEOUT (INIT_WAIT_FOR_HANDS_OUT);
+}
+
+/*
+ * reset the Y position of the bot.
+ * reset the angular position of the bot.
+ * move away from the wall (linear move).
+ */
+FSM_TRANS (INIT_GO_TO_THE_WALL,
+ bot_move_succeed,
+ INIT_GO_AWAY_FROM_THE_WALL)
+{
+ asserv_set_y_position (PG_Y (PG_LENGTH - BOT_SIZE_FRONT));
+ asserv_set_angle_position (PG_A_DEG (90));
+ /* Move away from the border. */
+ asserv_move_linearly (- INIT_DIST);
+ return FSM_NEXT (INIT_GO_TO_THE_WALL, bot_move_succeed);
+}
+
+/*
+ * turn to face the other wall.
+ */
+FSM_TRANS (INIT_GO_AWAY_FROM_THE_WALL,
+ bot_move_succeed,
+ INIT_FACE_OTHER_WALL)
+{
+ /* Face the other wall. */
+ asserv_goto_angle (PG_A_DEG (180));
+ return FSM_NEXT (INIT_GO_AWAY_FROM_THE_WALL, bot_move_succeed);
+}
+
+/*
+ * nothing to do.
+ */
+FSM_TRANS (INIT_FACE_OTHER_WALL,
+ bot_move_succeed,
+ INIT_WAIT_AFTER_ROTATION)
+{
+ return FSM_NEXT (INIT_FACE_OTHER_WALL, bot_move_succeed);
+}
+
+/*
+ * fuck the wall in front.
+ */
+FSM_TRANS_TIMEOUT (INIT_WAIT_AFTER_ROTATION, 100,
+ INIT_GO_TO_THE_WALL_AGAIN)
+{
+ /* Go to the wall, no backward. */
+ asserv_go_to_the_wall (0);
+ return FSM_NEXT_TIMEOUT (INIT_WAIT_AFTER_ROTATION);
+}
+
+/*
+ * reset the X position of the bot.
+ * move away from the wall (linear move).
+ */
+FSM_TRANS (INIT_GO_TO_THE_WALL_AGAIN,
+ bot_move_succeed,
+ INIT_GO_AWAY_FROM_THE_WALL_AGAIN)
+{
+ asserv_set_x_position (PG_X (BOT_SIZE_FRONT));
+ /* Move away from the border. */
+ asserv_move_linearly (- INIT_DIST);
+ return FSM_NEXT (INIT_GO_TO_THE_WALL_AGAIN, bot_move_succeed);
+}
+
+/*
+ * go to the start position with a go to movement.
+ */
+FSM_TRANS (INIT_GO_AWAY_FROM_THE_WALL_AGAIN,
+ bot_move_succeed,
+ INIT_GO_TO_START_POSITION)
+{
+ /* Move away from the border. */
+ asserv_goto_xya (BOT_INIT_XYA, 0);
+ return FSM_NEXT (INIT_GO_AWAY_FROM_THE_WALL_AGAIN, bot_move_succeed);
+}
+
+/*
+ * nothing to do, the bot is at the start position.
+ */
+FSM_TRANS (INIT_GO_TO_START_POSITION,
+ bot_move_succeed,
+ INIT_WAIT_SECOND_JACK_OUT)
+{
+ return FSM_NEXT (INIT_GO_TO_START_POSITION, bot_move_succeed);
+}
+
+/*
+ * tell other FSM the match begins.
+ * start the chrono.
+ */
+FSM_TRANS (INIT_WAIT_SECOND_JACK_OUT,
+ jack_removed_from_bot,
+ INIT_IDLE)
+{
+ /* Set the flag to transmit to other FSM. */
+ init_match_is_started = 1;
+ /* Start the chrono. */
+ chrono_init ();
+ return FSM_NEXT (INIT_WAIT_SECOND_JACK_OUT, jack_removed_from_bot);
+}
diff --git a/digital/io/src/init.fsm b/digital/io/src/init.fsm
deleted file mode 100644
index f780ba45..00000000
--- a/digital/io/src/init.fsm
+++ /dev/null
@@ -1,106 +0,0 @@
-# init FSM
-# This FSM is used to place the bot at a "start position" for a match. It is
-# more precise than a human hand and reset the asserv board to the start
-# position.
-# Some remarks:
-# - for security reason, we need to insert the jack then removed it, then
-# insert it back. Why? Because it can already be inserted.
-# - other FSM can do a similar thing by using the first jack out to
-# initialize.
-init
- place the bot in the start position for a match.
-
-States:
- *INIT_IDLE
- waiting for the beginning of the top FSM.
- INIT_WAIT_FIRST_JACK_IN
- wait for the jack to be inserted into the bot for the first time.
- INIT_WAIT_FIRST_JACK_OUT
- wait the jack to be removed from the bot for the first time.
- INIT_WAIT_SECOND_JACK_IN
- wait for the jack to be inserted into the bot for the second time.
- INIT_WAIT_FOR_HANDS_OUT[timeout=225]
- just wait for operator hands to be removed from the jack.
- INIT_GO_TO_THE_WALL
- go to the wall for the first time.
- INIT_GO_AWAY_FROM_THE_WALL
- reset the Y position of the bot.
- reset the angular position of the bot.
- go backward from the wall for a specific distance.
- INIT_FACE_OTHER_WALL
- make the bot face the other wall.
- INIT_WAIT_AFTER_ROTATION [timeout=100]
- make sure rotation is really finished before continuing.
- INIT_GO_TO_THE_WALL_AGAIN
- go to the wall for the second time.
- INIT_GO_AWAY_FROM_THE_WALL_AGAIN
- reset the X position of the bot.
- go backward from the wall for a specific distance.
- INIT_GO_TO_START_POSITION
- go to the start position.
- INIT_WAIT_SECOND_JACK_OUT
- wait the jack to be removed from the bot for the second time.
-
-Events:
- jack_inserted_into_bot
- the jack is inserted into the bot.
- jack_removed_from_bot
- the jack is removed from the bot.
-
-INIT_IDLE:
- start -> INIT_WAIT_FIRST_JACK_IN
- do nothing.
-
-INIT_WAIT_FIRST_JACK_IN:
- jack_inserted_into_bot -> INIT_WAIT_FIRST_JACK_OUT
- do nothing.
-
-INIT_WAIT_FIRST_JACK_OUT:
- jack_removed_from_bot -> INIT_WAIT_SECOND_JACK_IN
- start trace module.
- get and store the color of the bot.
-
-INIT_WAIT_SECOND_JACK_IN:
- jack_inserted_into_bot -> INIT_WAIT_FOR_HANDS_OUT
- do nothing.
-
-INIT_WAIT_FOR_HANDS_OUT:
- state_timeout -> INIT_GO_TO_THE_WALL
- fuck the wall in front.
-
-INIT_GO_TO_THE_WALL:
- bot_move_succeed -> INIT_GO_AWAY_FROM_THE_WALL
- reset the Y position of the bot.
- reset the angular position of the bot.
- move away from the wall (linear move).
-
-INIT_GO_AWAY_FROM_THE_WALL:
- bot_move_succeed -> INIT_FACE_OTHER_WALL
- turn to face the other wall.
-
-INIT_FACE_OTHER_WALL:
- bot_move_succeed -> INIT_WAIT_AFTER_ROTATION
- nothing to do.
-
-INIT_WAIT_AFTER_ROTATION:
- state_timeout -> INIT_GO_TO_THE_WALL_AGAIN
- fuck the wall in front.
-
-INIT_GO_TO_THE_WALL_AGAIN:
- bot_move_succeed -> INIT_GO_AWAY_FROM_THE_WALL_AGAIN
- reset the X position of the bot.
- move away from the wall (linear move).
-
-INIT_GO_AWAY_FROM_THE_WALL_AGAIN:
- bot_move_succeed -> INIT_GO_TO_START_POSITION
- go to the start position with a go to movement.
-
-INIT_GO_TO_START_POSITION:
- bot_move_succeed -> INIT_WAIT_SECOND_JACK_OUT
- nothing to do, the bot is at the start position.
-
-INIT_WAIT_SECOND_JACK_OUT:
- jack_removed_from_bot -> INIT_IDLE
- tell other FSM the match begins.
- start the chrono.
-
diff --git a/digital/io/src/loader.c b/digital/io/src/loader.c
index 4157023e..8dd557da 100644
--- a/digital/io/src/loader.c
+++ b/digital/io/src/loader.c
@@ -22,10 +22,16 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+#define FSM_NAME AI
+
#include "common.h"
#include "loader.h"
-
#include "fsm.h"
+#include "asserv.h"
+#include "mimot.h"
+#include "bot.h"
+#include "main.h"
+#include "food.h"
uint8_t loader_elements;
uint8_t loader_want_up;
@@ -34,13 +40,614 @@ void
loader_up (void)
{
loader_want_up = 1;
- fsm_handle_event (&ai_fsm, AI_EVENT_loader_up);
+ FSM_HANDLE (AI, loader_up);
}
void
loader_down (void)
{
loader_want_up = 0;
- fsm_handle_event (&ai_fsm, AI_EVENT_loader_down);
+ FSM_HANDLE (AI, loader_down);
+}
+
+FSM_STATES (
+ /* wait start */
+ LOADER_IDLE,
+ /* wait until the jack is inserted for the first time */
+ LOADER_WAIT_JACK_IN,
+ /* wait until the jack is removed to initialise robot */
+ LOADER_WAIT_JACK_OUT,
+ /* move the elevator down to find zero position */
+ LOADER_INIT_ELEVATOR_ZERO,
+ /* test clamp closing */
+ LOADER_INIT_CLAMP_CLOSE,
+ /* find clamp zero position */
+ LOADER_INIT_CLAMP_ZERO,
+ /* find gate zero */
+ LOADER_INIT_GATE_ZERO,
+ /* move the elevator up so that the robot can go to the wall */
+ LOADER_INIT_ELEVATOR_UP,
+ /* wait with gate open */
+ LOADER_INIT_GATE_WAIT,
+ /* close gate before start */
+ LOADER_INIT_GATE_CLOSE,
+ /* up position, clamp is protected */
+ LOADER_UP,
+ /* down position, ready to take an element */
+ LOADER_DOWN,
+ /* moving to the up position */
+ LOADER_UPING,
+ /* moving to the down position */
+ LOADER_DOWNING,
+ /* error while moving up, stop in an unknown state */
+ LOADER_ERROR,
+ /* error recovery moving down */
+ LOADER_ERROR_DOWNING,
+ /* error recovery moving down, open clamp */
+ LOADER_ERROR_DOWNING_OPEN,
+ /* error recovery moving up */
+ LOADER_ERROR_UPING,
+ /* error recovery moving up, open clamp */
+ LOADER_ERROR_UPING_OPEN,
+ /* close clamp */
+ LOADER_LOAD_CLOSING,
+ /* move load up to the unload position */
+ LOADER_LOAD_UPING,
+ /* open clamp to unload, wait until unload position */
+ LOADER_LOAD_UNLOADING,
+ /* check clamp is open */
+ LOADER_LOAD_UNLOADING_OPEN,
+ /* open clamp due to no elements to pick */
+ LOADER_LOAD_EMPTY_OPEN)
+
+FSM_EVENTS (
+ /* asserv success result */
+ elevator_succeed,
+ /* asserv failure result */
+ elevator_failed,
+ /* elevator above unloading position */
+ elevator_unload_position,
+ /* asserv failure result */
+ clamp_failed,
+ /* asserv success result */
+ gate_succeed,
+ /* asserv failure result */
+ gate_failed,
+ /* element sensed between clamp to be loaded */
+ loader_element,
+ /* make the loader ready to load an element */
+ loader_down,
+ /* protect the clamp by moving it up */
+ loader_up,
+ /* posted when loader successfully moved down */
+ loader_downed,
+ /* posted when loader successfully moved up */
+ loader_uped,
+ /* posted when loader in an unknown state due to error while moving up */
+ loader_errored,
+ /* loader blocked, due to a black corn */
+ loader_black)
+
+FSM_START_WITH (LOADER_IDLE)
+
+FSM_TRANS (LOADER_IDLE, start, LOADER_WAIT_JACK_IN)
+{
+ return FSM_NEXT (LOADER_IDLE, start);
+}
+
+FSM_TRANS (LOADER_WAIT_JACK_IN,
+ jack_inserted_into_bot,
+ LOADER_WAIT_JACK_OUT)
+{
+ return FSM_NEXT (LOADER_WAIT_JACK_IN, jack_inserted_into_bot);
+}
+
+FSM_TRANS (LOADER_WAIT_JACK_IN,
+ hola_start,
+ LOADER_IDLE)
+{
+ return FSM_NEXT (LOADER_WAIT_JACK_IN, hola_start);
+}
+
+/*
+ * find elevator zero
+ * close clamp
+ * find gate zero
+ */
+FSM_TRANS (LOADER_WAIT_JACK_OUT,
+ jack_removed_from_bot,
+ LOADER_INIT_ELEVATOR_ZERO)
+{
+ asserv_motor0_zero_position (-BOT_ELEVATOR_ZERO_SPEED);
+ mimot_motor0_clamp (BOT_CLAMP_ZERO_SPEED, 0);
+ mimot_motor1_clamp (BOT_CLAMP_ZERO_SPEED, 0);
+ asserv_motor1_zero_position (-BOT_GATE_SPEED);
+ return FSM_NEXT (LOADER_WAIT_JACK_OUT, jack_removed_from_bot);
+}
+
+FSM_TRANS (LOADER_INIT_ELEVATOR_ZERO,
+ elevator_succeed,
+ LOADER_INIT_CLAMP_CLOSE)
+{
+ return FSM_NEXT (LOADER_INIT_ELEVATOR_ZERO, elevator_succeed);
+}
+
+/*
+ * move elevator up
+ * find clamp zero
+ */
+FSM_TRANS (LOADER_INIT_CLAMP_CLOSE,
+ clamp_succeed,
+ LOADER_INIT_CLAMP_ZERO)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP,
+ BOT_ELEVATOR_ZERO_SPEED);
+ mimot_motor0_zero_position (-BOT_CLAMP_ZERO_SPEED);
+ mimot_motor1_zero_position (-BOT_CLAMP_ZERO_SPEED);
+ return FSM_NEXT (LOADER_INIT_CLAMP_CLOSE, clamp_succeed);
+}
+
+FSM_TRANS (LOADER_INIT_CLAMP_ZERO,
+ clamp_succeed,
+ LOADER_INIT_GATE_ZERO)
+{
+ return FSM_NEXT (LOADER_INIT_CLAMP_ZERO, clamp_succeed);
+}
+
+FSM_TRANS (LOADER_INIT_GATE_ZERO,
+ gate_succeed,
+ LOADER_INIT_ELEVATOR_UP)
+{
+ return FSM_NEXT (LOADER_INIT_GATE_ZERO, gate_succeed);
+}
+
+FSM_TRANS (LOADER_INIT_ELEVATOR_UP,
+ elevator_succeed,
+ LOADER_INIT_GATE_WAIT)
+{
+ return FSM_NEXT (LOADER_INIT_ELEVATOR_UP, elevator_succeed);
+}
+
+/*
+ * initialisation failure
+ */
+FSM_TRANS (LOADER_INIT_ELEVATOR_UP,
+ elevator_failed,
+ LOADER_IDLE)
+{
+ /* Move so that the operator notice it. */
+ asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP / 3,
+ BOT_ELEVATOR_ZERO_SPEED);
+ return FSM_NEXT (LOADER_INIT_ELEVATOR_UP, elevator_failed);
}
+/*
+ * close gate
+ */
+FSM_TRANS_TIMEOUT (LOADER_INIT_GATE_WAIT, 225,
+ LOADER_INIT_GATE_CLOSE)
+{
+ asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED);
+ return FSM_NEXT_TIMEOUT (LOADER_INIT_GATE_WAIT);
+}
+
+FSM_TRANS (LOADER_INIT_GATE_CLOSE,
+ gate_succeed,
+ LOADER_UP)
+{
+ return FSM_NEXT (LOADER_INIT_GATE_CLOSE, gate_succeed);
+}
+
+/*
+ * move down
+ */
+FSM_TRANS (LOADER_UP,
+ loader_down,
+ LOADER_DOWNING)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (LOADER_UP, loader_down);
+}
+
+/*
+ * move up
+ */
+FSM_TRANS (LOADER_DOWN,
+ loader_up,
+ LOADER_UPING)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (LOADER_DOWN, loader_up);
+}
+
+/*
+ * clamp
+ */
+FSM_TRANS (LOADER_DOWN,
+ loader_element,
+ LOADER_LOAD_CLOSING)
+{
+ mimot_motor0_clamp (BOT_CLAMP_SPEED, BOT_CLAMP_PWM);
+ mimot_motor1_clamp (BOT_CLAMP_SPEED, BOT_CLAMP_PWM);
+ return FSM_NEXT (LOADER_DOWN, loader_element);
+}
+
+/*
+ * post loader_uped event
+ */
+FSM_TRANS (LOADER_UPING,
+ elevator_succeed,
+ LOADER_UP)
+{
+ main_post_event (FSM_EVENT (AI,loader_uped));
+ return FSM_NEXT (LOADER_UPING, elevator_succeed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS (LOADER_UPING,
+ elevator_failed,
+ LOADER_ERROR)
+{
+ main_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT (LOADER_UPING, elevator_failed);
+}
+
+/*
+ * move down
+ */
+FSM_TRANS (LOADER_UPING,
+ loader_down,
+ LOADER_DOWNING)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (LOADER_UPING, loader_down);
+}
+
+/*
+ * release elevator motor
+ * post loader_downed event
+ */
+FSM_TRANS (LOADER_DOWNING,
+ elevator_succeed,
+ LOADER_DOWN)
+{
+ asserv_motor0_free ();
+ main_post_event (FSM_EVENT (AI, loader_downed));
+ return FSM_NEXT (LOADER_DOWNING, elevator_succeed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS (LOADER_DOWNING,
+ elevator_failed,
+ LOADER_ERROR)
+{
+ main_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT (LOADER_DOWNING, elevator_failed);
+}
+
+/*
+ * emergency stop
+ * post loader_errored event
+ */
+FSM_TRANS (LOADER_DOWNING,
+ loader_element,
+ LOADER_ERROR)
+{
+ asserv_motor0_free ();
+ main_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT (LOADER_DOWNING, loader_element);
+}
+
+/*
+ * move up
+ */
+FSM_TRANS (LOADER_DOWNING,
+ loader_up,
+ LOADER_UPING)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (LOADER_DOWNING, loader_up);
+}
+
+/*
+ * move down
+ */
+FSM_TRANS (LOADER_ERROR,
+ loader_down,
+ LOADER_ERROR_DOWNING)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (LOADER_ERROR, loader_down);
+}
+
+/*
+ * move up
+ */
+FSM_TRANS (LOADER_ERROR,
+ loader_up,
+ LOADER_ERROR_UPING)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP, BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (LOADER_ERROR, loader_up);
+}
+
+/*
+ * release elevator motor
+ * open clamp
+ */
+FSM_TRANS (LOADER_ERROR_DOWNING,
+ elevator_succeed,
+ LOADER_ERROR_DOWNING_OPEN)
+{
+ asserv_motor0_free ();
+ mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ return FSM_NEXT (LOADER_ERROR_DOWNING, elevator_succeed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS (LOADER_ERROR_DOWNING,
+ elevator_failed,
+ LOADER_ERROR)
+{
+ main_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT (LOADER_ERROR_DOWNING, elevator_failed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS_TIMEOUT (LOADER_ERROR_DOWNING, 225,
+ LOADER_ERROR)
+{
+ main_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT_TIMEOUT (LOADER_ERROR_DOWNING);
+}
+
+/*
+ * post loader_downed event
+ */
+FSM_TRANS (LOADER_ERROR_DOWNING_OPEN,
+ clamp_succeed,
+ LOADER_DOWN)
+{
+ main_post_event (FSM_EVENT (AI, loader_downed));
+ return FSM_NEXT (LOADER_ERROR_DOWNING_OPEN, clamp_succeed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS (LOADER_ERROR_DOWNING_OPEN,
+ clamp_failed,
+ LOADER_ERROR)
+{
+ main_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT (LOADER_ERROR_DOWNING_OPEN, clamp_failed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS_TIMEOUT (LOADER_ERROR_DOWNING_OPEN, 225,
+ LOADER_ERROR)
+{
+ main_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT_TIMEOUT (LOADER_ERROR_DOWNING_OPEN);
+}
+
+/*
+ * open clamp
+ */
+FSM_TRANS (LOADER_ERROR_UPING,
+ elevator_succeed,
+ LOADER_ERROR_UPING_OPEN)
+{
+ mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ return FSM_NEXT (LOADER_ERROR_UPING, elevator_succeed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS (LOADER_ERROR_UPING,
+ elevator_failed,
+ LOADER_ERROR)
+{
+ main_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT (LOADER_ERROR_UPING, elevator_failed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS_TIMEOUT (LOADER_ERROR_UPING, 225,
+ LOADER_ERROR)
+{
+ main_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT_TIMEOUT (LOADER_ERROR_UPING);
+}
+
+/*
+ * post loader_uped event
+ */
+FSM_TRANS (LOADER_ERROR_UPING_OPEN,
+ clamp_succeed,
+ LOADER_UP)
+{
+ main_post_event (FSM_EVENT (AI, loader_uped));
+ return FSM_NEXT (LOADER_ERROR_UPING_OPEN, clamp_succeed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS (LOADER_ERROR_UPING_OPEN,
+ clamp_failed,
+ LOADER_ERROR)
+{
+ main_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT (LOADER_ERROR_UPING_OPEN, clamp_failed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS_TIMEOUT (LOADER_ERROR_UPING_OPEN, 225,
+ LOADER_ERROR)
+{
+ main_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT_TIMEOUT (LOADER_ERROR_UPING_OPEN);
+}
+
+FSM_TRANS (LOADER_LOAD_CLOSING, clamp_succeed,
+ /* move up
+ * count one element */
+ full, LOADER_LOAD_UPING,
+ /* open clamp */
+ empty, LOADER_LOAD_EMPTY_OPEN)
+{
+ /* Measure load using clamp position. */
+ uint16_t tickness = BOT_CLAMP_WIDTH_STEP
+ - mimot_get_motor0_position ()
+ - mimot_get_motor1_position ();
+ if (tickness > BOT_CLAMP_EMPTY_STEP)
+ {
+ asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP,
+ BOT_ELEVATOR_SPEED);
+ position_t robot_position;
+ asserv_get_position (&robot_position);
+ food_taken (robot_position);
+ loader_elements++;
+ return FSM_NEXT (LOADER_LOAD_CLOSING, clamp_succeed, full);
+ }
+ else
+ {
+ mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ return FSM_NEXT (LOADER_LOAD_CLOSING, clamp_succeed, empty);
+ }
+}
+
+/*
+ * open clamp
+ */
+FSM_TRANS (LOADER_LOAD_UPING,
+ elevator_unload_position,
+ LOADER_LOAD_UNLOADING)
+{
+ mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ return FSM_NEXT (LOADER_LOAD_UPING, elevator_unload_position);
+}
+
+/*
+ * post loader_errored or loader_black event
+ * remove one element
+ * open clamp
+ */
+FSM_TRANS (LOADER_LOAD_UPING,
+ elevator_failed,
+ LOADER_ERROR)
+{
+ if (asserv_get_motor0_position () < BOT_ELEVATOR_BLACK_THRESHOLD_STEP)
+ {
+ if (loader_elements)
+ loader_elements--;
+ main_post_event (FSM_EVENT (AI, loader_black));
+ }
+ else
+ main_post_event (FSM_EVENT (AI, loader_errored));
+ mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ return FSM_NEXT (LOADER_LOAD_UPING, elevator_failed);
+}
+
+FSM_TRANS (LOADER_LOAD_UNLOADING,
+ elevator_succeed,
+ LOADER_LOAD_UNLOADING_OPEN)
+{
+ return FSM_NEXT (LOADER_LOAD_UNLOADING, elevator_succeed);
+}
+
+/*
+ * post loader_errored event
+ * open clamp
+ */
+FSM_TRANS (LOADER_LOAD_UNLOADING,
+ elevator_failed,
+ LOADER_ERROR)
+{
+ main_post_event (FSM_EVENT (AI, loader_errored));
+ mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ return FSM_NEXT (LOADER_LOAD_UNLOADING, elevator_failed);
+}
+
+FSM_TRANS (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed,
+ /* move down */
+ down, LOADER_DOWNING,
+ /* move up */
+ up, LOADER_UPING)
+{
+ if (loader_want_up)
+ {
+ asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP,
+ BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed, up);
+ }
+ else
+ {
+ asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP,
+ BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed, down);
+ }
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS (LOADER_LOAD_UNLOADING_OPEN,
+ clamp_failed,
+ LOADER_ERROR)
+{
+ main_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT (LOADER_LOAD_UNLOADING_OPEN, clamp_failed);
+}
+
+FSM_TRANS (LOADER_LOAD_EMPTY_OPEN, clamp_succeed,
+ /* post loader_downed event */
+ down, LOADER_DOWN,
+ /* move up */
+ up, LOADER_UPING)
+{
+ if (loader_want_up)
+ {
+ asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP,
+ BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, up);
+ }
+ else
+ {
+ main_post_event (FSM_EVENT (AI, loader_downed));
+ return FSM_NEXT (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, down);
+ }
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS (LOADER_LOAD_EMPTY_OPEN,
+ clamp_failed,
+ LOADER_ERROR)
+{
+ main_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT (LOADER_LOAD_EMPTY_OPEN, clamp_failed);
+}
diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm
deleted file mode 100644
index a188194e..00000000
--- a/digital/io/src/loader.fsm
+++ /dev/null
@@ -1,227 +0,0 @@
-# loader FSM
-loader
- Handle Eurobot 2010 robot clamp, elevator and output door.
-
-States:
- *LOADER_IDLE
- wait start
- LOADER_WAIT_JACK_IN
- wait until the jack is inserted for the first time
- LOADER_WAIT_JACK_OUT
- wait until the jack is removed to initialise robot
- LOADER_INIT_ELEVATOR_ZERO
- move the elevator down to find zero position
- LOADER_INIT_CLAMP_CLOSE
- test clamp closing
- LOADER_INIT_CLAMP_ZERO
- find clamp zero position
- LOADER_INIT_GATE_ZERO
- find gate zero
- LOADER_INIT_ELEVATOR_UP
- move the elevator up so that the robot can go to the wall
- LOADER_INIT_GATE_WAIT[timeout=225]
- wait with gate open
- LOADER_INIT_GATE_CLOSE
- close gate before start
- LOADER_UP
- up position, clamp is protected
- LOADER_DOWN
- down position, ready to take an element
- LOADER_UPING
- moving to the up position
- LOADER_DOWNING
- moving to the down position
- LOADER_ERROR
- error while moving up, stop in an unknown state
- LOADER_ERROR_DOWNING [timeout=225]
- error recovery moving down
- LOADER_ERROR_DOWNING_OPEN [timeout=225]
- error recovery moving down, open clamp
- LOADER_ERROR_UPING [timeout=225]
- error recovery moving up
- LOADER_ERROR_UPING_OPEN [timeout=225]
- error recovery moving up, open clamp
- LOADER_LOAD_CLOSING
- close clamp
- LOADER_LOAD_UPING
- move load up to the unload position
- LOADER_LOAD_UNLOADING
- open clamp to unload, wait until unload position
- LOADER_LOAD_UNLOADING_OPEN
- check clamp is open
- LOADER_LOAD_EMPTY_OPEN
- open clamp due to no elements to pick
-
-Events:
- elevator_succeed
- asserv success result
- elevator_failed
- asserv failure result
- elevator_unload_position
- elevator above unloading position
- clamp_failed
- asserv failure result
- gate_succeed
- asserv success result
- gate_failed
- asserv failure result
- loader_element
- element sensed between clamp to be loaded
- loader_down
- make the loader ready to load an element
- loader_up
- protect the clamp by moving it up
- loader_downed
- posted when loader successfully moved down
- loader_uped
- posted when loader successfully moved up
- loader_errored
- posted when loader in an unknown state due to error while moving up
- loader_black
- loader blocked, due to a black corn
-
-LOADER_IDLE:
- start -> LOADER_WAIT_JACK_IN
-
-LOADER_WAIT_JACK_IN:
- jack_inserted_into_bot -> LOADER_WAIT_JACK_OUT
- hola_start -> LOADER_IDLE
-
-LOADER_WAIT_JACK_OUT:
- jack_removed_from_bot -> LOADER_INIT_ELEVATOR_ZERO
- find elevator zero
- close clamp
- find gate zero
-
-LOADER_INIT_ELEVATOR_ZERO:
- elevator_succeed -> LOADER_INIT_CLAMP_CLOSE
-
-LOADER_INIT_CLAMP_CLOSE:
- clamp_succeed -> LOADER_INIT_CLAMP_ZERO
- move elevator up
- find clamp zero
-
-LOADER_INIT_CLAMP_ZERO:
- clamp_succeed -> LOADER_INIT_GATE_ZERO
-
-LOADER_INIT_GATE_ZERO:
- gate_succeed -> LOADER_INIT_ELEVATOR_UP
-
-LOADER_INIT_ELEVATOR_UP:
- elevator_succeed -> LOADER_INIT_GATE_WAIT
- elevator_failed -> LOADER_IDLE
- initialisation failure
-
-LOADER_INIT_GATE_WAIT:
- state_timeout -> LOADER_INIT_GATE_CLOSE
- close gate
-
-LOADER_INIT_GATE_CLOSE:
- gate_succeed -> LOADER_UP
-
-LOADER_UP:
- loader_down -> LOADER_DOWNING
- move down
-
-LOADER_DOWN:
- loader_up -> LOADER_UPING
- move up
- loader_element -> LOADER_LOAD_CLOSING
- clamp
-
-LOADER_ERROR:
- loader_down -> LOADER_ERROR_DOWNING
- move down
- loader_up -> LOADER_ERROR_UPING
- move up
-
-LOADER_ERROR_DOWNING:
- elevator_succeed -> LOADER_ERROR_DOWNING_OPEN
- release elevator motor
- open clamp
- elevator_failed -> LOADER_ERROR
- post loader_errored event
- state_timeout -> LOADER_ERROR
- post loader_errored event
-
-LOADER_ERROR_DOWNING_OPEN:
- clamp_succeed -> LOADER_DOWN
- post loader_downed event
- clamp_failed -> LOADER_ERROR
- post loader_errored event
- state_timeout -> LOADER_ERROR
- post loader_errored event
-
-LOADER_ERROR_UPING:
- elevator_succeed -> LOADER_ERROR_UPING_OPEN
- open clamp
- elevator_failed -> LOADER_ERROR
- post loader_errored event
- state_timeout -> LOADER_ERROR
- post loader_errored event
-
-LOADER_ERROR_UPING_OPEN:
- clamp_succeed -> LOADER_UP
- post loader_uped event
- clamp_failed -> LOADER_ERROR
- post loader_errored event
- state_timeout -> LOADER_ERROR
- post loader_errored event
-
-LOADER_DOWNING:
- elevator_succeed -> LOADER_DOWN
- release elevator motor
- post loader_downed event
- elevator_failed -> LOADER_ERROR
- post loader_errored event
- loader_element -> LOADER_ERROR
- emergency stop
- post loader_errored event
- loader_up -> LOADER_UPING
- move up
-
-LOADER_UPING:
- elevator_succeed -> LOADER_UP
- post loader_uped event
- elevator_failed -> LOADER_ERROR
- post loader_errored event
- loader_down -> LOADER_DOWNING
- move down
-
-LOADER_LOAD_CLOSING:
- clamp_succeed: full -> LOADER_LOAD_UPING
- move up
- count one element
- clamp_succeed: empty -> LOADER_LOAD_EMPTY_OPEN
- open clamp
-
-LOADER_LOAD_UPING:
- elevator_unload_position -> LOADER_LOAD_UNLOADING
- open clamp
- elevator_failed -> LOADER_ERROR
- post loader_errored or loader_black event
- remove one element
- open clamp
-
-LOADER_LOAD_UNLOADING:
- elevator_succeed -> LOADER_LOAD_UNLOADING_OPEN
- elevator_failed -> LOADER_ERROR
- post loader_errored event
- open clamp
-
-LOADER_LOAD_UNLOADING_OPEN:
- clamp_succeed: down -> LOADER_DOWNING
- move down
- clamp_succeed: up -> LOADER_UPING
- move up
- clamp_failed -> LOADER_ERROR
- post loader_errored event
-
-LOADER_LOAD_EMPTY_OPEN:
- clamp_succeed: down -> LOADER_DOWN
- post loader_downed event
- clamp_succeed: up -> LOADER_UPING
- move up
- clamp_failed -> LOADER_ERROR
- post loader_errored event
-
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index 35ee83c1..dc98d12d 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -35,6 +35,8 @@
/* AVR include, non HOST */
#ifndef HOST
# include "switch.h" /* Manage switches (jack, color selector) */
+#else
+#include <string.h>
#endif /* HOST */
#include "main_timer.h"
@@ -44,7 +46,8 @@
#include "mimot.h"
#include "twi_master.h"
#include "eeprom.h" /* Parameters loaded/stored in the EEPROM */
-#include "fsm.h" /* fsm_* */
+#define FSM_NAME AI
+#include "fsm.h"
#include "bot.h"
#include "servo_pos.h"
#include "usdist.h"
@@ -144,16 +147,18 @@ main_event_to_fsm (void)
{
/* If an event is handled, stop generating any other event, because a
* transition may have invalidated the current robot state. */
-#define FSM_HANDLE_EVENT(fsm, event) \
- do { if (fsm_handle_event ((fsm), (event))) return; } while (0)
-#define FSM_HANDLE_TIMEOUT(fsm) \
- do { if (fsm_handle_timeout (fsm)) return; } while (0)
+#define FSM_HANDLE_E(fsm, event) \
+ do { if (FSM_HANDLE (fsm, event)) return; } while (0)
+#define FSM_HANDLE_VAR_E(fsm, event) \
+ do { if (FSM_HANDLE_VAR (fsm, event)) return; } while (0)
+#define FSM_HANDLE_TIMEOUT_E(fsm) \
+ do { if (FSM_HANDLE_TIMEOUT (fsm)) return; } while (0)
/* Update FSM timeouts. */
- FSM_HANDLE_TIMEOUT (&ai_fsm);
+ FSM_HANDLE_TIMEOUT_E (AI);
/* If we have entering this function, last command of the asserv board has
* been aquited. */
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_asserv_last_cmd_ack);
+ FSM_HANDLE_E (AI, asserv_last_cmd_ack);
asserv_status_e
move_status = none,
@@ -171,50 +176,50 @@ main_event_to_fsm (void)
/* Check commands move status. */
if (move_status == success)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_bot_move_succeed);
+ FSM_HANDLE_E (AI, bot_move_succeed);
else if (move_status == failure)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_bot_move_failed);
+ FSM_HANDLE_E (AI, bot_move_failed);
if (motor0_status == success)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_elevator_succeed);
+ FSM_HANDLE_E (AI, elevator_succeed);
else if (motor0_status == failure)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_elevator_failed);
+ FSM_HANDLE_E (AI, elevator_failed);
if (motor1_status == success)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_gate_succeed);
+ FSM_HANDLE_E (AI, gate_succeed);
else if (motor1_status == failure)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_gate_failed);
+ FSM_HANDLE_E (AI, gate_failed);
if (motorm0_status == success && motorm1_status == success)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_clamp_succeed);
+ FSM_HANDLE_E (AI, clamp_succeed);
else if (motorm0_status == failure || motorm1_status == failure)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_clamp_failed);
+ FSM_HANDLE_E (AI, clamp_failed);
/* Check positions. */
if (asserv_get_motor0_position () > BOT_ELEVATOR_UNLOAD_STEP)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_elevator_unload_position);
+ FSM_HANDLE_E (AI, elevator_unload_position);
/* Contacts. */
if (asserv_get_motor0_position () < BOT_ELEVATOR_REST_STEP
&& (!IO_GET (CONTACT_BUMPER0) || !IO_GET (CONTACT_BUMPER1)))
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_loader_element);
+ FSM_HANDLE_E (AI, loader_element);
if (!IO_GET (CONTACT_STRATEGY))
{
if (switch_get_color ())
loader_down ();
else
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_hola_start);
+ FSM_HANDLE_E (AI, hola_start);
}
/* Jack */
if (switch_get_jack ())
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_jack_removed_from_bot);
+ FSM_HANDLE_E (AI, jack_removed_from_bot);
else
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_jack_inserted_into_bot);
+ FSM_HANDLE_E (AI, jack_inserted_into_bot);
if (init_match_is_started)
{
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_init_match_is_started);
+ FSM_HANDLE_E (AI, init_match_is_started);
/* This must be done in the last part of this block. */
init_match_is_started = 0;
@@ -229,7 +234,7 @@ main_event_to_fsm (void)
/* We need to save the event before reseting it */
uint8_t save_event = main_pop_event ();
/* Post the event */
- FSM_HANDLE_EVENT (&ai_fsm, save_event);
+ FSM_HANDLE_VAR_E (AI, save_event);
}
/* Check obstacles. */
@@ -240,7 +245,7 @@ main_event_to_fsm (void)
position_t robot_pos;
asserv_get_position (&robot_pos);
if (robot_pos.v.y < PG_FIELD_Y_MAX)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_in_field);
+ FSM_HANDLE_E (AI, in_field);
/* TODO: Check other sensors */
}
@@ -276,10 +281,8 @@ main_init (void)
usdist_init ();
/* Top. */
top_init ();
- /* Init FSM. */
- fsm_init (&ai_fsm);
/* Start FSM. */
- fsm_handle_event (&ai_fsm, AI_EVENT_start);
+ FSM_HANDLE (AI, start);
/* PWM module */
pwm_init ();
/* Servo pos init. */
@@ -305,12 +308,12 @@ main_loop (void)
uint8_t timer_count = main_timer_wait ();
if (main_stats_timer_)
proto_send1b('M', timer_count);
- if (timer_count == 1)
- {
- /* Main timer has reached overflow earlier!
- We are late and this is really bad. */
- TRACE (TRACE_MAIN_TIMER__LATE);
- }
+ if (timer_count == 1)
+ {
+ /* Main timer has reached overflow earlier!
+ We are late and this is really bad. */
+ TRACE (TRACE_MAIN_TIMER__LATE);
+ }
}
/* Update chrono. */
@@ -706,6 +709,18 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
int
main (int argc, char **argv)
{
+ /* produce AVR's FSM headers. */
+#ifdef HOST
+ int i;
+ if (argc > 1)
+ for (i = 1; i < argc; i++)
+ if (strcmp (argv[i], "--gen") == 0)
+ {
+ FSM_GENERATE (AVR, 0);
+ return 0;
+ }
+#endif
+
avr_init (argc, argv);
/* Initialize the main and its subsystems */
diff --git a/digital/io/src/move.c b/digital/io/src/move.c
index e4f59c39..fa65418d 100644
--- a/digital/io/src/move.c
+++ b/digital/io/src/move.c
@@ -22,6 +22,9 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+
+#define FSM_NAME AI
+
#include "common.h"
#include "move.h"
#include "fsm.h"
@@ -29,9 +32,25 @@
#include "radar.h"
#include "asserv.h"
#include "main.h"
-
+#include "events.h"
+#include "playground.h"
+#include "loader.h"
#include "modules/path/path.h"
#include "modules/utils/utils.h"
+#include "main.h" /* main_post_event_for_top_fsm */
+#include "modules/math/fixed/fixed.h" /* fixed_* */
+#include "modules/trace/trace.h"
+#include "debug.host.h"
+#include <math.h>
+
+#define MOVE_LOADER_UNBLOCKING_DISTANCE 70
+
+/** Used to define a grid. If robot is outside the grid, be careful. */
+#define MOVE_GRID_X 450
+#define MOVE_GRID_X_ORIGIN 150
+#define MOVE_GRID_Y 250
+#define MOVE_GRID_Y_ORIGIN 128
+#define MOVE_GRID_MARGIN 50
/**
* Internal data used by the move FSM.
@@ -50,7 +69,7 @@ move_start (position_t position, uint8_t backward)
/* Reset try counter. */
move_data.try_again_counter = 3;
/* Start the FSM. */
- fsm_handle_event (&ai_fsm, AI_EVENT_move_start);
+ FSM_HANDLE (AI, move_start);
}
void
@@ -65,7 +84,7 @@ move_start_noangle (vect_t position, uint8_t backward, int16_t shorten)
/* Reset try counter. */
move_data.try_again_counter = 3;
/* Start the FSM. */
- fsm_handle_event (&ai_fsm, AI_EVENT_move_start);
+ FSM_HANDLE (AI, move_start);
}
void
@@ -80,15 +99,591 @@ move_obstacles_update (void)
uint8_t
move_check_obstacles (void)
{
- if (fsm_can_handle_event (&ai_fsm, AI_EVENT_obstacle_in_front))
+ if (FSM_CAN_HANDLE (AI, obstacle_in_front))
{
position_t robot_pos;
asserv_get_position (&robot_pos);
if (radar_blocking (&robot_pos.v, &move_data.step, main_obstacles_pos,
main_obstacles_nb))
- if (fsm_handle_event (&ai_fsm, AI_EVENT_obstacle_in_front))
+ if (FSM_HANDLE (AI, obstacle_in_front))
return 1;
}
return 0;
}
+/* Define FSM */
+FSM_STATES (
+ /* waiting for the beginning of the move FSM. */
+ MOVE_IDLE,
+ /* rotating towards next point. */
+ MOVE_ROTATING,
+ /* moving to a position (intermediate or final). */
+ MOVE_MOVING,
+ /* moving backward to go away from what is blocking the bot. */
+ MOVE_MOVING_BACKWARD_TO_TURN_FREELY,
+ /* waiting for obstacle to disappear. */
+ MOVE_WAIT_FOR_CLEAR_PATH,
+ /* moving loader up and move backward to unblock loader. */
+ MOVE_LOADER_UNBLOCKING_UPING,
+ /* moving loader down. */
+ MOVE_LOADER_UNBLOCKING_DOWNING)
+
+FSM_EVENTS (
+ /* initialize the FSM and start the movement directly. */
+ move_start,
+ /* the bot has seen something (front is the same when going backward). */
+ obstacle_in_front)
+
+FSM_START_WITH (MOVE_IDLE)
+
+/** Go to current step, low level function. */
+static void
+move_go (void)
+{
+ vect_t dst = move_data.step;
+ /* Modify final point if requested. */
+ if (move_data.final_move && move_data.shorten)
+ {
+ /* Compute a vector from destination to robot with lenght
+ * 'shorten'. */
+ position_t robot_position;
+ asserv_get_position (&robot_position);
+ vect_t v = robot_position.v;
+ vect_sub (&v, &move_data.step);
+ int16_t d = vect_norm (&v);
+ if (d > move_data.shorten)
+ {
+ vect_scale_f824 (&v, 0x1000000 / d * move_data.shorten);
+ vect_translate (&dst, &v);
+ }
+ }
+ if (move_data.step_with_angle)
+ asserv_goto_xya (dst.x, dst.y, move_data.step_angle,
+ move_data.step_backward);
+ else
+ asserv_goto (dst.x, dst.y, move_data.step_backward);
+}
+
+/** Go or rotate toward position, returns 1 for linear move, 2 for angular
+ * move. */
+static uint8_t
+move_go_or_rotate (vect_t dst, uint16_t angle, uint8_t with_angle,
+ uint8_t backward)
+{
+ position_t robot_position;
+ /* Remember step. */
+ move_data.step = dst;
+ move_data.step_angle = angle;
+ move_data.step_with_angle = with_angle;
+ move_data.step_backward = backward;
+ /* Compute angle to destination. */
+ asserv_get_position (&robot_position);
+ vect_t v = dst; vect_sub (&v, &robot_position.v);
+ uint16_t dst_angle = atan2 (v.y, v.x) * ((1l << 16) / (2 * M_PI));
+ if (backward & ASSERV_BACKWARD)
+ dst_angle += 0x8000;
+ if ((backward & ASSERV_REVERT_OK)
+ && (dst_angle ^ robot_position.a) & 0x8000)
+ dst_angle += 0x8000;
+ int16_t diff = dst_angle - robot_position.a;
+ /* Move or rotate. */
+ if (UTILS_ABS (diff) < 0x1000)
+ {
+ loader_down ();
+ move_go ();
+ return 1;
+ }
+ else
+ {
+ loader_up ();
+ asserv_goto_angle (dst_angle);
+ return 2;
+ }
+}
+
+/** Go to next position computed by path module, to be called by
+ * move_path_init and move_path_next. Returns 1 for linear move, 2 for angular
+ * move. */
+static uint8_t
+move_go_to_next (vect_t dst)
+{
+ uint8_t r;
+ /* If it is not the last position. */
+ if (dst.x != move_data.final.v.x || dst.y != move_data.final.v.y)
+ {
+ /* Not final position. */
+ move_data.final_move = 0;
+ /* Goto without angle. */
+ r = move_go_or_rotate (dst, 0, 0, move_data.backward_movement_allowed
+ | (move_data.slow ? ASSERV_REVERT_OK : 0));
+ }
+ else
+ {
+ /* Final position. */
+ move_data.final_move = 1;
+ /* Goto with angle if requested. */
+ r = move_go_or_rotate (dst, move_data.final.a, move_data.with_angle,
+ move_data.backward_movement_allowed);
+ }
+ TRACE (TRACE_MOVE__GO_TO, dst.x, dst.y);
+ /* Next time, do not use slow. */
+ move_data.slow = 0;
+ return r;
+}
+
+/** Update and go to first position, return non zero if a path is found, 1 for
+ * linear move, 2 for angular move. */
+static uint8_t
+move_path_init (void)
+{
+ uint8_t found;
+ vect_t dst;
+ /* Get the current position */
+ position_t current_pos;
+ asserv_get_position (&current_pos);
+ /* Give the current position of the bot to the path module */
+ path_endpoints (current_pos.v, move_data.final.v);
+ /* Update the path module */
+ move_data.slow = 0;
+ path_update ();
+ found = path_get_next (&dst);
+ /* If not found, try to escape. */
+ if (!found)
+ {
+ move_data.slow = 1;
+ path_escape (8);
+ path_update ();
+ found = path_get_next (&dst);
+ }
+ /* If not on the grid, slow down. */
+ int16_t mx = current_pos.v.x % MOVE_GRID_X;
+ int16_t my = current_pos.v.y % MOVE_GRID_Y;
+ if (mx < MOVE_GRID_X_ORIGIN - MOVE_GRID_MARGIN
+ || mx > MOVE_GRID_X_ORIGIN + MOVE_GRID_MARGIN
+ || my < MOVE_GRID_Y_ORIGIN - MOVE_GRID_MARGIN
+ || my > MOVE_GRID_Y_ORIGIN + MOVE_GRID_MARGIN
+ )
+ move_data.slow = 1;
+ /* If found, go. */
+ if (found)
+ {
+ return move_go_to_next (dst);
+ }
+ else
+ {
+ /* Error, not final move. */
+ move_data.final_move = 0;
+ return 0;
+ }
+}
+
+/** Go to next position in path. Returns 1 for linear move, 2 for angular
+ * move. */
+static uint8_t
+move_path_next (void)
+{
+ vect_t dst;
+ path_get_next (&dst);
+ return move_go_to_next (dst);
+}
+
+FSM_TRANS (MOVE_IDLE, move_start,
+ /* rotate towards next position. */
+ path_found_rotate, MOVE_ROTATING,
+ /* move to next position. */
+ path_found, MOVE_MOVING,
+ /* post failure event. */
+ no_path_found, MOVE_IDLE)
+{
+ uint8_t next = move_path_init ();
+ if (next)
+ {
+ if (next == 2)
+ return FSM_NEXT (MOVE_IDLE, move_start, path_found_rotate);
+ else
+ return FSM_NEXT (MOVE_IDLE, move_start, path_found);
+ }
+ else
+ {
+ main_post_event (FSM_EVENT (AI, move_fsm_failed));
+ return FSM_NEXT (MOVE_IDLE, move_start, no_path_found);
+ }
+}
+
+/*
+ * move to next position.
+ */
+FSM_TRANS (MOVE_ROTATING,
+ bot_move_succeed,
+ MOVE_MOVING)
+{
+ loader_down ();
+ move_go ();
+ return FSM_NEXT (MOVE_ROTATING, bot_move_succeed);
+}
+
+/*
+ * move to next position.
+ */
+FSM_TRANS (MOVE_ROTATING,
+ bot_move_failed,
+ MOVE_MOVING)
+{
+ loader_down ();
+ move_go ();
+ return FSM_NEXT (MOVE_ROTATING, bot_move_failed);
+}
+
+/*
+ * move to next position.
+ */
+FSM_TRANS_TIMEOUT (MOVE_ROTATING, 1125,
+ MOVE_MOVING)
+{
+ loader_down ();
+ move_go ();
+ return FSM_NEXT_TIMEOUT (MOVE_ROTATING);
+}
+
+/*
+ * move backward
+ * loader up
+ */
+FSM_TRANS (MOVE_ROTATING,
+ loader_errored,
+ MOVE_LOADER_UNBLOCKING_UPING)
+{
+ asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
+ loader_up ();
+ return FSM_NEXT (MOVE_ROTATING, loader_errored);
+}
+
+FSM_TRANS (MOVE_MOVING, bot_move_succeed,
+ /* post success event. */
+ done, MOVE_IDLE,
+ /* rotate towards next position. */
+ path_found_rotate, MOVE_ROTATING,
+ /* move to next position. */
+ path_found, MOVE_MOVING,
+ /* post failure event */
+ no_path_found, MOVE_IDLE)
+{
+ if (move_data.final_move)
+ {
+ main_post_event (FSM_EVENT (AI, move_fsm_succeed));
+ return FSM_NEXT (MOVE_MOVING, bot_move_succeed, done);
+ }
+ else
+ {
+ uint8_t next = move_path_next ();
+ if (next == 2)
+ return FSM_NEXT (MOVE_MOVING, bot_move_succeed, path_found_rotate);
+ else
+ return FSM_NEXT (MOVE_MOVING, bot_move_succeed, path_found);
+ }
+ //return FSM_NEXT (MOVE_MOVING, bot_move_succeed, no_path_found);
+}
+
+
+/*
+ * reset final_move.
+ * move backward to turn freely.
+ */
+void
+move_MOVE_MOVING_bot_move_failed_MOVE_MOVING_BACKWARD_TO_TURN_FREELY ()
+{
+ move_data.final_move = 0;
+ /* Assume there is an obstacle in front of the robot. */
+ position_t robot_pos;
+ asserv_get_position (&robot_pos);
+ vect_t obstacle_pos;
+ int16_t dist = asserv_get_last_moving_direction () == 1
+ ? BOT_SIZE_FRONT + MOVE_REAL_OBSTACLE_RADIUS
+ : -(BOT_SIZE_BACK + MOVE_REAL_OBSTACLE_RADIUS);
+ vect_from_polar_uf016 (&obstacle_pos, dist, robot_pos.a);
+ vect_translate (&obstacle_pos, &robot_pos.v);
+ path_obstacle (0, obstacle_pos, MOVE_OBSTACLE_RADIUS, 0,
+ MOVE_OBSTACLE_VALIDITY);
+ /* Move backward to turn freely. */
+ asserv_move_linearly (asserv_get_last_moving_direction () == 1 ?
+ - 300 : 300);
+}
+
+FSM_TRANS (MOVE_MOVING,
+ bot_move_failed,
+ MOVE_MOVING_BACKWARD_TO_TURN_FREELY)
+{
+ move_MOVE_MOVING_bot_move_failed_MOVE_MOVING_BACKWARD_TO_TURN_FREELY ();
+ return FSM_NEXT (MOVE_MOVING, bot_move_failed);
+}
+
+/*
+ * reset final_move.
+ * move backward to turn freely.
+ */
+FSM_TRANS_TIMEOUT (MOVE_MOVING, 2250,
+ MOVE_MOVING_BACKWARD_TO_TURN_FREELY)
+{
+ move_MOVE_MOVING_bot_move_failed_MOVE_MOVING_BACKWARD_TO_TURN_FREELY ();
+ return FSM_NEXT_TIMEOUT (MOVE_MOVING);
+}
+
+FSM_TRANS (MOVE_MOVING, obstacle_in_front,
+ /* reset final_move.
+ * stop the bot. */
+ tryagain, MOVE_WAIT_FOR_CLEAR_PATH,
+ /* stop the bot.
+ * post failure event. */
+ tryout, MOVE_IDLE)
+{
+ move_data.final_move = 0;
+ asserv_stop_motor ();
+ if (--move_data.try_again_counter == 0)
+ {
+ main_post_event (FSM_EVENT (AI, move_fsm_failed));
+ return FSM_NEXT (MOVE_MOVING, obstacle_in_front, tryout);
+ }
+ else
+ return FSM_NEXT (MOVE_MOVING, obstacle_in_front, tryagain);
+}
+
+/*
+ * move backward
+ * loader up
+ */
+FSM_TRANS (MOVE_MOVING,
+ loader_errored,
+ MOVE_LOADER_UNBLOCKING_UPING)
+{
+ asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
+ loader_up ();
+ return FSM_NEXT (MOVE_MOVING, loader_errored);
+}
+
+FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed,
+ /* post failure event. */
+ tryout, MOVE_IDLE,
+ /* rotate towards next position. */
+ path_found_rotate, MOVE_ROTATING,
+ /* move to next position. */
+ path_found, MOVE_MOVING,
+ /* post failure event. */
+ no_path_found, MOVE_IDLE)
+{
+ if (--move_data.try_again_counter == 0)
+ {
+ main_post_event (FSM_EVENT (AI, move_fsm_failed));
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, tryout);
+ }
+ else
+ {
+ uint8_t next = move_path_init ();
+ if (next)
+ {
+ if (next == 2)
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, path_found_rotate);
+ else
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, path_found);
+ }
+ else
+ {
+ main_post_event (FSM_EVENT (AI, move_fsm_failed));
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, no_path_found);
+ }
+ }
+}
+
+FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed,
+ /* post failure event. */
+ tryout, MOVE_IDLE,
+ /* rotate towards next position. */
+ path_found_rotate, MOVE_ROTATING,
+ /* move to next position. */
+ path_found, MOVE_MOVING,
+ /* nothing to do. */
+ no_path_found_tryagain, MOVE_WAIT_FOR_CLEAR_PATH,
+ /* post failure event. */
+ no_path_found_tryout, MOVE_IDLE)
+{
+ if (--move_data.try_again_counter == 0)
+ {
+ main_post_event (FSM_EVENT (AI, move_fsm_failed));
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, tryout);
+ }
+ else
+ {
+ uint8_t next = move_path_init ();
+ if (next)
+ {
+ if (next == 2)
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, path_found_rotate);
+ else
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, path_found);
+ }
+ else
+ {
+ if (--move_data.try_again_counter == 0)
+ {
+ main_post_event (FSM_EVENT (AI, move_fsm_failed));
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_path_found_tryout);
+ }
+ else
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_path_found_tryagain);
+ }
+ }
+}
+
+FSM_TRANS_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, 255,
+ /* rotate towards next position. */
+ path_found_rotate, MOVE_ROTATING,
+ /* move to next position. */
+ path_found, MOVE_MOVING,
+ /* decrement counter. */
+ no_path_found_tryagain, MOVE_WAIT_FOR_CLEAR_PATH,
+ /* post failure. */
+ no_path_found_tryout, MOVE_IDLE)
+{
+ /* Try to move. */
+ uint8_t next = move_path_init ();
+ if (next)
+ {
+ if (next == 2)
+ return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, path_found_rotate);
+ else
+ return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, path_found);
+ }
+ else
+ {
+ /* Error, no new position, should we try again? */
+ if (--move_data.try_again_counter == 0)
+ {
+ main_post_event (FSM_EVENT (AI, move_fsm_failed));
+ return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, no_path_found_tryout);
+ }
+ else
+ return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, no_path_found_tryagain);
+ }
+}
+
+/*
+ * loader down
+ * reset unblocking retry counter
+ */
+FSM_TRANS (MOVE_LOADER_UNBLOCKING_UPING,
+ bot_move_succeed,
+ MOVE_LOADER_UNBLOCKING_DOWNING)
+{
+ loader_down ();
+ move_data.loader_unblocking_retry = 2;
+ return FSM_NEXT (MOVE_LOADER_UNBLOCKING_UPING, bot_move_succeed);
+}
+
+/*
+ * loader down
+ * reset unblocking retry counter
+ */
+FSM_TRANS (MOVE_LOADER_UNBLOCKING_UPING,
+ bot_move_failed,
+ MOVE_LOADER_UNBLOCKING_DOWNING)
+{
+ loader_down ();
+ move_data.loader_unblocking_retry = 2;
+ return FSM_NEXT (MOVE_LOADER_UNBLOCKING_UPING, bot_move_failed);
+}
+
+FSM_TRANS (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed,
+ /* rotate towards next position. */
+ path_found_rotate, MOVE_ROTATING,
+ /* move to next position. */
+ path_found, MOVE_MOVING,
+ /* post failure. */
+ no_path_found, MOVE_IDLE)
+{
+ /* Try to move. */
+ uint8_t next = move_path_init ();
+ if (next)
+ {
+ if (next == 2)
+ return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, path_found_rotate);
+ else
+ return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, path_found);
+ }
+ else
+ {
+ main_post_event (FSM_EVENT (AI, move_fsm_failed));
+ return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, no_path_found);
+ }
+}
+
+FSM_TRANS (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored,
+ /* move backward.
+ * loader up. */
+ tryagain, MOVE_LOADER_UNBLOCKING_UPING,
+ /* rotate towards next position. */
+ tryout_path_found_rotate, MOVE_ROTATING,
+ /* move to next position. */
+ tryout_path_found, MOVE_ROTATING,
+ /* post failure. */
+ tryout_no_path_found, MOVE_IDLE)
+{
+ if (--move_data.loader_unblocking_retry)
+ {
+ asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
+ loader_up ();
+ return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryagain);
+ }
+ else
+ {
+ /* Try to move. */
+ uint8_t next = move_path_init ();
+ if (next)
+ {
+ if (next == 2)
+ return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_path_found_rotate);
+ else
+ return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_path_found);
+ }
+ else
+ {
+ main_post_event (FSM_EVENT (AI, move_fsm_failed));
+ return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_no_path_found);
+ }
+ }
+}
+
+FSM_TRANS_TIMEOUT (MOVE_LOADER_UNBLOCKING_DOWNING, 450,
+ /* move backward.
+ * loader up. */
+ tryagain, MOVE_LOADER_UNBLOCKING_UPING,
+ /* rotate towards next position. */
+ tryout_path_found_rotate, MOVE_ROTATING,
+ /* move to next position. */
+ tryout_path_found, MOVE_ROTATING,
+ /* post failure. */
+ tryout_no_path_found, MOVE_IDLE)
+{
+ if (--move_data.loader_unblocking_retry)
+ {
+ asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
+ loader_up ();
+ return FSM_NEXT_TIMEOUT (MOVE_LOADER_UNBLOCKING_DOWNING, tryagain);
+ }
+ else
+ {
+ /* Try to move. */
+ uint8_t next = move_path_init ();
+ if (next)
+ {
+ if (next == 2)
+ return FSM_NEXT_TIMEOUT (MOVE_LOADER_UNBLOCKING_DOWNING, tryout_path_found_rotate);
+ else
+ return FSM_NEXT_TIMEOUT (MOVE_LOADER_UNBLOCKING_DOWNING, tryout_path_found);
+ }
+ else
+ {
+ main_post_event (FSM_EVENT (AI, move_fsm_failed));
+ return FSM_NEXT_TIMEOUT (MOVE_LOADER_UNBLOCKING_DOWNING, tryout_no_path_found);
+ }
+ }
+}
+
diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm
deleted file mode 100644
index add29b05..00000000
--- a/digital/io/src/move.fsm
+++ /dev/null
@@ -1,137 +0,0 @@
-# Move FSM
-# This FSM is responsible to move the bot from the current position to the
-# desired one. It will try to avoid obstacle and manage the case when the
-# asserv is blocked.
-move
- move with avoid obstacle.
-
-States:
- *MOVE_IDLE
- waiting for the beginning of the move FSM.
- MOVE_ROTATING [timeout=1125]
- rotating towards next point.
- MOVE_MOVING [timeout=2250]
- moving to a position (intermediate or final).
- MOVE_MOVING_BACKWARD_TO_TURN_FREELY
- moving backward to go away from what is blocking the bot.
- MOVE_WAIT_FOR_CLEAR_PATH [timeout=255]
- waiting for obstacle to disappear.
- MOVE_LOADER_UNBLOCKING_UPING
- moving loader up and move backward to unblock loader.
- MOVE_LOADER_UNBLOCKING_DOWNING [timeout=450]
- moving loader down.
-
-Events:
- move_start
- initialize the FSM and start the movement directly.
- obstacle_in_front
- the bot has seen something (front is the same when going backward).
-
-MOVE_IDLE:
- move_start: path_found_rotate -> MOVE_ROTATING
- rotate towards next position.
- move_start: path_found -> MOVE_MOVING
- move to next position.
- move_start: no_path_found -> MOVE_IDLE
- post failure event.
-
-MOVE_ROTATING:
- bot_move_succeed -> MOVE_MOVING
- move to next position.
- bot_move_failed -> MOVE_MOVING
- move to next position.
- state_timeout -> MOVE_MOVING
- move to next position.
- loader_errored -> MOVE_LOADER_UNBLOCKING_UPING
- move backward
- loader up
-
-MOVE_MOVING:
- bot_move_succeed: done -> MOVE_IDLE
- post success event.
- bot_move_succeed: path_found_rotate -> MOVE_ROTATING
- rotate towards next position.
- bot_move_succeed: path_found -> MOVE_MOVING
- move to next position.
- bot_move_succeed: no_path_found -> MOVE_IDLE
- post failure event.
- bot_move_failed -> MOVE_MOVING_BACKWARD_TO_TURN_FREELY
- reset final_move.
- move backward to turn freely.
- state_timeout -> MOVE_MOVING_BACKWARD_TO_TURN_FREELY
- reset final_move.
- move backward to turn freely.
- obstacle_in_front: tryagain -> MOVE_WAIT_FOR_CLEAR_PATH
- reset final_move.
- stop the bot.
- obstacle_in_front: tryout -> MOVE_IDLE
- stop the bot.
- post failure event.
- loader_errored -> MOVE_LOADER_UNBLOCKING_UPING
- move backward
- loader up
-
-MOVE_MOVING_BACKWARD_TO_TURN_FREELY:
- bot_move_succeed: tryout -> MOVE_IDLE
- post failure event.
- bot_move_succeed: path_found_rotate -> MOVE_ROTATING
- rotate towards next position.
- bot_move_succeed: path_found -> MOVE_MOVING
- move to next position.
- bot_move_succeed: no_path_found -> MOVE_IDLE
- post failure event.
- bot_move_failed: tryout -> MOVE_IDLE
- post failure event.
- bot_move_failed: path_found_rotate -> MOVE_ROTATING
- rotate towards next position.
- bot_move_failed: path_found -> MOVE_MOVING
- move to next position.
- bot_move_failed: no_path_found_tryagain -> MOVE_WAIT_FOR_CLEAR_PATH
- nothing to do.
- bot_move_failed: no_path_found_tryout -> MOVE_IDLE
- post failure event.
-
-MOVE_WAIT_FOR_CLEAR_PATH:
- state_timeout: path_found_rotate -> MOVE_ROTATING
- rotate towards next position.
- state_timeout: path_found -> MOVE_MOVING
- move to next position.
- state_timeout: no_path_found_tryagain -> .
- decrement counter.
- state_timeout: no_path_found_tryout -> MOVE_IDLE
- post failure.
-
-MOVE_LOADER_UNBLOCKING_UPING:
- bot_move_succeed -> MOVE_LOADER_UNBLOCKING_DOWNING
- loader down
- reset unblocking retry counter
- bot_move_failed -> MOVE_LOADER_UNBLOCKING_DOWNING
- loader down
- reset unblocking retry counter
-
-MOVE_LOADER_UNBLOCKING_DOWNING:
- loader_downed: path_found_rotate -> MOVE_ROTATING
- rotate towards next position.
- loader_downed: path_found -> MOVE_MOVING
- move to next position.
- loader_downed: no_path_found -> MOVE_IDLE
- post failure.
- loader_errored: tryagain -> MOVE_LOADER_UNBLOCKING_UPING
- move backward
- loader up
- loader_errored: tryout_path_found_rotate -> MOVE_ROTATING
- rotate towards next position.
- loader_errored: tryout_path_found -> MOVE_ROTATING
- move to next position.
- loader_errored: tryout_no_path_found -> MOVE_IDLE
- post failure.
- state_timeout: tryagain -> MOVE_LOADER_UNBLOCKING_UPING
- move backward
- loader up
- state_timeout: tryout_path_found_rotate -> MOVE_ROTATING
- rotate towards next position.
- state_timeout: tryout_path_found -> MOVE_ROTATING
- move to next position.
- state_timeout: tryout_no_path_found -> MOVE_IDLE
- post failure.
-
diff --git a/digital/io/src/top.c b/digital/io/src/top.c
index 5bfeaa4d..bc6eb20e 100644
--- a/digital/io/src/top.c
+++ b/digital/io/src/top.c
@@ -22,6 +22,10 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+
+#define FSM_NAME AI
+
+#include "fsm.h"
#include "common.h"
#include "defs.h"
#include "top.h"
@@ -88,3 +92,358 @@ top_collect (uint8_t force)
}
}
+FSM_INIT
+
+FSM_STATES (IDLE,
+ WAIT_INIT_TO_FINISH,
+ FIRST_GO_BEGIN_OF_LINE_FAST,
+ FIRST_GO_END_OF_LINE_FAST,
+ FIRST_GO_END_OF_LINE_SLOW,
+ UNLOAD,
+ UNLOAD_LOADER_UP,
+ UNLOAD_FACE_BIN,
+ UNLOAD_FACE_BIN_UNBLOCK,
+ UNLOAD_BACK_BIN,
+ UNLOAD_UNLOAD,
+ COLLECT,
+ COLLECT_SLOW_MOTION,
+ COLLECT_BLACK)
+
+FSM_EVENTS (start,
+ move_fsm_succeed,
+ move_fsm_failed,
+ bot_move_succeed,
+ bot_move_failed,
+ init_match_is_started,
+ in_field,
+ asserv_last_cmd_ack,
+ clamp_succeed)
+
+FSM_START_WITH (IDLE)
+
+/* Nothing to do. */
+FSM_TRANS (IDLE, start, WAIT_INIT_TO_FINISH)
+{
+ return FSM_NEXT (IDLE, start);
+}
+
+/*
+ * the match start
+ * set fast speed
+ * go to first sequence begin of line
+ */
+FSM_TRANS (WAIT_INIT_TO_FINISH,
+ init_match_is_started,
+ FIRST_GO_BEGIN_OF_LINE_FAST)
+{
+ asserv_set_speed (BOT_MOVE_FAST);
+ move_start_noangle (PG_VECT (375, 1503), 0, 0);
+ return FSM_NEXT (WAIT_INIT_TO_FINISH, init_match_is_started);
+}
+
+/*
+ * go to end of line
+ * loader down
+ */
+FSM_TRANS (FIRST_GO_BEGIN_OF_LINE_FAST,
+ move_fsm_succeed,
+ FIRST_GO_END_OF_LINE_FAST)
+{
+ move_start_noangle (PG_VECT (2625, 253), 0, 0);
+ loader_down ();
+ return FSM_NEXT (FIRST_GO_BEGIN_OF_LINE_FAST, move_fsm_succeed);
+}
+
+/*
+ * set slow speed
+ * retry
+ */
+FSM_TRANS (FIRST_GO_BEGIN_OF_LINE_FAST,
+ move_fsm_failed,
+ FIRST_GO_BEGIN_OF_LINE_FAST)
+{
+ asserv_set_speed (BOT_MOVE_SLOW);
+ move_start_noangle (PG_VECT (375, 1503), 0, 0);
+ return FSM_NEXT (FIRST_GO_BEGIN_OF_LINE_FAST, move_fsm_failed);
+}
+
+/*
+ * set slow speed
+ */
+FSM_TRANS (FIRST_GO_END_OF_LINE_FAST,
+ in_field,
+ FIRST_GO_END_OF_LINE_SLOW)
+{
+ asserv_set_speed (BOT_MOVE_SLOW);
+ return FSM_NEXT (FIRST_GO_END_OF_LINE_FAST, in_field);
+}
+
+/*
+ * set slow speed
+ * move loader up
+ */
+FSM_TRANS (FIRST_GO_END_OF_LINE_FAST,
+ move_fsm_succeed,
+ UNLOAD_LOADER_UP)
+{
+ asserv_set_speed (BOT_MOVE_SLOW);
+ loader_up ();
+ return FSM_NEXT (FIRST_GO_END_OF_LINE_FAST, move_fsm_succeed);
+}
+
+/*
+ * set slow speed
+ * retry
+ */
+FSM_TRANS (FIRST_GO_END_OF_LINE_FAST,
+ move_fsm_failed,
+ FIRST_GO_END_OF_LINE_SLOW)
+{
+ asserv_set_speed (BOT_MOVE_SLOW);
+ move_start_noangle (PG_VECT (2625, 253), 0, 0);
+ return FSM_NEXT (FIRST_GO_END_OF_LINE_FAST, move_fsm_failed);
+}
+
+/*
+ * move loader up
+ */
+FSM_TRANS (FIRST_GO_END_OF_LINE_SLOW,
+ move_fsm_succeed,
+ UNLOAD_LOADER_UP)
+{
+ loader_up ();
+ return FSM_NEXT (FIRST_GO_END_OF_LINE_SLOW, move_fsm_succeed);
+}
+
+/*
+ * retry
+ */
+FSM_TRANS (FIRST_GO_END_OF_LINE_SLOW,
+ move_fsm_failed,
+ FIRST_GO_END_OF_LINE_SLOW)
+{
+ move_start_noangle (PG_VECT (2625, 253), 0, 0);
+ return FSM_NEXT (FIRST_GO_END_OF_LINE_SLOW, move_fsm_failed);
+}
+
+/*
+ * move loader up
+ */
+FSM_TRANS (UNLOAD, move_fsm_succeed, UNLOAD_LOADER_UP)
+{
+ loader_up ();
+ return FSM_NEXT (UNLOAD, move_fsm_succeed);
+}
+
+/*
+ * retry
+ */
+FSM_TRANS (UNLOAD, move_fsm_failed, UNLOAD)
+{
+ move_start_noangle (PG_VECT (2625, 253), 0, 0);
+ return FSM_NEXT (UNLOAD, move_fsm_failed);
+}
+
+/*
+ * turn toward bin
+ */
+FSM_TRANS (UNLOAD_LOADER_UP, loader_uped, UNLOAD_FACE_BIN)
+{
+ asserv_goto_angle (PG_A_DEG (90));
+ return FSM_NEXT (UNLOAD_LOADER_UP, loader_uped);
+}
+
+/*
+ * turn toward bin
+ */
+FSM_TRANS (UNLOAD_LOADER_UP, loader_errored, UNLOAD_FACE_BIN)
+{
+ asserv_goto_angle (PG_A_DEG (90));
+ return FSM_NEXT (UNLOAD_LOADER_UP, loader_errored);
+}
+
+/*
+ * go backward to bin
+ */
+FSM_TRANS (UNLOAD_FACE_BIN, bot_move_succeed, UNLOAD_BACK_BIN)
+{
+ asserv_move_linearly (-(128 + 250 / 2 - BOT_SIZE_BACK - 50));
+ return FSM_NEXT (UNLOAD_FACE_BIN, bot_move_succeed);
+}
+
+/*
+ * move backward
+ */
+FSM_TRANS (UNLOAD_FACE_BIN, bot_move_failed, UNLOAD_FACE_BIN_UNBLOCK)
+{
+ asserv_move_linearly (-40);
+ return FSM_NEXT (UNLOAD_FACE_BIN, bot_move_failed);
+}
+
+/*
+ * turn toward bin
+ */
+FSM_TRANS (UNLOAD_FACE_BIN_UNBLOCK, bot_move_succeed, UNLOAD_FACE_BIN)
+{
+ asserv_goto_angle (PG_A_DEG (90));
+ return FSM_NEXT (UNLOAD_FACE_BIN_UNBLOCK, bot_move_succeed);
+}
+
+/*
+ * turn toward bin
+ */
+FSM_TRANS (UNLOAD_FACE_BIN_UNBLOCK, bot_move_failed, UNLOAD_FACE_BIN)
+{
+ asserv_goto_angle (PG_A_DEG (90));
+ return FSM_NEXT (UNLOAD_FACE_BIN_UNBLOCK, bot_move_failed);
+}
+
+/*
+ * unload
+ */
+FSM_TRANS (UNLOAD_BACK_BIN, bot_move_succeed, UNLOAD_UNLOAD)
+{
+ asserv_move_motor1_absolute (0, BOT_GATE_SPEED);
+ return FSM_NEXT (UNLOAD_BACK_BIN, bot_move_succeed);
+}
+
+/*
+ * unload
+ */
+FSM_TRANS (UNLOAD_BACK_BIN, bot_move_failed, UNLOAD_UNLOAD)
+{
+ asserv_move_motor1_absolute (0, BOT_GATE_SPEED);
+ return FSM_NEXT (UNLOAD_BACK_BIN, bot_move_failed);
+}
+
+/*
+ * close gate
+ * loader down
+ * choose best food to collect
+ */
+FSM_TRANS_TIMEOUT (UNLOAD_UNLOAD, 255, COLLECT)
+{
+ loader_elements = 0;
+ asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED);
+ loader_down ();
+ top_collect (1);
+ return FSM_NEXT_TIMEOUT (UNLOAD_UNLOAD);
+}
+
+/*
+ * post loader_element event
+ */
+FSM_TRANS (COLLECT, move_fsm_succeed,
+ unload, UNLOAD,
+ collect, COLLECT,
+ slow_motion, COLLECT_SLOW_MOTION)
+{
+ int16_t slow_motion = food_slow_motion (top_food);
+ if (slow_motion)
+ {
+ FSM_HANDLE (AI, loader_element);
+ return FSM_NEXT (COLLECT, move_fsm_succeed, slow_motion);
+ }
+ else if (top_collect (0))
+ return FSM_NEXT (COLLECT, move_fsm_succeed, collect);
+ else
+ return FSM_NEXT (COLLECT, move_fsm_succeed, unload);
+}
+
+/*
+ * cancel this element
+ */
+FSM_TRANS (COLLECT, move_fsm_failed,
+ unload, UNLOAD,
+ collect, COLLECT)
+{
+ if (top_collect (0))
+ {
+ food_cancel (top_food);
+ return FSM_NEXT (COLLECT, move_fsm_failed, collect);
+ }
+ else
+ return FSM_NEXT (COLLECT, move_fsm_failed, unload);
+}
+
+/*
+ * move backward
+ * mark as black
+ */
+FSM_TRANS (COLLECT, loader_black, COLLECT_BLACK)
+{
+ asserv_move_linearly (-90);
+ food_black (top_food);
+ return FSM_NEXT (COLLECT, loader_black);
+}
+
+/*
+ * collect
+ */
+FSM_TRANS (COLLECT_SLOW_MOTION, loader_downed,
+ unload, UNLOAD,
+ collect, COLLECT)
+{
+ if (top_collect (0))
+ return FSM_NEXT (COLLECT_SLOW_MOTION, loader_downed, collect);
+ else
+ return FSM_NEXT (COLLECT_SLOW_MOTION, loader_downed, unload);
+}
+
+/*
+ * same as above
+ */
+FSM_TRANS (COLLECT_SLOW_MOTION, loader_errored,
+ unload, UNLOAD,
+ collect, COLLECT)
+{
+ if (top_collect (0))
+ return FSM_NEXT (COLLECT_SLOW_MOTION, loader_errored, collect);
+ else
+ return FSM_NEXT (COLLECT_SLOW_MOTION, loader_errored, unload);
+}
+
+/*
+ * same as above
+ */
+FSM_TRANS_TIMEOUT (COLLECT_SLOW_MOTION, 450,
+ unload, UNLOAD,
+ collect, COLLECT)
+{
+ if (top_collect (0))
+ return FSM_NEXT_TIMEOUT (COLLECT_SLOW_MOTION, collect);
+ else
+ return FSM_NEXT_TIMEOUT (COLLECT_SLOW_MOTION, unload);
+}
+
+/*
+ * move backward
+ * mark as black
+ */
+FSM_TRANS (COLLECT_SLOW_MOTION, loader_black, COLLECT_BLACK)
+{
+ asserv_move_linearly (-90);
+ food_black (top_food);
+ return FSM_NEXT (COLLECT_SLOW_MOTION, loader_black);
+}
+
+FSM_TRANS (COLLECT_BLACK, bot_move_succeed,
+ unload, UNLOAD,
+ collect, COLLECT)
+{
+ if (top_collect (0))
+ return FSM_NEXT (COLLECT_BLACK, bot_move_succeed, collect);
+ else
+ return FSM_NEXT (COLLECT_BLACK, bot_move_succeed, unload);
+}
+
+FSM_TRANS (COLLECT_BLACK, bot_move_failed,
+ unload, UNLOAD,
+ collect, COLLECT)
+{
+ if (top_collect (0))
+ return FSM_NEXT (COLLECT_BLACK, bot_move_failed, collect);
+ else
+ return FSM_NEXT (COLLECT_BLACK, bot_move_failed, unload);
+}
+
diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm
deleted file mode 100644
index 277f0a5a..00000000
--- a/digital/io/src/top.fsm
+++ /dev/null
@@ -1,162 +0,0 @@
-# Top FSM.
-top
- Main FSM calling other FSM.
-
-States:
- *IDLE
- waiting for the beginning of the top FSM.
- WAIT_INIT_TO_FINISH
- waiting for the end of init.
- FIRST_GO_BEGIN_OF_LINE_FAST
- first sequence, go to start of diagonal line
- FIRST_GO_END_OF_LINE_FAST
- first sequence, go to end of line
- FIRST_GO_END_OF_LINE_SLOW
- slow down to take elements
- UNLOAD
- go to unloading position
- UNLOAD_LOADER_UP
- move loader to up position
- UNLOAD_FACE_BIN
- orient robot to unload angle
- UNLOAD_FACE_BIN_UNBLOCK
- move backward to unblock
- UNLOAD_BACK_BIN
- move back to unload bin
- UNLOAD_UNLOAD[timeout=225]
- unloading, wait
- COLLECT
- collecting elements
- COLLECT_SLOW_MOTION[timeout=450]
- picking element before bumper contact
- COLLECT_BLACK
- moving backward after trying to collect a black corn
-
-Events:
- start
- initialize the FSM.
- move_fsm_succeed
- when the move FSM returns with success.
- move_fsm_failed
- when the move FSM returns with an error.
- bot_move_succeed
- when the bot has succeed its asserv movement.
- bot_move_failed
- when the bot has failed its asserv movement.
- init_match_is_started
- when the init FSM has finished and the match start.
- in_field
- the robot is in the zone containing interesting elements.
- state_timeout
- timeout in cycle count.
- asserv_last_cmd_ack
- last command sent to the asserv board has been acquitted.
- clamp_succeed
- asserv success result
-
-import init.fsm
-import hola.fsm
-import loader.fsm
-import move.fsm
-
-IDLE:
- start -> WAIT_INIT_TO_FINISH
- nothing to do.
-
-WAIT_INIT_TO_FINISH:
- init_match_is_started -> FIRST_GO_BEGIN_OF_LINE_FAST
- the match start
- set fast speed
- go to first sequence begin of line
-
-FIRST_GO_BEGIN_OF_LINE_FAST:
- move_fsm_succeed -> FIRST_GO_END_OF_LINE_FAST
- go to end of line
- loader down
- move_fsm_failed -> .
- set slow speed
- retry
-
-FIRST_GO_END_OF_LINE_FAST:
- in_field -> FIRST_GO_END_OF_LINE_SLOW
- set slow speed
- move_fsm_succeed -> UNLOAD_LOADER_UP
- set slow speed
- move loader up
- move_fsm_failed -> FIRST_GO_END_OF_LINE_SLOW
- set slow speed
- retry
-
-FIRST_GO_END_OF_LINE_SLOW:
- move_fsm_succeed -> UNLOAD_LOADER_UP
- move loader up
- move_fsm_failed -> FIRST_GO_END_OF_LINE_SLOW
- retry
-
-UNLOAD:
- move_fsm_succeed -> UNLOAD_LOADER_UP
- move loader up
- move_fsm_failed -> .
- retry
-
-UNLOAD_LOADER_UP:
- loader_uped -> UNLOAD_FACE_BIN
- turn toward bin
- loader_errored -> UNLOAD_FACE_BIN
- turn toward bin
-
-UNLOAD_FACE_BIN:
- bot_move_succeed -> UNLOAD_BACK_BIN
- go backward to bin
- bot_move_failed -> UNLOAD_FACE_BIN_UNBLOCK
- move backward
-
-UNLOAD_FACE_BIN_UNBLOCK:
- bot_move_succeed -> UNLOAD_FACE_BIN
- turn toward bin
- bot_move_failed -> UNLOAD_FACE_BIN
- turn toward bin
-
-UNLOAD_BACK_BIN:
- bot_move_succeed -> UNLOAD_UNLOAD
- unload
- bot_move_failed -> UNLOAD_UNLOAD
- unload
-
-UNLOAD_UNLOAD:
- state_timeout -> COLLECT
- close gate
- loader down
- choose best food to collect
-
-COLLECT:
- move_fsm_succeed: unload -> UNLOAD
- move_fsm_succeed: collect -> COLLECT
- move_fsm_succeed: slow_motion -> COLLECT_SLOW_MOTION
- post loader_element event
- move_fsm_failed: unload -> UNLOAD
- move_fsm_failed: collect -> COLLECT
- cancel this element
- loader_black -> COLLECT_BLACK
- move backward
- mark as black
-
-COLLECT_SLOW_MOTION:
- loader_downed: unload -> UNLOAD
- loader_downed: collect -> COLLECT
- collect
- loader_errored: unload -> UNLOAD
- loader_errored: collect -> COLLECT
- same as above
- state_timeout: unload -> UNLOAD
- state_timeout: collect -> COLLECT
- same as above
- loader_black -> COLLECT_BLACK
- move backward
- mark as black
-
-COLLECT_BLACK:
- bot_move_succeed: unload -> UNLOAD
- bot_move_succeed: collect -> COLLECT
- bot_move_failed: unload -> UNLOAD
- bot_move_failed: collect -> COLLECT