summaryrefslogtreecommitdiff
path: root/digital/io
diff options
context:
space:
mode:
authorNicolas Schodet2011-05-08 16:15:06 +0200
committerNicolas Schodet2011-05-08 16:15:06 +0200
commit0feb542a8cf7630a5d442387727414f5251aad83 (patch)
tree02092fbede27ef619ee63466f671513a889fb2ea /digital/io
parent4cd9c4458eaa49c269171145f4bc2a184948eae1 (diff)
parentd385a9eacb2e8580239f101f96356f7ed216dd97 (diff)
Merge branch 'master' into efrei-robotique
Conflicts: digital/mimot/src/dirty/counter_ext.avr.c digital/mimot/tools/mimot/init.py
Diffstat (limited to 'digital/io')
-rw-r--r--digital/io/src/Makefile27
-rw-r--r--digital/io/src/ai.conf12
-rw-r--r--digital/io/src/ai_hola_cb.c165
-rw-r--r--digital/io/src/ai_init_cb.c213
-rw-r--r--digital/io/src/ai_loader_cb.c636
-rw-r--r--digital/io/src/ai_move_cb.c648
-rw-r--r--digital/io/src/ai_top_cb.c434
-rw-r--r--digital/io/src/asserv.c452
-rw-r--r--digital/io/src/asserv.h304
-rw-r--r--digital/io/src/bot.h19
-rw-r--r--digital/io/src/chrono.c156
-rw-r--r--digital/io/src/chrono.h97
-rw-r--r--digital/io/src/defs.h46
-rw-r--r--digital/io/src/fsm.c137
-rw-r--r--digital/io/src/fsm.h126
-rw-r--r--digital/io/src/fsm_templates/template_cb.h10
-rw-r--r--digital/io/src/fsm_templates/template_cb_decl.h6
-rw-r--r--digital/io/src/fsm_templates/template_cb_impl.c9
-rw-r--r--digital/io/src/fsm_templates/template_fsm.c55
-rw-r--r--digital/io/src/fsm_templates/template_fsm.h52
-rw-r--r--digital/io/src/fsm_templates/template_hola_cb_skel.c11
-rw-r--r--digital/io/src/fsm_templates/template_init_cb_skel.c11
-rw-r--r--digital/io/src/fsm_templates/template_loader_cb_skel.c11
-rw-r--r--digital/io/src/fsm_templates/template_move_cb_skel.c11
-rw-r--r--digital/io/src/fsm_templates/template_top_cb_skel.c11
-rw-r--r--digital/io/src/hola.c115
-rw-r--r--digital/io/src/hola.fsm59
-rw-r--r--digital/io/src/init.c206
-rw-r--r--digital/io/src/init.fsm106
-rw-r--r--digital/io/src/loader.c613
-rw-r--r--digital/io/src/loader.fsm227
-rw-r--r--digital/io/src/main.c129
-rw-r--r--digital/io/src/main.h4
-rw-r--r--digital/io/src/main_timer.avr.c60
-rw-r--r--digital/io/src/main_timer.h78
-rw-r--r--digital/io/src/mimot.c183
-rw-r--r--digital/io/src/mimot.h95
-rw-r--r--digital/io/src/move.c605
-rw-r--r--digital/io/src/move.fsm137
-rw-r--r--digital/io/src/playground.h96
-rw-r--r--digital/io/src/playground_2010.h (renamed from digital/io/src/debug.host.h)32
-rw-r--r--digital/io/src/radar.c3
-rw-r--r--digital/io/src/simu.host.c4
-rw-r--r--digital/io/src/switch.avr.c2
-rw-r--r--digital/io/src/switch.h3
-rw-r--r--digital/io/src/top.c360
-rw-r--r--digital/io/src/top.fsm162
-rw-r--r--digital/io/src/trace.trc2
-rw-r--r--digital/io/src/twi_master.c219
-rw-r--r--digital/io/src/twi_master.h68
-rw-r--r--digital/io/src/usdist.c2
-rw-r--r--digital/io/src/usdist.h2
-rw-r--r--digital/io/tools/dump_usdist.py7
-rw-r--r--digital/io/tools/io/init.py14
-rw-r--r--digital/io/tools/io/io.py7
-rw-r--r--digital/io/tools/test_simu.py156
-rw-r--r--digital/io/tools/test_simu_control.py113
57 files changed, 2007 insertions, 5521 deletions
diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile
index 5dfea34b..5cbea0ba 100644
--- a/digital/io/src/Makefile
+++ b/digital/io/src/Makefile
@@ -3,14 +3,16 @@ BASE = ../../avr
# Name of the program to build.
PROGS = io
# Sources to compile.
-io_SOURCES = main.c servo.avr.c eeprom.avr.c pwm.c \
- switch.avr.c chrono.c main_timer.avr.c servo_pos.c \
+io_SOURCES = main.c fsm_queue.c servo.avr.c eeprom.avr.c pwm.c \
+ switch.avr.c chrono.c timer.avr.c servo_pos.c \
twi_master.c asserv.c mimot.c \
simu.host.c contact.c usdist.c radar.c \
- path.c food.c events.host.c
+ path.c food.c events.host.c \
+ fsm.host.c init.c move.c top.c hola.c loader.c fsm_AI_gen.avr.c
# Modules needed for IO.
MODULES = proto uart twi utils adc math/fixed math/geometry path/astar \
trace flash spi
+AI_MODULES = twi_master common utils fsm
# Configuration file.
CONFIGFILE = avrconfig.h
# IO board use an ATMega128.
@@ -19,11 +21,20 @@ AVR_MCU = atmega128
OPTIMIZE = -O2
HOST_LIBS = -lm
-# FSM.
-FSM = ai
-FSM_SOURCES := top init move loader hola
-# Include FSM makefile.
-include $(BASE)/make/Makefile.fsm
+vpath %.c $(AI_MODULES:%=../../ai/src/%)
+vpath %.h $(AI_MODULES:%=../../ai/src/%)
+INCLUDES += -I. $(AI_MODULES:%=-I../../ai/src/%)
+
+EXTRA_CLEAN_FILES = fsm_AI_gen.h fsm_AI_gen.avr.c
+
+include $(BASE)/make/Makefile.gen
+
+# FSM generation.
+obj/main.avr.o: fsm_AI_gen.h
+fsm_AI_gen.avr.c: fsm_AI_gen.h
+fsm_AI_gen.h: io.host
+ ./$< --gen
+ mv fsm_AI_gen.c fsm_AI_gen.avr.c
events.h events.host.c: trace.trc
PYTHONPATH=$(BASE)/../../host python $(BASE)/../../tools/trace/trace.py -t create -i $< -o
diff --git a/digital/io/src/ai.conf b/digital/io/src/ai.conf
deleted file mode 100644
index ce7f6374..00000000
--- a/digital/io/src/ai.conf
+++ /dev/null
@@ -1,12 +0,0 @@
-[user]
-template-dir = fsm_templates
-
-[templates]
-template_fsm.h = %_fsm.h
-template_fsm.c = %_fsm.c
-template_cb.h = %_cb.h
-template_top_cb_skel.c = %_top_cb_skel.c
-template_init_cb_skel.c = %_init_cb_skel.c
-template_move_cb_skel.c = %_move_cb_skel.c
-template_loader_cb_skel.c = %_loader_cb_skel.c
-template_hola_cb_skel.c = %_hola_cb_skel.c
diff --git a/digital/io/src/ai_hola_cb.c b/digital/io/src/ai_hola_cb.c
deleted file mode 100644
index 6b76b058..00000000
--- a/digital/io/src/ai_hola_cb.c
+++ /dev/null
@@ -1,165 +0,0 @@
-/* ai_hola_cb.c */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
- *
- * Copyright (C) 2010 Nicolas Schodet
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}} */
-#include "common.h"
-#include "fsm.h"
-#include "ai_cb.h"
-#include "asserv.h"
-#include "mimot.h"
-#include "bot.h"
-#include "playground.h"
-
-#include "modules/math/fixed/fixed.h"
-
-uint32_t hola_angle;
-int16_t hola_timeout;
-
-/*
- * HOLA_IDLE =start=>
- * => HOLA_WAIT_HOLA
- */
-fsm_branch_t
-ai__HOLA_IDLE__start (void)
-{
- return ai_next (HOLA_IDLE, start);
-}
-
-/*
- * HOLA_WAIT_HOLA =hola_start=>
- * => HOLA_WAIT_JACK_IN
- */
-fsm_branch_t
-ai__HOLA_WAIT_HOLA__hola_start (void)
-{
- return ai_next (HOLA_WAIT_HOLA, hola_start);
-}
-
-/*
- * HOLA_WAIT_JACK_IN =jack_inserted_into_bot=>
- * => HOLA_WAIT_JACK_OUT
- */
-fsm_branch_t
-ai__HOLA_WAIT_JACK_IN__jack_inserted_into_bot (void)
-{
- return ai_next (HOLA_WAIT_JACK_IN, jack_inserted_into_bot);
-}
-
-/*
- * HOLA_WAIT_JACK_OUT =jack_removed_from_bot=>
- * => HOLA_ROUNDS
- */
-fsm_branch_t
-ai__HOLA_WAIT_JACK_OUT__jack_removed_from_bot (void)
-{
- return ai_next (HOLA_WAIT_JACK_OUT, jack_removed_from_bot);
-}
-
-/*
- * HOLA_ROUNDS =asserv_last_cmd_ack=>
- * no_timeout => HOLA_ROUNDS
- * move clamp rounds
- * timeout => HOLA_UP
- * move up
- * rotate
- */
-fsm_branch_t
-ai__HOLA_ROUNDS__asserv_last_cmd_ack (void)
-{
- if (++hola_timeout < 150)
- {
- int32_t cos = fixed_cos_f824 (hola_angle) + 0x1000000;
- int32_t sin = fixed_sin_f824 (hola_angle) + 0x1000000;
- int32_t dx = BOT_CLAMP_OPEN_STEP
- + fixed_mul_f824 (BOT_CLAMP_STROKE_STEP / 3 / 2, cos);
- int32_t dy = BOT_ELEVATOR_ZERO_SPEED
- + fixed_mul_f824 (BOT_ELEVATOR_REST_STEP / 3 / 2, sin);
- mimot_move_motor0_absolute (dx, BOT_CLAMP_SPEED);
- mimot_move_motor1_absolute (dx, BOT_CLAMP_SPEED);
- asserv_move_motor0_absolute (dy, BOT_ELEVATOR_SPEED);
- hola_angle += 0x1000000 / (225 / 12);
- return ai_next_branch (HOLA_ROUNDS, asserv_last_cmd_ack, no_timeout);
- }
- else
- {
- asserv_set_speed (0x10, 0x1c, 0x10, 0x1c);
- asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED / 3);
- asserv_move_angularly (POSITION_A_DEG (174));
- return ai_next_branch (HOLA_ROUNDS, asserv_last_cmd_ack, timeout);
- }
-}
-
-/*
- * HOLA_UP =bot_move_succeed=>
- * => HOLA_CENTER
- * rotate to center
- */
-fsm_branch_t
-ai__HOLA_UP__bot_move_succeed (void)
-{
- asserv_set_speed (0x10, 0x20, 0x10, 0x20);
- asserv_move_angularly (POSITION_A_DEG (-90));
- asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP / 2, BOT_ELEVATOR_SPEED);
- return ai_next (HOLA_UP, bot_move_succeed);
-}
-
-/*
- * HOLA_CENTER =bot_move_succeed=>
- * => HOLA_CLAP_CLOSE
- * close clamp
- */
-fsm_branch_t
-ai__HOLA_CENTER__bot_move_succeed (void)
-{
- mimot_motor0_clamp (BOT_CLAMP_ZERO_SPEED, 0);
- mimot_motor1_clamp (BOT_CLAMP_ZERO_SPEED, 0);
- return ai_next (HOLA_CENTER, bot_move_succeed);
-}
-
-/*
- * HOLA_CLAP_CLOSE =clamp_succeed=>
- * => HOLA_CLAP_OPEN
- * open clamp
- */
-fsm_branch_t
-ai__HOLA_CLAP_CLOSE__clamp_succeed (void)
-{
- mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- return ai_next (HOLA_CLAP_CLOSE, clamp_succeed);
-}
-
-/*
- * HOLA_CLAP_OPEN =clamp_succeed=>
- * => HOLA_CLAP_CLOSE
- * close clamp
- */
-fsm_branch_t
-ai__HOLA_CLAP_OPEN__clamp_succeed (void)
-{
- mimot_motor0_clamp (BOT_CLAMP_SPEED, 0);
- mimot_motor1_clamp (BOT_CLAMP_SPEED, 0);
- return ai_next (HOLA_CLAP_OPEN, clamp_succeed);
-}
-
-
diff --git a/digital/io/src/ai_init_cb.c b/digital/io/src/ai_init_cb.c
deleted file mode 100644
index 444b867b..00000000
--- a/digital/io/src/ai_init_cb.c
+++ /dev/null
@@ -1,213 +0,0 @@
-/* init_cb.c - init FSM callbacks. */
-/* {{{
- *
- * Copyright (C) 2009 Nicolas Haller
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}}
- * init:
- * place the bot in the start position for a match.
- */
-#include "common.h"
-#include "fsm.h"
-#include "ai_cb.h"
-#include "asserv.h"
-#include "init.h"
-#include "playground.h"
-#include "main.h"
-#include "bot.h"
-#include "switch.h"
-#include "chrono.h"
-#include "playground.h"
-#include "modules/trace/trace.h"
-#include "modules/path/path.h"
-
-/*
- * INIT_IDLE =start=>
- * => INIT_WAIT_FIRST_JACK_IN
- * do nothing.
- */
-fsm_branch_t
-ai__INIT_IDLE__start (void)
-{
- return ai_next (INIT_IDLE, start);
-}
-
-/*
- * INIT_WAIT_FIRST_JACK_IN =jack_inserted_into_bot=>
- * => INIT_WAIT_FIRST_JACK_OUT
- * do nothing.
- */
-fsm_branch_t
-ai__INIT_WAIT_FIRST_JACK_IN__jack_inserted_into_bot (void)
-{
- return ai_next (INIT_WAIT_FIRST_JACK_IN, jack_inserted_into_bot);
-}
-
-/*
- * INIT_WAIT_FIRST_JACK_OUT =jack_removed_from_bot=>
- * => INIT_WAIT_SECOND_JACK_IN
- * start trace module.
- * get and store the color of the bot.
- */
-fsm_branch_t
-ai__INIT_WAIT_FIRST_JACK_OUT__jack_removed_from_bot (void)
-{
- /* Initialize trace module (erase the flash). */
- trace_init ();
- /* Get the color. */
- bot_color = switch_get_color ();
- return ai_next (INIT_WAIT_FIRST_JACK_OUT, jack_removed_from_bot);
-}
-
-/*
- * INIT_WAIT_SECOND_JACK_IN =jack_inserted_into_bot=>
- * => INIT_WAIT_FOR_HANDS_OUT
- * do nothing.
- */
-fsm_branch_t
-ai__INIT_WAIT_SECOND_JACK_IN__jack_inserted_into_bot (void)
-{
- return ai_next (INIT_WAIT_SECOND_JACK_IN, jack_inserted_into_bot);
-}
-
-/*
- * INIT_WAIT_FOR_HANDS_OUT =state_timeout=>
- * => INIT_GO_TO_THE_WALL
- * fuck the wall in front.
- */
-fsm_branch_t
-ai__INIT_WAIT_FOR_HANDS_OUT__state_timeout (void)
-{
- /* Go to the wall, no backward. */
- asserv_go_to_the_wall (0);
- return ai_next (INIT_WAIT_FOR_HANDS_OUT, state_timeout);
-}
-
-/*
- * INIT_GO_TO_THE_WALL =bot_move_succeed=>
- * => INIT_GO_AWAY_FROM_THE_WALL
- * reset the Y position of the bot.
- * reset the angular position of the bot.
- * move away from the wall (linear move).
- */
-fsm_branch_t
-ai__INIT_GO_TO_THE_WALL__bot_move_succeed (void)
-{
- asserv_set_y_position (PG_Y (PG_LENGTH - BOT_SIZE_FRONT));
- asserv_set_angle_position (PG_A_DEG (90));
- /* Move away from the border. */
- asserv_move_linearly (- INIT_DIST);
- return ai_next (INIT_GO_TO_THE_WALL, bot_move_succeed);
-}
-
-/*
- * INIT_GO_AWAY_FROM_THE_WALL =bot_move_succeed=>
- * => INIT_FACE_OTHER_WALL
- * turn to face the other wall.
- */
-fsm_branch_t
-ai__INIT_GO_AWAY_FROM_THE_WALL__bot_move_succeed (void)
-{
- /* Face the other wall. */
- asserv_goto_angle (PG_A_DEG (180));
- return ai_next (INIT_GO_AWAY_FROM_THE_WALL, bot_move_succeed);
-}
-
-/*
- * INIT_FACE_OTHER_WALL =bot_move_succeed=>
- * => INIT_WAIT_AFTER_ROTATION
- * nothing to do.
- */
-fsm_branch_t
-ai__INIT_FACE_OTHER_WALL__bot_move_succeed (void)
-{
- return ai_next (INIT_FACE_OTHER_WALL, bot_move_succeed);
-}
-
-/*
- * INIT_WAIT_AFTER_ROTATION =state_timeout=>
- * => INIT_GO_TO_THE_WALL_AGAIN
- * fuck the wall in front.
- */
-fsm_branch_t
-ai__INIT_WAIT_AFTER_ROTATION__state_timeout (void)
-{
- /* Go to the wall, no backward. */
- asserv_go_to_the_wall (0);
- return ai_next (INIT_WAIT_AFTER_ROTATION, state_timeout);
-}
-
-/*
- * INIT_GO_TO_THE_WALL_AGAIN =bot_move_succeed=>
- * => INIT_GO_AWAY_FROM_THE_WALL_AGAIN
- * reset the X position of the bot.
- * move away from the wall (linear move).
- */
-fsm_branch_t
-ai__INIT_GO_TO_THE_WALL_AGAIN__bot_move_succeed (void)
-{
- asserv_set_x_position (PG_X (BOT_SIZE_FRONT));
- /* Move away from the border. */
- asserv_move_linearly (- INIT_DIST);
- return ai_next (INIT_GO_TO_THE_WALL_AGAIN, bot_move_succeed);
-}
-
-/*
- * INIT_GO_AWAY_FROM_THE_WALL_AGAIN =bot_move_succeed=>
- * => INIT_GO_TO_START_POSITION
- * go to the start position with a go to movement.
- */
-fsm_branch_t
-ai__INIT_GO_AWAY_FROM_THE_WALL_AGAIN__bot_move_succeed (void)
-{
- /* Move away from the border. */
- asserv_goto_xya (BOT_INIT_XYA, 0);
- return ai_next (INIT_GO_AWAY_FROM_THE_WALL_AGAIN, bot_move_succeed);
-}
-
-/*
- * INIT_GO_TO_START_POSITION =bot_move_succeed=>
- * => INIT_WAIT_SECOND_JACK_OUT
- * nothing to do, the bot is at the start position.
- */
-fsm_branch_t
-ai__INIT_GO_TO_START_POSITION__bot_move_succeed (void)
-{
- return ai_next (INIT_GO_TO_START_POSITION, bot_move_succeed);
-}
-
-/*
- * INIT_WAIT_SECOND_JACK_OUT =jack_removed_from_bot=>
- * => INIT_IDLE
- * tell other FSM the match begins.
- * start the chrono.
- */
-fsm_branch_t
-ai__INIT_WAIT_SECOND_JACK_OUT__jack_removed_from_bot (void)
-{
- /* Set the flag to transmit to other FSM. */
- init_match_is_started = 1;
- /* Start the chrono. */
- chrono_init ();
- return ai_next (INIT_WAIT_SECOND_JACK_OUT, jack_removed_from_bot);
-}
-
-
diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c
deleted file mode 100644
index fdb628a9..00000000
--- a/digital/io/src/ai_loader_cb.c
+++ /dev/null
@@ -1,636 +0,0 @@
-/* ai_loader_cb.c */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
- *
- * Copyright (C) 2010 Nicolas Schodet
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}} */
-#include "common.h"
-#include "fsm.h"
-#include "ai_cb.h"
-#include "asserv.h"
-#include "mimot.h"
-#include "bot.h"
-#include "main.h"
-#include "food.h"
-#include "loader.h"
-
-/*
- * LOADER_IDLE =start=>
- * => LOADER_WAIT_JACK_IN
- */
-fsm_branch_t
-ai__LOADER_IDLE__start (void)
-{
- return ai_next (LOADER_IDLE, start);
-}
-
-/*
- * LOADER_WAIT_JACK_IN =jack_inserted_into_bot=>
- * => LOADER_WAIT_JACK_OUT
- */
-fsm_branch_t
-ai__LOADER_WAIT_JACK_IN__jack_inserted_into_bot (void)
-{
- return ai_next (LOADER_WAIT_JACK_IN, jack_inserted_into_bot);
-}
-
-/*
- * LOADER_WAIT_JACK_IN =hola_start=>
- * => LOADER_IDLE
- */
-fsm_branch_t
-ai__LOADER_WAIT_JACK_IN__hola_start (void)
-{
- return ai_next (LOADER_WAIT_JACK_IN, hola_start);
-}
-
-/*
- * LOADER_WAIT_JACK_OUT =jack_removed_from_bot=>
- * => LOADER_INIT_ELEVATOR_ZERO
- * find elevator zero
- * close clamp
- * find gate zero
- */
-fsm_branch_t
-ai__LOADER_WAIT_JACK_OUT__jack_removed_from_bot (void)
-{
- asserv_motor0_zero_position (-BOT_ELEVATOR_ZERO_SPEED);
- mimot_motor0_clamp (BOT_CLAMP_ZERO_SPEED, 0);
- mimot_motor1_clamp (BOT_CLAMP_ZERO_SPEED, 0);
- asserv_motor1_zero_position (-BOT_GATE_SPEED);
- return ai_next (LOADER_WAIT_JACK_OUT, jack_removed_from_bot);
-}
-
-/*
- * LOADER_INIT_ELEVATOR_ZERO =elevator_succeed=>
- * => LOADER_INIT_CLAMP_CLOSE
- */
-fsm_branch_t
-ai__LOADER_INIT_ELEVATOR_ZERO__elevator_succeed (void)
-{
- return ai_next (LOADER_INIT_ELEVATOR_ZERO, elevator_succeed);
-}
-
-/*
- * LOADER_INIT_CLAMP_CLOSE =clamp_succeed=>
- * => LOADER_INIT_CLAMP_ZERO
- * move elevator up
- * find clamp zero
- */
-fsm_branch_t
-ai__LOADER_INIT_CLAMP_CLOSE__clamp_succeed (void)
-{
- asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP,
- BOT_ELEVATOR_ZERO_SPEED);
- mimot_motor0_zero_position (-BOT_CLAMP_ZERO_SPEED);
- mimot_motor1_zero_position (-BOT_CLAMP_ZERO_SPEED);
- return ai_next (LOADER_INIT_CLAMP_CLOSE, clamp_succeed);
-}
-
-/*
- * LOADER_INIT_CLAMP_ZERO =clamp_succeed=>
- * => LOADER_INIT_GATE_ZERO
- */
-fsm_branch_t
-ai__LOADER_INIT_CLAMP_ZERO__clamp_succeed (void)
-{
- return ai_next (LOADER_INIT_CLAMP_ZERO, clamp_succeed);
-}
-
-/*
- * LOADER_INIT_GATE_ZERO =gate_succeed=>
- * => LOADER_INIT_ELEVATOR_UP
- */
-fsm_branch_t
-ai__LOADER_INIT_GATE_ZERO__gate_succeed (void)
-{
- return ai_next (LOADER_INIT_GATE_ZERO, gate_succeed);
-}
-
-/*
- * LOADER_INIT_ELEVATOR_UP =elevator_succeed=>
- * => LOADER_INIT_GATE_WAIT
- */
-fsm_branch_t
-ai__LOADER_INIT_ELEVATOR_UP__elevator_succeed (void)
-{
- return ai_next (LOADER_INIT_ELEVATOR_UP, elevator_succeed);
-}
-
-/*
- * LOADER_INIT_ELEVATOR_UP =elevator_failed=>
- * => LOADER_IDLE
- * initialisation failure
- */
-fsm_branch_t
-ai__LOADER_INIT_ELEVATOR_UP__elevator_failed (void)
-{
- /* Move so that the operator notice it. */
- asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP / 3,
- BOT_ELEVATOR_ZERO_SPEED);
- return ai_next (LOADER_INIT_ELEVATOR_UP, elevator_failed);
-}
-
-/*
- * LOADER_INIT_GATE_WAIT =state_timeout=>
- * => LOADER_INIT_GATE_CLOSE
- * close gate
- */
-fsm_branch_t
-ai__LOADER_INIT_GATE_WAIT__state_timeout (void)
-{
- asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED);
- return ai_next (LOADER_INIT_GATE_WAIT, state_timeout);
-}
-
-/*
- * LOADER_INIT_GATE_CLOSE =gate_succeed=>
- * => LOADER_UP
- */
-fsm_branch_t
-ai__LOADER_INIT_GATE_CLOSE__gate_succeed (void)
-{
- return ai_next (LOADER_INIT_GATE_CLOSE, gate_succeed);
-}
-
-/*
- * LOADER_UP =loader_down=>
- * => LOADER_DOWNING
- * move down
- */
-fsm_branch_t
-ai__LOADER_UP__loader_down (void)
-{
- asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED);
- return ai_next (LOADER_UP, loader_down);
-}
-
-/*
- * LOADER_DOWN =loader_up=>
- * => LOADER_UPING
- * move up
- */
-fsm_branch_t
-ai__LOADER_DOWN__loader_up (void)
-{
- asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED);
- return ai_next (LOADER_DOWN, loader_up);
-}
-
-/*
- * LOADER_DOWN =loader_element=>
- * => LOADER_LOAD_CLOSING
- * clamp
- */
-fsm_branch_t
-ai__LOADER_DOWN__loader_element (void)
-{
- mimot_motor0_clamp (BOT_CLAMP_SPEED, BOT_CLAMP_PWM);
- mimot_motor1_clamp (BOT_CLAMP_SPEED, BOT_CLAMP_PWM);
- return ai_next (LOADER_DOWN, loader_element);
-}
-
-/*
- * LOADER_UPING =elevator_succeed=>
- * => LOADER_UP
- * post loader_uped event
- */
-fsm_branch_t
-ai__LOADER_UPING__elevator_succeed (void)
-{
- main_post_event (AI_EVENT_loader_uped);
- return ai_next (LOADER_UPING, elevator_succeed);
-}
-
-/*
- * LOADER_UPING =elevator_failed=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_UPING__elevator_failed (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_UPING, elevator_failed);
-}
-
-/*
- * LOADER_UPING =loader_down=>
- * => LOADER_DOWNING
- * move down
- */
-fsm_branch_t
-ai__LOADER_UPING__loader_down (void)
-{
- asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED);
- return ai_next (LOADER_UPING, loader_down);
-}
-
-/*
- * LOADER_DOWNING =elevator_succeed=>
- * => LOADER_DOWN
- * release elevator motor
- * post loader_downed event
- */
-fsm_branch_t
-ai__LOADER_DOWNING__elevator_succeed (void)
-{
- asserv_motor0_free ();
- main_post_event (AI_EVENT_loader_downed);
- return ai_next (LOADER_DOWNING, elevator_succeed);
-}
-
-/*
- * LOADER_DOWNING =elevator_failed=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_DOWNING__elevator_failed (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_DOWNING, elevator_failed);
-}
-
-/*
- * LOADER_DOWNING =loader_element=>
- * => LOADER_ERROR
- * emergency stop
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_DOWNING__loader_element (void)
-{
- asserv_motor0_free ();
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_DOWNING, loader_element);
-}
-
-/*
- * LOADER_DOWNING =loader_up=>
- * => LOADER_UPING
- * move up
- */
-fsm_branch_t
-ai__LOADER_DOWNING__loader_up (void)
-{
- asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED);
- return ai_next (LOADER_DOWNING, loader_up);
-}
-
-/*
- * LOADER_ERROR =loader_down=>
- * => LOADER_ERROR_DOWNING
- * move down
- */
-fsm_branch_t
-ai__LOADER_ERROR__loader_down (void)
-{
- asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED);
- return ai_next (LOADER_ERROR, loader_down);
-}
-
-/*
- * LOADER_ERROR =loader_up=>
- * => LOADER_ERROR_UPING
- * move up
- */
-fsm_branch_t
-ai__LOADER_ERROR__loader_up (void)
-{
- asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP, BOT_ELEVATOR_SPEED);
- return ai_next (LOADER_ERROR, loader_up);
-}
-
-/*
- * LOADER_ERROR_DOWNING =elevator_succeed=>
- * => LOADER_ERROR_DOWNING_OPEN
- * release elevator motor
- * open clamp
- */
-fsm_branch_t
-ai__LOADER_ERROR_DOWNING__elevator_succeed (void)
-{
- asserv_motor0_free ();
- mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- return ai_next (LOADER_ERROR_DOWNING, elevator_succeed);
-}
-
-/*
- * LOADER_ERROR_DOWNING =elevator_failed=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_ERROR_DOWNING__elevator_failed (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_ERROR_DOWNING, elevator_failed);
-}
-
-/*
- * LOADER_ERROR_DOWNING =state_timeout=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_ERROR_DOWNING__state_timeout (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_ERROR_DOWNING, state_timeout);
-}
-
-/*
- * LOADER_ERROR_DOWNING_OPEN =clamp_succeed=>
- * => LOADER_DOWN
- * post loader_downed event
- */
-fsm_branch_t
-ai__LOADER_ERROR_DOWNING_OPEN__clamp_succeed (void)
-{
- main_post_event (AI_EVENT_loader_downed);
- return ai_next (LOADER_ERROR_DOWNING_OPEN, clamp_succeed);
-}
-
-/*
- * LOADER_ERROR_DOWNING_OPEN =clamp_failed=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_ERROR_DOWNING_OPEN__clamp_failed (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_ERROR_DOWNING_OPEN, clamp_failed);
-}
-
-/*
- * LOADER_ERROR_DOWNING_OPEN =state_timeout=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_ERROR_DOWNING_OPEN__state_timeout (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_ERROR_DOWNING_OPEN, state_timeout);
-}
-
-/*
- * LOADER_ERROR_UPING =elevator_succeed=>
- * => LOADER_ERROR_UPING_OPEN
- * open clamp
- */
-fsm_branch_t
-ai__LOADER_ERROR_UPING__elevator_succeed (void)
-{
- mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- return ai_next (LOADER_ERROR_UPING, elevator_succeed);
-}
-
-/*
- * LOADER_ERROR_UPING =elevator_failed=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_ERROR_UPING__elevator_failed (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_ERROR_UPING, elevator_failed);
-}
-
-/*
- * LOADER_ERROR_UPING =state_timeout=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_ERROR_UPING__state_timeout (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_ERROR_UPING, state_timeout);
-}
-
-/*
- * LOADER_ERROR_UPING_OPEN =clamp_succeed=>
- * => LOADER_UP
- * post loader_uped event
- */
-fsm_branch_t
-ai__LOADER_ERROR_UPING_OPEN__clamp_succeed (void)
-{
- main_post_event (AI_EVENT_loader_uped);
- return ai_next (LOADER_ERROR_UPING_OPEN, clamp_succeed);
-}
-
-/*
- * LOADER_ERROR_UPING_OPEN =clamp_failed=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_ERROR_UPING_OPEN__clamp_failed (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_ERROR_UPING_OPEN, clamp_failed);
-}
-
-/*
- * LOADER_ERROR_UPING_OPEN =state_timeout=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_ERROR_UPING_OPEN__state_timeout (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_ERROR_UPING_OPEN, state_timeout);
-}
-
-/*
- * LOADER_LOAD_CLOSING =clamp_succeed=>
- * full => LOADER_LOAD_UPING
- * move up
- * count one element
- * empty => LOADER_LOAD_EMPTY_OPEN
- * open clamp
- */
-fsm_branch_t
-ai__LOADER_LOAD_CLOSING__clamp_succeed (void)
-{
- /* Measure load using clamp position. */
- uint16_t tickness = BOT_CLAMP_WIDTH_STEP
- - mimot_get_motor0_position ()
- - mimot_get_motor1_position ();
- if (tickness > BOT_CLAMP_EMPTY_STEP)
- {
- asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP,
- BOT_ELEVATOR_SPEED);
- position_t robot_position;
- asserv_get_position (&robot_position);
- food_taken (robot_position);
- loader_elements++;
- return ai_next_branch (LOADER_LOAD_CLOSING, clamp_succeed, full);
- }
- else
- {
- mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- return ai_next_branch (LOADER_LOAD_CLOSING, clamp_succeed, empty);
- }
-}
-
-/*
- * LOADER_LOAD_UPING =elevator_unload_position=>
- * => LOADER_LOAD_UNLOADING
- * open clamp
- */
-fsm_branch_t
-ai__LOADER_LOAD_UPING__elevator_unload_position (void)
-{
- mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- return ai_next (LOADER_LOAD_UPING, elevator_unload_position);
-}
-
-/*
- * LOADER_LOAD_UPING =elevator_failed=>
- * => LOADER_ERROR
- * post loader_errored or loader_black event
- * remove one element
- * open clamp
- */
-fsm_branch_t
-ai__LOADER_LOAD_UPING__elevator_failed (void)
-{
- if (asserv_get_motor0_position () < BOT_ELEVATOR_BLACK_THRESHOLD_STEP)
- {
- if (loader_elements)
- loader_elements--;
- main_post_event (AI_EVENT_loader_black);
- }
- else
- main_post_event (AI_EVENT_loader_errored);
- mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- return ai_next (LOADER_LOAD_UPING, elevator_failed);
-}
-
-/*
- * LOADER_LOAD_UNLOADING =elevator_succeed=>
- * => LOADER_LOAD_UNLOADING_OPEN
- */
-fsm_branch_t
-ai__LOADER_LOAD_UNLOADING__elevator_succeed (void)
-{
- return ai_next (LOADER_LOAD_UNLOADING, elevator_succeed);
-}
-
-/*
- * LOADER_LOAD_UNLOADING =elevator_failed=>
- * => LOADER_ERROR
- * post loader_errored event
- * open clamp
- */
-fsm_branch_t
-ai__LOADER_LOAD_UNLOADING__elevator_failed (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- return ai_next (LOADER_LOAD_UNLOADING, elevator_failed);
-}
-
-/*
- * LOADER_LOAD_UNLOADING_OPEN =clamp_succeed=>
- * down => LOADER_DOWNING
- * move down
- * up => LOADER_UPING
- * move up
- */
-fsm_branch_t
-ai__LOADER_LOAD_UNLOADING_OPEN__clamp_succeed (void)
-{
- if (loader_want_up)
- {
- asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP,
- BOT_ELEVATOR_SPEED);
- return ai_next_branch (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed, up);
- }
- else
- {
- asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP,
- BOT_ELEVATOR_SPEED);
- return ai_next_branch (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed, down);
- }
-}
-
-/*
- * LOADER_LOAD_UNLOADING_OPEN =clamp_failed=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_LOAD_UNLOADING_OPEN__clamp_failed (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_LOAD_UNLOADING_OPEN, clamp_failed);
-}
-
-/*
- * LOADER_LOAD_EMPTY_OPEN =clamp_succeed=>
- * down => LOADER_DOWN
- * post loader_downed event
- * up => LOADER_UPING
- * move up
- */
-fsm_branch_t
-ai__LOADER_LOAD_EMPTY_OPEN__clamp_succeed (void)
-{
- if (loader_want_up)
- {
- asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP,
- BOT_ELEVATOR_SPEED);
- return ai_next_branch (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, up);
- }
- else
- {
- main_post_event (AI_EVENT_loader_downed);
- return ai_next_branch (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, down);
- }
-}
-
-/*
- * LOADER_LOAD_EMPTY_OPEN =clamp_failed=>
- * => LOADER_ERROR
- * post loader_errored event
- */
-fsm_branch_t
-ai__LOADER_LOAD_EMPTY_OPEN__clamp_failed (void)
-{
- main_post_event (AI_EVENT_loader_errored);
- return ai_next (LOADER_LOAD_EMPTY_OPEN, clamp_failed);
-}
-
-
diff --git a/digital/io/src/ai_move_cb.c b/digital/io/src/ai_move_cb.c
deleted file mode 100644
index 32cdd221..00000000
--- a/digital/io/src/ai_move_cb.c
+++ /dev/null
@@ -1,648 +0,0 @@
-/* move_cb.c */
-/* {{{
- *
- * Copyright (C) 2008 Nélio Laranjeiro
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}}
- * move:
- * move with avoid obstacle.
- */
-#include "common.h"
-#include "fsm.h"
-#include "ai_cb.h"
-
-#include "asserv.h"
-#include "playground.h"
-#include "move.h"
-#include "bot.h"
-#include "radar.h"
-#include "events.h"
-#include "loader.h"
-
-#include "main.h" /* main_post_event_for_top_fsm */
-#include "modules/math/fixed/fixed.h" /* fixed_* */
-#include "modules/path/path.h"
-#include "modules/utils/utils.h"
-#include "modules/trace/trace.h"
-
-#include "debug.host.h"
-
-#include <math.h>
-
-#define MOVE_LOADER_UNBLOCKING_DISTANCE 70
-
-/** Used to define a grid. If robot is outside the grid, be careful. */
-#define MOVE_GRID_X 450
-#define MOVE_GRID_X_ORIGIN 150
-#define MOVE_GRID_Y 250
-#define MOVE_GRID_Y_ORIGIN 128
-#define MOVE_GRID_MARGIN 50
-
-/** Go to current step, low level function. */
-static void
-move_go (void)
-{
- vect_t dst = move_data.step;
- /* Modify final point if requested. */
- if (move_data.final_move && move_data.shorten)
- {
- /* Compute a vector from destination to robot with lenght
- * 'shorten'. */
- position_t robot_position;
- asserv_get_position (&robot_position);
- vect_t v = robot_position.v;
- vect_sub (&v, &move_data.step);
- int16_t d = vect_norm (&v);
- if (d > move_data.shorten)
- {
- vect_scale_f824 (&v, 0x1000000 / d * move_data.shorten);
- vect_translate (&dst, &v);
- }
- }
- if (move_data.step_with_angle)
- asserv_goto_xya (dst.x, dst.y, move_data.step_angle,
- move_data.step_backward);
- else
- asserv_goto (dst.x, dst.y, move_data.step_backward);
-}
-
-/** Go or rotate toward position, returns 1 for linear move, 2 for angular
- * move. */
-static uint8_t
-move_go_or_rotate (vect_t dst, uint16_t angle, uint8_t with_angle,
- uint8_t backward)
-{
- position_t robot_position;
- /* Remember step. */
- move_data.step = dst;
- move_data.step_angle = angle;
- move_data.step_with_angle = with_angle;
- move_data.step_backward = backward;
- /* Compute angle to destination. */
- asserv_get_position (&robot_position);
- vect_t v = dst; vect_sub (&v, &robot_position.v);
- uint16_t dst_angle = atan2 (v.y, v.x) * ((1l << 16) / (2 * M_PI));
- if (backward & ASSERV_BACKWARD)
- dst_angle += 0x8000;
- if ((backward & ASSERV_REVERT_OK)
- && (dst_angle ^ robot_position.a) & 0x8000)
- dst_angle += 0x8000;
- int16_t diff = dst_angle - robot_position.a;
- /* Move or rotate. */
- if (UTILS_ABS (diff) < 0x1000)
- {
- loader_down ();
- move_go ();
- return 1;
- }
- else
- {
- loader_up ();
- asserv_goto_angle (dst_angle);
- return 2;
- }
-}
-
-/** Go to next position computed by path module, to be called by
- * move_path_init and move_path_next. Returns 1 for linear move, 2 for angular
- * move. */
-static uint8_t
-move_go_to_next (vect_t dst)
-{
- uint8_t r;
- /* If it is not the last position. */
- if (dst.x != move_data.final.v.x || dst.y != move_data.final.v.y)
- {
- /* Not final position. */
- move_data.final_move = 0;
- /* Goto without angle. */
- r = move_go_or_rotate (dst, 0, 0, move_data.backward_movement_allowed
- | (move_data.slow ? ASSERV_REVERT_OK : 0));
- }
- else
- {
- /* Final position. */
- move_data.final_move = 1;
- /* Goto with angle if requested. */
- r = move_go_or_rotate (dst, move_data.final.a, move_data.with_angle,
- move_data.backward_movement_allowed);
- }
- TRACE (TRACE_MOVE__GO_TO, dst.x, dst.y);
- /* Next time, do not use slow. */
- move_data.slow = 0;
- return r;
-}
-
-/** Update and go to first position, return non zero if a path is found, 1 for
- * linear move, 2 for angular move. */
-static uint8_t
-move_path_init (void)
-{
- uint8_t found;
- vect_t dst;
- /* Get the current position */
- position_t current_pos;
- asserv_get_position (&current_pos);
- /* Give the current position of the bot to the path module */
- path_endpoints (current_pos.v, move_data.final.v);
- /* Update the path module */
- move_data.slow = 0;
- path_update ();
- found = path_get_next (&dst);
- /* If not found, try to escape. */
- if (!found)
- {
- move_data.slow = 1;
- path_escape (8);
- path_update ();
- found = path_get_next (&dst);
- }
- /* If not on the grid, slow down. */
- int16_t mx = current_pos.v.x % MOVE_GRID_X;
- int16_t my = current_pos.v.y % MOVE_GRID_Y;
- if (mx < MOVE_GRID_X_ORIGIN - MOVE_GRID_MARGIN
- || mx > MOVE_GRID_X_ORIGIN + MOVE_GRID_MARGIN
- || my < MOVE_GRID_Y_ORIGIN - MOVE_GRID_MARGIN
- || my > MOVE_GRID_Y_ORIGIN + MOVE_GRID_MARGIN
- )
- move_data.slow = 1;
- /* If found, go. */
- if (found)
- {
- return move_go_to_next (dst);
- }
- else
- {
- /* Error, not final move. */
- move_data.final_move = 0;
- return 0;
- }
-}
-
-/** Go to next position in path. Returns 1 for linear move, 2 for angular
- * move. */
-static uint8_t
-move_path_next (void)
-{
- vect_t dst;
- path_get_next (&dst);
- return move_go_to_next (dst);
-}
-
-/*
- * MOVE_IDLE =move_start=>
- * path_found_rotate => MOVE_ROTATING
- * rotate towards next position.
- * path_found => MOVE_MOVING
- * move to next position.
- * no_path_found => MOVE_IDLE
- * post failure event.
- */
-fsm_branch_t
-ai__MOVE_IDLE__move_start (void)
-{
- uint8_t next = move_path_init ();
- if (next)
- {
- if (next == 2)
- return ai_next_branch (MOVE_IDLE, move_start, path_found_rotate);
- else
- return ai_next_branch (MOVE_IDLE, move_start, path_found);
- }
- else
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_IDLE, move_start, no_path_found);
- }
-}
-
-/*
- * MOVE_ROTATING =bot_move_succeed=>
- * => MOVE_MOVING
- * move to next position.
- */
-fsm_branch_t
-ai__MOVE_ROTATING__bot_move_succeed (void)
-{
- loader_down ();
- move_go ();
- return ai_next (MOVE_ROTATING, bot_move_succeed);
-}
-
-/*
- * MOVE_ROTATING =bot_move_failed=>
- * => MOVE_MOVING
- * move to next position.
- */
-fsm_branch_t
-ai__MOVE_ROTATING__bot_move_failed (void)
-{
- ai__MOVE_ROTATING__bot_move_succeed ();
- return ai_next (MOVE_ROTATING, bot_move_failed);
-}
-
-/*
- * MOVE_ROTATING =state_timeout=>
- * => MOVE_MOVING
- * move to next position.
- */
-fsm_branch_t
-ai__MOVE_ROTATING__state_timeout (void)
-{
- ai__MOVE_ROTATING__bot_move_succeed ();
- return ai_next (MOVE_ROTATING, state_timeout);
-}
-
-/*
- * MOVE_ROTATING =loader_errored=>
- * => MOVE_LOADER_UNBLOCKING_UPING
- * move backward
- * loader up
- */
-fsm_branch_t
-ai__MOVE_ROTATING__loader_errored (void)
-{
- asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
- loader_up ();
- return ai_next (MOVE_ROTATING, loader_errored);
-}
-
-/*
- * MOVE_MOVING =bot_move_succeed=>
- * done => MOVE_IDLE
- * post success event.
- * path_found_rotate => MOVE_ROTATING
- * rotate towards next position.
- * path_found => MOVE_MOVING
- * move to next position.
- * no_path_found => MOVE_IDLE
- * post failure event.
- */
-fsm_branch_t
-ai__MOVE_MOVING__bot_move_succeed (void)
-{
- if (move_data.final_move)
- {
- main_post_event (AI_EVENT_move_fsm_succeed);
- return ai_next_branch (MOVE_MOVING, bot_move_succeed, done);
- }
- else
- {
- uint8_t next = move_path_next ();
- if (next == 2)
- return ai_next_branch (MOVE_MOVING, bot_move_succeed, path_found_rotate);
- else
- return ai_next_branch (MOVE_MOVING, bot_move_succeed, path_found);
- }
- //return ai_next_branch (MOVE_MOVING, bot_move_succeed, no_path_found);
-}
-
-/*
- * MOVE_MOVING =bot_move_failed=>
- * => MOVE_MOVING_BACKWARD_TO_TURN_FREELY
- * reset final_move.
- * move backward to turn freely.
- */
-fsm_branch_t
-ai__MOVE_MOVING__bot_move_failed (void)
-{
- move_data.final_move = 0;
- /* Assume there is an obstacle in front of the robot. */
- position_t robot_pos;
- asserv_get_position (&robot_pos);
- vect_t obstacle_pos;
- int16_t dist = asserv_get_last_moving_direction () == 1
- ? BOT_SIZE_FRONT + MOVE_REAL_OBSTACLE_RADIUS
- : -(BOT_SIZE_BACK + MOVE_REAL_OBSTACLE_RADIUS);
- vect_from_polar_uf016 (&obstacle_pos, dist, robot_pos.a);
- vect_translate (&obstacle_pos, &robot_pos.v);
- path_obstacle (0, obstacle_pos, MOVE_OBSTACLE_RADIUS, 0,
- MOVE_OBSTACLE_VALIDITY);
- /* Move backward to turn freely. */
- asserv_move_linearly (asserv_get_last_moving_direction () == 1 ?
- - 300 : 300);
- return ai_next (MOVE_MOVING, bot_move_failed);
-}
-
-/*
- * MOVE_MOVING =state_timeout=>
- * => MOVE_MOVING_BACKWARD_TO_TURN_FREELY
- * reset final_move.
- * move backward to turn freely.
- */
-fsm_branch_t
-ai__MOVE_MOVING__state_timeout (void)
-{
- ai__MOVE_MOVING__bot_move_failed ();
- return ai_next (MOVE_MOVING, state_timeout);
-}
-
-/*
- * MOVE_MOVING =obstacle_in_front=>
- * tryagain => MOVE_WAIT_FOR_CLEAR_PATH
- * reset final_move.
- * stop the bot.
- * tryout => MOVE_IDLE
- * stop the bot.
- * post failure event.
- */
-fsm_branch_t
-ai__MOVE_MOVING__obstacle_in_front (void)
-{
- move_data.final_move = 0;
- asserv_stop_motor ();
- if (--move_data.try_again_counter == 0)
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_MOVING, obstacle_in_front, tryout);
- }
- else
- return ai_next_branch (MOVE_MOVING, obstacle_in_front, tryagain);
-}
-
-/*
- * MOVE_MOVING =loader_errored=>
- * => MOVE_LOADER_UNBLOCKING_UPING
- * move backward
- * loader up
- */
-fsm_branch_t
-ai__MOVE_MOVING__loader_errored (void)
-{
- asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
- loader_up ();
- return ai_next (MOVE_MOVING, loader_errored);
-}
-
-/*
- * MOVE_MOVING_BACKWARD_TO_TURN_FREELY =bot_move_succeed=>
- * tryout => MOVE_IDLE
- * post failure event.
- * path_found_rotate => MOVE_ROTATING
- * rotate towards next position.
- * path_found => MOVE_MOVING
- * move to next position.
- * no_path_found => MOVE_IDLE
- * post failure event.
- */
-fsm_branch_t
-ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_succeed (void)
-{
- if (--move_data.try_again_counter == 0)
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, tryout);
- }
- else
- {
- uint8_t next = move_path_init ();
- if (next)
- {
- if (next == 2)
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, path_found_rotate);
- else
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, path_found);
- }
- else
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, no_path_found);
- }
- }
-}
-
-/*
- * MOVE_MOVING_BACKWARD_TO_TURN_FREELY =bot_move_failed=>
- * tryout => MOVE_IDLE
- * post failure event.
- * path_found_rotate => MOVE_ROTATING
- * rotate towards next position.
- * path_found => MOVE_MOVING
- * move to next position.
- * no_path_found_tryagain => MOVE_WAIT_FOR_CLEAR_PATH
- * nothing to do.
- * no_path_found_tryout => MOVE_IDLE
- * post failure event.
- */
-fsm_branch_t
-ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_failed (void)
-{
- if (--move_data.try_again_counter == 0)
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, tryout);
- }
- else
- {
- uint8_t next = move_path_init ();
- if (next)
- {
- if (next == 2)
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, path_found_rotate);
- else
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, path_found);
- }
- else
- {
- if (--move_data.try_again_counter == 0)
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_path_found_tryout);
- }
- else
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_path_found_tryagain);
- }
- }
-}
-
-/*
- * MOVE_WAIT_FOR_CLEAR_PATH =state_timeout=>
- * path_found_rotate => MOVE_ROTATING
- * rotate towards next position.
- * path_found => MOVE_MOVING
- * move to next position.
- * no_path_found_tryagain => MOVE_WAIT_FOR_CLEAR_PATH
- * decrement counter.
- * no_path_found_tryout => MOVE_IDLE
- * post failure.
- */
-fsm_branch_t
-ai__MOVE_WAIT_FOR_CLEAR_PATH__state_timeout (void)
-{
- /* Try to move. */
- uint8_t next = move_path_init ();
- if (next)
- {
- if (next == 2)
- return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, path_found_rotate);
- else
- return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, path_found);
- }
- else
- {
- /* Error, no new position, should we try again? */
- if (--move_data.try_again_counter == 0)
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, no_path_found_tryout);
- }
- else
- return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, no_path_found_tryagain);
- }
-}
-
-/*
- * MOVE_LOADER_UNBLOCKING_UPING =bot_move_succeed=>
- * => MOVE_LOADER_UNBLOCKING_DOWNING
- * loader down
- * reset unblocking retry counter
- */
-fsm_branch_t
-ai__MOVE_LOADER_UNBLOCKING_UPING__bot_move_succeed (void)
-{
- loader_down ();
- move_data.loader_unblocking_retry = 2;
- return ai_next (MOVE_LOADER_UNBLOCKING_UPING, bot_move_succeed);
-}
-
-/*
- * MOVE_LOADER_UNBLOCKING_UPING =bot_move_failed=>
- * => MOVE_LOADER_UNBLOCKING_DOWNING
- * loader down
- * reset unblocking retry counter
- */
-fsm_branch_t
-ai__MOVE_LOADER_UNBLOCKING_UPING__bot_move_failed (void)
-{
- loader_down ();
- move_data.loader_unblocking_retry = 2;
- return ai_next (MOVE_LOADER_UNBLOCKING_UPING, bot_move_failed);
-}
-
-/*
- * MOVE_LOADER_UNBLOCKING_DOWNING =loader_downed=>
- * path_found_rotate => MOVE_ROTATING
- * rotate towards next position.
- * path_found => MOVE_MOVING
- * move to next position.
- * no_path_found => MOVE_IDLE
- * post failure.
- */
-fsm_branch_t
-ai__MOVE_LOADER_UNBLOCKING_DOWNING__loader_downed (void)
-{
- /* Try to move. */
- uint8_t next = move_path_init ();
- if (next)
- {
- if (next == 2)
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, path_found_rotate);
- else
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, path_found);
- }
- else
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, no_path_found);
- }
-}
-
-/*
- * MOVE_LOADER_UNBLOCKING_DOWNING =loader_errored=>
- * tryagain => MOVE_LOADER_UNBLOCKING_UPING
- * move backward
- * loader up
- * tryout_path_found_rotate => MOVE_ROTATING
- * rotate towards next position.
- * tryout_path_found => MOVE_ROTATING
- * move to next position.
- * tryout_no_path_found => MOVE_IDLE
- * post failure.
- */
-fsm_branch_t
-ai__MOVE_LOADER_UNBLOCKING_DOWNING__loader_errored (void)
-{
- if (--move_data.loader_unblocking_retry)
- {
- asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
- loader_up ();
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryagain);
- }
- else
- {
- /* Try to move. */
- uint8_t next = move_path_init ();
- if (next)
- {
- if (next == 2)
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_path_found_rotate);
- else
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_path_found);
- }
- else
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_no_path_found);
- }
- }
-}
-
-/*
- * MOVE_LOADER_UNBLOCKING_DOWNING =state_timeout=>
- * tryagain => MOVE_LOADER_UNBLOCKING_UPING
- * move backward
- * loader up
- * tryout_path_found_rotate => MOVE_ROTATING
- * rotate towards next position.
- * tryout_path_found => MOVE_ROTATING
- * move to next position.
- * tryout_no_path_found => MOVE_IDLE
- * post failure.
- */
-fsm_branch_t
-ai__MOVE_LOADER_UNBLOCKING_DOWNING__state_timeout (void)
-{
- if (--move_data.loader_unblocking_retry)
- {
- asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
- loader_up ();
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, state_timeout, tryagain);
- }
- else
- {
- /* Try to move. */
- uint8_t next = move_path_init ();
- if (next)
- {
- if (next == 2)
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, state_timeout, tryout_path_found_rotate);
- else
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, state_timeout, tryout_path_found);
- }
- else
- {
- main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, state_timeout, tryout_no_path_found);
- }
- }
-}
-
-
diff --git a/digital/io/src/ai_top_cb.c b/digital/io/src/ai_top_cb.c
deleted file mode 100644
index ed1ae038..00000000
--- a/digital/io/src/ai_top_cb.c
+++ /dev/null
@@ -1,434 +0,0 @@
-/* ai_top_cb.c */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
- *
- * Copyright (C) 2010 Nicolas Schodet
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}}
- * Main FSM calling other FSM.
- */
-#include "common.h"
-#include "fsm.h"
-#include "ai_cb.h"
-#include "top.h"
-#include "move.h"
-#include "chrono.h"
-#include "playground.h"
-#include "asserv.h"
-#include "loader.h"
-#include "food.h"
-
-/*
- * IDLE =start=>
- * => WAIT_INIT_TO_FINISH
- * nothing to do.
- */
-fsm_branch_t
-ai__IDLE__start (void)
-{
- return ai_next (IDLE, start);
-}
-
-/*
- * WAIT_INIT_TO_FINISH =init_match_is_started=>
- * => FIRST_GO_BEGIN_OF_LINE_FAST
- * the match start
- * set fast speed
- * go to first sequence begin of line
- */
-fsm_branch_t
-ai__WAIT_INIT_TO_FINISH__init_match_is_started (void)
-{
- asserv_set_speed (BOT_MOVE_FAST);
- move_start_noangle (PG_VECT (375, 1503), 0, 0);
- return ai_next (WAIT_INIT_TO_FINISH, init_match_is_started);
-}
-
-/*
- * FIRST_GO_BEGIN_OF_LINE_FAST =move_fsm_succeed=>
- * => FIRST_GO_END_OF_LINE_FAST
- * go to end of line
- * loader down
- */
-fsm_branch_t
-ai__FIRST_GO_BEGIN_OF_LINE_FAST__move_fsm_succeed (void)
-{
- move_start_noangle (PG_VECT (2625, 253), 0, 0);
- loader_down ();
- return ai_next (FIRST_GO_BEGIN_OF_LINE_FAST, move_fsm_succeed);
-}
-
-/*
- * FIRST_GO_BEGIN_OF_LINE_FAST =move_fsm_failed=>
- * => FIRST_GO_BEGIN_OF_LINE_FAST
- * set slow speed
- * retry
- */
-fsm_branch_t
-ai__FIRST_GO_BEGIN_OF_LINE_FAST__move_fsm_failed (void)
-{
- asserv_set_speed (BOT_MOVE_SLOW);
- move_start_noangle (PG_VECT (375, 1503), 0, 0);
- return ai_next (FIRST_GO_BEGIN_OF_LINE_FAST, move_fsm_failed);
-}
-
-/*
- * FIRST_GO_END_OF_LINE_FAST =in_field=>
- * => FIRST_GO_END_OF_LINE_SLOW
- * set slow speed
- */
-fsm_branch_t
-ai__FIRST_GO_END_OF_LINE_FAST__in_field (void)
-{
- asserv_set_speed (BOT_MOVE_SLOW);
- return ai_next (FIRST_GO_END_OF_LINE_FAST, in_field);
-}
-
-/*
- * FIRST_GO_END_OF_LINE_FAST =move_fsm_succeed=>
- * => UNLOAD_LOADER_UP
- * set slow speed
- * move loader up
- */
-fsm_branch_t
-ai__FIRST_GO_END_OF_LINE_FAST__move_fsm_succeed (void)
-{
- asserv_set_speed (BOT_MOVE_SLOW);
- ai__FIRST_GO_END_OF_LINE_SLOW__move_fsm_succeed ();
- return ai_next (FIRST_GO_END_OF_LINE_FAST, move_fsm_succeed);
-}
-
-/*
- * FIRST_GO_END_OF_LINE_FAST =move_fsm_failed=>
- * => FIRST_GO_END_OF_LINE_SLOW
- * set slow speed
- * retry
- */
-fsm_branch_t
-ai__FIRST_GO_END_OF_LINE_FAST__move_fsm_failed (void)
-{
- asserv_set_speed (BOT_MOVE_SLOW);
- ai__FIRST_GO_END_OF_LINE_SLOW__move_fsm_failed ();
- return ai_next (FIRST_GO_END_OF_LINE_FAST, move_fsm_failed);
-}
-
-/*
- * FIRST_GO_END_OF_LINE_SLOW =move_fsm_succeed=>
- * => UNLOAD_LOADER_UP
- * move loader up
- */
-fsm_branch_t
-ai__FIRST_GO_END_OF_LINE_SLOW__move_fsm_succeed (void)
-{
- loader_up ();
- return ai_next (FIRST_GO_END_OF_LINE_SLOW, move_fsm_succeed);
-}
-
-/*
- * FIRST_GO_END_OF_LINE_SLOW =move_fsm_failed=>
- * => FIRST_GO_END_OF_LINE_SLOW
- * retry
- */
-fsm_branch_t
-ai__FIRST_GO_END_OF_LINE_SLOW__move_fsm_failed (void)
-{
- move_start_noangle (PG_VECT (2625, 253), 0, 0);
- return ai_next (FIRST_GO_END_OF_LINE_SLOW, move_fsm_failed);
-}
-
-/*
- * UNLOAD =move_fsm_succeed=>
- * => UNLOAD_LOADER_UP
- * move loader up
- */
-fsm_branch_t
-ai__UNLOAD__move_fsm_succeed (void)
-{
- loader_up ();
- return ai_next (UNLOAD, move_fsm_succeed);
-}
-
-/*
- * UNLOAD =move_fsm_failed=>
- * => UNLOAD
- * retry
- */
-fsm_branch_t
-ai__UNLOAD__move_fsm_failed (void)
-{
- move_start_noangle (PG_VECT (2625, 253), 0, 0);
- return ai_next (UNLOAD, move_fsm_failed);
-}
-
-/*
- * UNLOAD_LOADER_UP =loader_uped=>
- * => UNLOAD_FACE_BIN
- * turn toward bin
- */
-fsm_branch_t
-ai__UNLOAD_LOADER_UP__loader_uped (void)
-{
- asserv_goto_angle (PG_A_DEG (90));
- return ai_next (UNLOAD_LOADER_UP, loader_uped);
-}
-
-/*
- * UNLOAD_LOADER_UP =loader_errored=>
- * => UNLOAD_FACE_BIN
- * turn toward bin
- */
-fsm_branch_t
-ai__UNLOAD_LOADER_UP__loader_errored (void)
-{
- asserv_goto_angle (PG_A_DEG (90));
- return ai_next (UNLOAD_LOADER_UP, loader_errored);
-}
-
-/*
- * UNLOAD_FACE_BIN =bot_move_succeed=>
- * => UNLOAD_BACK_BIN
- * go backward to bin
- */
-fsm_branch_t
-ai__UNLOAD_FACE_BIN__bot_move_succeed (void)
-{
- asserv_move_linearly (-(128 + 250 / 2 - BOT_SIZE_BACK - 50));
- return ai_next (UNLOAD_FACE_BIN, bot_move_succeed);
-}
-
-/*
- * UNLOAD_FACE_BIN =bot_move_failed=>
- * => UNLOAD_FACE_BIN_UNBLOCK
- * move backward
- */
-fsm_branch_t
-ai__UNLOAD_FACE_BIN__bot_move_failed (void)
-{
- asserv_move_linearly (-40);
- return ai_next (UNLOAD_FACE_BIN, bot_move_failed);
-}
-
-/*
- * UNLOAD_FACE_BIN_UNBLOCK =bot_move_succeed=>
- * => UNLOAD_FACE_BIN
- * turn toward bin
- */
-fsm_branch_t
-ai__UNLOAD_FACE_BIN_UNBLOCK__bot_move_succeed (void)
-{
- asserv_goto_angle (PG_A_DEG (90));
- return ai_next (UNLOAD_FACE_BIN_UNBLOCK, bot_move_succeed);
-}
-
-/*
- * UNLOAD_FACE_BIN_UNBLOCK =bot_move_failed=>
- * => UNLOAD_FACE_BIN
- * turn toward bin
- */
-fsm_branch_t
-ai__UNLOAD_FACE_BIN_UNBLOCK__bot_move_failed (void)
-{
- asserv_goto_angle (PG_A_DEG (90));
- return ai_next (UNLOAD_FACE_BIN_UNBLOCK, bot_move_failed);
-}
-
-/*
- * UNLOAD_BACK_BIN =bot_move_succeed=>
- * => UNLOAD_UNLOAD
- * unload
- */
-fsm_branch_t
-ai__UNLOAD_BACK_BIN__bot_move_succeed (void)
-{
- asserv_move_motor1_absolute (0, BOT_GATE_SPEED);
- return ai_next (UNLOAD_BACK_BIN, bot_move_succeed);
-}
-
-/*
- * UNLOAD_BACK_BIN =bot_move_failed=>
- * => UNLOAD_UNLOAD
- * unload
- */
-fsm_branch_t
-ai__UNLOAD_BACK_BIN__bot_move_failed (void)
-{
- ai__UNLOAD_BACK_BIN__bot_move_succeed ();
- return ai_next (UNLOAD_BACK_BIN, bot_move_failed);
-}
-
-/*
- * UNLOAD_UNLOAD =state_timeout=>
- * => COLLECT
- * close gate
- * loader down
- * choose best food to collect
- */
-fsm_branch_t
-ai__UNLOAD_UNLOAD__state_timeout (void)
-{
- loader_elements = 0;
- asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED);
- loader_down ();
- top_collect (1);
- return ai_next (UNLOAD_UNLOAD, state_timeout);
-}
-
-/*
- * COLLECT =move_fsm_succeed=>
- * unload => UNLOAD
- * collect => COLLECT
- * slow_motion => COLLECT_SLOW_MOTION
- * post loader_element event
- */
-fsm_branch_t
-ai__COLLECT__move_fsm_succeed (void)
-{
- int16_t slow_motion = food_slow_motion (top_food);
- if (slow_motion)
- {
- fsm_handle_event (&ai_fsm, AI_EVENT_loader_element);
- return ai_next_branch (COLLECT, move_fsm_succeed, slow_motion);
- }
- else if (top_collect (0))
- return ai_next_branch (COLLECT, move_fsm_succeed, collect);
- else
- return ai_next_branch (COLLECT, move_fsm_succeed, unload);
-}
-
-/*
- * COLLECT =move_fsm_failed=>
- * unload => UNLOAD
- * collect => COLLECT
- * cancel this element
- */
-fsm_branch_t
-ai__COLLECT__move_fsm_failed (void)
-{
- if (top_collect (0))
- {
- food_cancel (top_food);
- return ai_next_branch (COLLECT, move_fsm_failed, collect);
- }
- else
- return ai_next_branch (COLLECT, move_fsm_failed, unload);
-}
-
-/*
- * COLLECT =loader_black=>
- * => COLLECT_BLACK
- * move backward
- * mark as black
- */
-fsm_branch_t
-ai__COLLECT__loader_black (void)
-{
- asserv_move_linearly (-90);
- food_black (top_food);
- return ai_next (COLLECT, loader_black);
-}
-
-/*
- * COLLECT_SLOW_MOTION =loader_downed=>
- * unload => UNLOAD
- * collect => COLLECT
- * collect
- */
-fsm_branch_t
-ai__COLLECT_SLOW_MOTION__loader_downed (void)
-{
- if (top_collect (0))
- return ai_next_branch (COLLECT_SLOW_MOTION, loader_downed, collect);
- else
- return ai_next_branch (COLLECT_SLOW_MOTION, loader_downed, unload);
-}
-
-/*
- * COLLECT_SLOW_MOTION =loader_errored=>
- * unload => UNLOAD
- * collect => COLLECT
- * same as above
- */
-fsm_branch_t
-ai__COLLECT_SLOW_MOTION__loader_errored (void)
-{
- if (top_collect (0))
- return ai_next_branch (COLLECT_SLOW_MOTION, loader_errored, collect);
- else
- return ai_next_branch (COLLECT_SLOW_MOTION, loader_errored, unload);
-}
-
-/*
- * COLLECT_SLOW_MOTION =state_timeout=>
- * unload => UNLOAD
- * collect => COLLECT
- * same as above
- */
-fsm_branch_t
-ai__COLLECT_SLOW_MOTION__state_timeout (void)
-{
- if (top_collect (0))
- return ai_next_branch (COLLECT_SLOW_MOTION, state_timeout, collect);
- else
- return ai_next_branch (COLLECT_SLOW_MOTION, state_timeout, unload);
-}
-
-/*
- * COLLECT_SLOW_MOTION =loader_black=>
- * => COLLECT_BLACK
- * move backward
- * mark as black
- */
-fsm_branch_t
-ai__COLLECT_SLOW_MOTION__loader_black (void)
-{
- asserv_move_linearly (-90);
- food_black (top_food);
- return ai_next (COLLECT_SLOW_MOTION, loader_black);
-}
-
-/*
- * COLLECT_BLACK =bot_move_succeed=>
- * unload => UNLOAD
- * collect => COLLECT
- */
-fsm_branch_t
-ai__COLLECT_BLACK__bot_move_succeed (void)
-{
- if (top_collect (0))
- return ai_next_branch (COLLECT_BLACK, bot_move_succeed, collect);
- else
- return ai_next_branch (COLLECT_BLACK, bot_move_succeed, unload);
-}
-
-/*
- * COLLECT_BLACK =bot_move_failed=>
- * unload => UNLOAD
- * collect => COLLECT
- */
-fsm_branch_t
-ai__COLLECT_BLACK__bot_move_failed (void)
-{
- if (top_collect (0))
- return ai_next_branch (COLLECT_BLACK, bot_move_failed, collect);
- else
- return ai_next_branch (COLLECT_BLACK, bot_move_failed, unload);
-}
-
-
diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c
deleted file mode 100644
index e9adcbbc..00000000
--- a/digital/io/src/asserv.c
+++ /dev/null
@@ -1,452 +0,0 @@
-/* asserv.c */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
- *
- * Copyright (C) 2008 Dufour Jérémy
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}} */
-#include "common.h"
-#include "asserv.h"
-
-#include "twi_master.h"
-
-#include "modules/utils/byte.h" /* v*_to_v* */
-#include "modules/math/fixed/fixed.h"
-#include "bot.h"
-#include "io.h"
-
-/**
- * @defgroup AsservPrivate Asserv module private variables and functions
- * declarations and definitions
- * @{
- */
-
-/**
- * Flag bit position value for the status byte of the asserv.
- */
-enum asserv_status_flag_e
-{
- /** Bot movement finished with success. */
- asserv_status_flag_move_succeed = 0,
- /** Bot movement finished with failure: the bot is blocked. */
- asserv_status_flag_move_failed = 1,
- /** Bot is moving forward (linear speed greater than 0). */
- asserv_status_flag_move_forward = 2,
- /** Bot is moving backward (linear speed smaller than 0). */
- asserv_status_flag_move_backward = 3,
- /** Motor0 movement finished with success. */
- asserv_status_flag_motor0_succeed = 4,
- /** Motor0 movement finished with failure. */
- asserv_status_flag_motor0_failed = 5,
- /** Motor1 movement finished with success. */
- asserv_status_flag_motor1_succeed = 6,
- /** Motor1 movement finished with failure. */
- asserv_status_flag_motor1_failed = 7,
-};
-typedef enum asserv_status_flag_e asserv_status_flag_e;
-
-/** Scaling factor. */
-static uint32_t asserv_scale;
-
-/** Scaling factor inverse. */
-static uint32_t asserv_scale_inv;
-
-/** Last moving direction. */
-static uint8_t asserv_last_moving_direction;
-
-/**
- * Structure for storing a position for the bot using asserv units.
- */
-typedef struct asserv_position_t
-{
- /** X position. */
- uint32_t x;
- /** Y position. */
- uint32_t y;
- /** Angle. */
- uint16_t a;
-} asserv_position_t;
-
-/**
- * Status structure maintains by the update command.
- */
-typedef struct asserv_struct_s
-{
- /** Status flags. */
- uint8_t status;
- /** Asserv board input port. */
- uint8_t input_port;
- /** Bot position. */
- asserv_position_t position;
- /** Motor0 position. */
- uint16_t motor0_position;
- /** Motor1 position. */
- uint16_t motor1_position;
-} asserv_struct_s;
-
-/**
- * Status variable.
- */
-asserv_struct_s asserv_status;
-
-void
-asserv_init (void)
-{
- asserv_set_scale (BOT_SCALE * (1L << 24));
-}
-
-void
-asserv_status_cb (uint8_t *status)
-{
- /* Parse received data and store them. */
- asserv_status.status = status[0];
- asserv_status.input_port = status[1];
- asserv_status.position.x = v8_to_v32 (0, status[3], status[4], status[5]);
- asserv_status.position.y = v8_to_v32 (0, status[6], status[7], status[8]);
- asserv_status.position.a = v8_to_v16 (status[9], status[10]);
- asserv_status.motor0_position = v8_to_v16 (status[11], status[12]);
- asserv_status.motor1_position = v8_to_v16 (status[13], status[14]);
- /* Update moving direction. */
- if (asserv_get_moving_direction () != 0)
- asserv_last_moving_direction = asserv_get_moving_direction ();
-}
-
-asserv_status_e
-asserv_move_cmd_status (void)
-{
- /* Check Motor Finished flag */
- if (asserv_status.status & _BV (asserv_status_flag_move_succeed))
- return success;
- /* Check Motor Blocked flag */
- else if (asserv_status.status & _BV (asserv_status_flag_move_failed))
- return failure;
- /* Otherwise, not finished nor failure */
- return none;
-}
-
-asserv_status_e
-asserv_motor0_cmd_status (void)
-{
- /* Check Motor0 Finished flag */
- if (asserv_status.status & _BV (asserv_status_flag_motor0_succeed))
- return success;
- /* Check Motor0 Blocked flag */
- else if (asserv_status.status & _BV (asserv_status_flag_motor0_failed))
- return failure;
- /* Otherwise, not finished nor failure */
- return none;
-}
-
-asserv_status_e
-asserv_motor1_cmd_status (void)
-{
- /* Check Motor1 Finished flag */
- if (asserv_status.status & _BV (asserv_status_flag_motor1_succeed))
- return success;
- /* Check Motor1 Blocked flag */
- else if (asserv_status.status & _BV (asserv_status_flag_motor1_failed))
- return failure;
- /* Otherwise, not finished nor failure */
- return none;
-}
-
-void
-asserv_get_position (position_t *current_position)
-{
- assert (current_position);
- /* Copy last received status buffer information to current position */
- current_position->v.x = fixed_mul_f824 (asserv_status.position.x,
- asserv_scale);
- current_position->v.y = fixed_mul_f824 (asserv_status.position.y,
- asserv_scale);
- current_position->a = asserv_status.position.a;
-}
-
-uint16_t
-asserv_get_motor0_position (void)
-{
- /* Return the position of the motor0 of the current status buffer */
- return asserv_status.motor0_position;
-}
-
-uint16_t
-asserv_get_motor1_position (void)
-{
- /* Return the position of the motor1 of the current status buffer */
- return asserv_status.motor1_position;
-}
-
-uint8_t
-asserv_get_moving_direction (void)
-{
- /* Foward move? */
- if (asserv_status.status & _BV (asserv_status_flag_move_forward))
- return 1;
- /* Backward move? */
- if (asserv_status.status & _BV (asserv_status_flag_move_backward))
- return 2;
- /* Not moving */
- return 0;
-}
-
-uint8_t
-asserv_get_last_moving_direction (void)
-{
- return asserv_last_moving_direction;
-}
-
-void
-asserv_reset (void)
-{
- uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
- buffer[0] = 'z';
- twi_master_send_buffer (1);
-}
-
-void
-asserv_free_motor (void)
-{
- uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
- buffer[0] = 'w';
- twi_master_send_buffer (1);
-}
-
-void
-asserv_stop_motor (void)
-{
- uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
- buffer[0] = 's';
- twi_master_send_buffer (1);
-}
-
-void
-asserv_move_linearly (int32_t distance)
-{
- distance = fixed_mul_f824 (distance, asserv_scale_inv);
- uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
- buffer[0] = 'l';
- buffer[1] = v32_to_v8 (distance, 2);
- buffer[2] = v32_to_v8 (distance, 1);
- buffer[3] = v32_to_v8 (distance, 0);
- twi_master_send_buffer (4);
-}
-
-void
-asserv_move_angularly (int16_t angle)
-{
- uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
- buffer[0] = 'a';
- buffer[1] = v16_to_v8 (angle, 1);
- buffer[2] = v16_to_v8 (angle, 0);
- twi_master_send_buffer (3);
-}
-
-void
-asserv_goto_angle (int16_t angle)
-{
- uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
- buffer[0] = 'y';
- buffer[1] = v16_to_v8 (angle, 1);
- buffer[2] = v16_to_v8 (angle, 0);
- twi_master_send_buffer (3);
-}
-
-void
-asserv_goto_xya (uint32_t x, uint32_t y, int16_t a, uint8_t backward)
-{
- x = fixed_mul_f824 (x, asserv_scale_inv);
- y = fixed_mul_f824 (y, asserv_scale_inv);
- uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
- buffer[0] = 'X';
- buffer[1] = v32_to_v8 (x, 2);
- buffer[2] = v32_to_v8 (x, 1);
- buffer[3] = v32_to_v8 (x, 0);
- buffer[4] = v32_to_v8 (y, 2);
- buffer[5] = v32_to_v8 (y, 1);
- buffer[6] = v32_to_v8 (y, 0);
- buffer[7] = v16_to_v8 (a, 1);
- buffer[8] = v16_to_v8 (a, 0);
- buffer[9] = backward;
- twi_master_send_buffer (10);
-}
-
-void
-asserv_go_to_the_wall (uint8_t backward)
-{
- uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
- buffer[0] = 'f';
- buffer[1] = backward;
- twi_master_send_buffer (2);
-}
-
-void
-asserv_move_motor0_absolute (uint16_t position, uint8_t speed)
-{
- uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
- buffer[0] = 'b';
- buffer[1] = v16_to_v8 (position, 1);
- buffer[2] = v16_to_v8 (position, 0);
- buffer[3] = speed;
- twi_master_send_buffer (4);
-}
-
-void
-asserv_move_motor1_absolute (uint16_t position, uint8_t speed)
-{
- uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
- buffer[0] = 'c';
- buffer[1] = v16_to_v8 (position, 1);
- buffer[2] = v16_to_v8 (position, 0);
- buffer[3] = speed;
- twi_master_send_buffer (4);
-}
-
-void
-asserv_set_x_position (int32_t x)
-{
- x = fixed_mul_f824 (x, asserv_scale_inv);
- uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
- buffer[0] = 'p';
- buffer[1] = 'X';
- buffer[2] = v32_to_v8 (x, 2);
- buffer[3] = v32_to_v8 (x, 1);
- buffer[4] = v32_to_v8 (x, 0);
- twi_master_send_buffer (5);
-}
-
-void
-asserv_set_y_position (int32_t y)
-{
- y = fixed_mul_f824 (y, asserv_scale_inv);
- uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
- buffer[0] = 'p';
- buffer[1] = 'Y';
- buffer[2] = v32_to_v8 (y, 2);
- buffer[3] = v32_to_v8 (y, 1);
- buffer[4] = v32_to_v8 (y, 0);
- twi_master_send_buffer (5);
-}
-
-void
-asserv_set_angle_position (int16_t angle)
-{
- uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
- buffer[0] = 'p';
- buffer[1] = 'A';
- buffer[2] = v32_to_v8 (angle, 1);
- buffer[3] = v32_to_v8 (angle, 0);
- twi_master_send_buffer (4);
-}
-
-void
-asserv_set_speed (uint8_t linear_high, uint8_t angular_high,
- uint8_t linear_low, uint8_t angular_low)
-{
- uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
- buffer[0] = 'p';
- buffer[1] = 's';
- buffer[2] = linear_high;
- buffer[3] = angular_high;
- buffer[4] = linear_low;
- buffer[5] = angular_low;
- twi_master_send_buffer (6);
-}
-
-void
-asserv_set_position (int32_t x, int32_t y, int16_t angle)
-{
- x = fixed_mul_f824 (x, asserv_scale_inv);
- y = fixed_mul_f824 (y, asserv_scale_inv);
- uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
- buffer[0] = 'p';
- buffer[1] = 'X';
- buffer[2] = v32_to_v8 (x, 2);
- buffer[3] = v32_to_v8 (x, 1);
- buffer[4] = v32_to_v8 (x, 0);
- buffer[5] = 'Y';
- buffer[6] = v32_to_v8 (y, 2);
- buffer[7] = v32_to_v8 (y, 1);
- buffer[8] = v32_to_v8 (y, 0);
- buffer[9] = 'A';
- buffer[10] = v32_to_v8 (angle, 1);
- buffer[11] = v32_to_v8 (angle, 0);
- twi_master_send_buffer (12);
-}
-
-void
-asserv_goto (uint32_t x, uint32_t y, uint8_t backward)
-{
- x = fixed_mul_f824 (x, asserv_scale_inv);
- y = fixed_mul_f824 (y, asserv_scale_inv);
- uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
- buffer[0] = 'x';
- buffer[1] = v32_to_v8 (x, 2);
- buffer[2] = v32_to_v8 (x, 1);
- buffer[3] = v32_to_v8 (x, 0);
- buffer[4] = v32_to_v8 (y, 2);
- buffer[5] = v32_to_v8 (y, 1);
- buffer[6] = v32_to_v8 (y, 0);
- buffer[7] = backward;
- twi_master_send_buffer (8);
-}
-
-void
-asserv_set_scale (uint32_t scale)
-{
- asserv_scale = scale;
- asserv_scale_inv = fixed_div_f824 (1L << 24, scale);
-}
-
-void
-asserv_motor0_zero_position (int8_t speed)
-{
- uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
- buffer[0] = 'B';
- buffer[1] = speed;
- twi_master_send_buffer (2);
-}
-
-void
-asserv_motor1_zero_position (int8_t speed)
-{
- uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
- buffer[0] = 'C';
- buffer[1] = speed;
- twi_master_send_buffer (2);
-}
-
-void
-asserv_motor0_free (void)
-{
- uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
- buffer[0] = 'r';
- buffer[1] = 0;
- twi_master_send_buffer (2);
-}
-
-void
-asserv_motor1_free (void)
-{
- uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
- buffer[0] = 'r';
- buffer[1] = 1;
- twi_master_send_buffer (2);
-}
-
diff --git a/digital/io/src/asserv.h b/digital/io/src/asserv.h
deleted file mode 100644
index a92d95f7..00000000
--- a/digital/io/src/asserv.h
+++ /dev/null
@@ -1,304 +0,0 @@
-#ifndef asserv_h
-#define asserv_h
-/* asserv.h */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
- *
- * Copyright (C) 2008 Dufour Jérémy
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}} */
-#include "defs.h"
-
-/**
- * @file Control the asserv board from io using the TWI protocol.
- * This files contains the 'public' functions to send commands to the asserv
- * board using a protocol over TWI communication.
- * @see trunk/digital/io/doc/proto_asserv.txt
- */
-
-/** Slave number in twi_master list. */
-#define ASSERV_SLAVE 0
-
-/** Asserv TWI address. */
-#define ASSERV_TWI_ADDRESS 4
-
-/** Length of the status buffer (not including CRC). */
-#define ASSERV_STATUS_LENGTH 15
-
-/** Use backward movements. */
-#define ASSERV_BACKWARD 1
-/** Authorise reverse the requested movement direction, may be or'ed with the
- * previous define. */
-#define ASSERV_REVERT_OK 2
-
-/** Initialize the asserv control module. */
-void
-asserv_init (void);
-
-/** Called when a new status buffer is received, update the asserv
- * information. */
-void
-asserv_status_cb (uint8_t *status);
-
-/**
- * Status of a move or motor class command.
- * It is return by status functions.
- */
-typedef enum asserv_status_e
-{
- /** No status is available. The command is not finished yet. */
- none,
- /** The command has succeed. */
- success,
- /** The command has failed. The bot or the motor0 is blocked */
- failure
-} asserv_status_e;
-
-/**
- * Is last move class command has successfully ended?
- * This function is used to know the status of the last move command. It looks
- * at the status register.
- * @return the status of the last move class command.
- */
-asserv_status_e
-asserv_move_cmd_status (void);
-
-/**
- * Is last motor0 class command has successfully ended?
- * This function is used to know the status of the last motor0 command. It
- * looks at the status register.
- * @return the status of the last move class command.
- */
-asserv_status_e
-asserv_motor0_cmd_status (void);
-
-/**
- * Is last motor1 class command has successfully ended?
- * This function is used to know the status of the last motor1 command. It
- * looks at the status register.
- * @return the status of the last move class command.
- */
-asserv_status_e
-asserv_motor1_cmd_status (void);
-
-/**
- * Get the current position of the bot.
- * @param current_position the current position to update.
- */
-void
-asserv_get_position (position_t *current_position);
-
-/**
- * Get the motor0 position.
- * @return the position of the motor0 (in steps).
- */
-uint16_t
-asserv_get_motor0_position (void);
-
-/**
- * Get the motor1 position.
- * @return the position of the motor1 (in steps).
- */
-uint16_t
-asserv_get_motor1_position (void);
-
-/**
- * Are we moving forward/backward?
- * @return
- * - 0 we are not moving;
- * - 1 we are moving forward;
- * - 2 we are moving backward.
- */
-uint8_t
-asserv_get_moving_direction (void);
-
-/**
- * Get the last moving direction of the bot.
- * @return 1 is forward, 2 is backward.
- */
-uint8_t
-asserv_get_last_moving_direction (void);
-
-/**
- * Reset the asserv board.
- * Other class command.
- */
-void
-asserv_reset (void);
-
-/**
- * Free the motors (stop controlling them).
- * Other class command.
- */
-void
-asserv_free_motor (void);
-
-/**
- * Stop the motor (and the bot).
- * Other class command.
- */
-void
-asserv_stop_motor (void);
-
-/**
- * Move linearly.
- * Move class command.
- * @param distance the distance to move (mm).
- */
-void
-asserv_move_linearly (int32_t distance);
-
-/**
- * Move angularly (turn).
- * Move class command.
- * @param angle the angle to turn.
- */
-void
-asserv_move_angularly (int16_t angle);
-
-/**
- * Make the bot turn of an absolute angle.
- * The angle is absolute and not a difference with the current one.
- * @param a the absolute angle in degrees
- */
-void
-asserv_goto_angle (int16_t angle);
-
-/**
- * Go to an absolute position and then an absolute angle.
- * @param x the absolute position on the X axis.
- * @param y the absolute position on the Y axis.
- * @param a the absolute angle.
- * @param backward 0 no backward, ASSERV_BACKWARD backward compulsary,
- * ASSERV_REVERT_OK backward allowed.
- */
-void
-asserv_goto_xya (uint32_t x, uint32_t y, int16_t a, uint8_t backward);
-
-/**
- * Go to the wall (moving backward).
- * Move class command.
- */
-void
-asserv_go_to_the_wall (uint8_t backward);
-
-/**
- * Move the motor0.
- * Motor0 class command.
- * This function take the number of steps you want to move to. This is an
- * absolute position.
- * @param position desired goal position (in step).
- * @param speed speed of the movement.
- */
-void
-asserv_move_motor0_absolute (uint16_t position, uint8_t speed);
-
-/**
- * Move the motor1.
- * Motor1 class command.
- * This function take the number of steps you want to move to. This is an
- * absolute position.
- * @param position desired goal position (in step).
- * @param speed speed of the movement.
- */
-void
-asserv_move_motor1_absolute (uint16_t position, uint8_t speed);
-
-/**
- * Set current X position.
- * Other class command.
- * @param x X position.
- */
-void
-asserv_set_x_position (int32_t x);
-
-/**
- * Set current Y position.
- * Other class command.
- * @param y Y position.
- */
-void
-asserv_set_y_position (int32_t y);
-
-/**
- * Set current angular position.
- * Other class command.
- * @param angle angular position.
- */
-void
-asserv_set_angle_position (int16_t angle);
-
-/**
- * Set speeds of movements.
- * Other class command.
- * @param linear_high linear high speed
- * @param angular_high angular high speed
- * @param linear_low linear low speed
- * @param angular_low angular low speed
- */
-void
-asserv_set_speed (uint8_t linear_high, uint8_t angular_high,
- uint8_t linear_low, uint8_t angular_low);
-
-/**
- * Set the complete position of the bot.
- * This is an helpful function preventing you from calling multiples other
- * ones.
- * It calls other class commands.
- * @param x X position
- * @param y Y position
- * @param angle angular position
- */
-void
-asserv_set_position (int32_t x, int32_t y, int16_t angle);
-
-/**
- * Go to an absolute position in (X, Y).
- * @param x the x position on the table.
- * @param y the y position on the table.
- * @param backward 0 no backward, ASSERV_BACKWARD backward compulsary,
- * ASSERV_REVERT_OK backward allowed.
- */
-void
-asserv_goto (uint32_t x, uint32_t y, uint8_t backward);
-
-/** Set scale.
- * @param scale number of millimeter per step (f8.24).
- */
-void
-asserv_set_scale (uint32_t scale);
-
-/** Reset the motor0 to the zero position. */
-void
-asserv_motor0_zero_position (int8_t speed);
-
-/** Reset the motor1 to the zero position. */
-void
-asserv_motor1_zero_position (int8_t speed);
-
-/** Set PWM to zero for motor0. */
-void
-asserv_motor0_free (void);
-
-/** Set PWM to zero for motor1. */
-void
-asserv_motor1_free (void);
-
-#endif /* asserv_h */
diff --git a/digital/io/src/bot.h b/digital/io/src/bot.h
index 06f6559b..b4112e92 100644
--- a/digital/io/src/bot.h
+++ b/digital/io/src/bot.h
@@ -30,11 +30,6 @@
*/
/**
- * Duration of a match in milliseconds, with margin.
- */
-#define MATCH_DURATION_MS (90000 - 2500)
-
-/**
* The scaling factor, millimeter per step.
*/
#ifdef HOST
@@ -128,18 +123,4 @@
/** Gate stroke in steps. */
#define BOT_GATE_STROKE_STEP -0x203e
-/**
- * Definition of the colors.
- */
-enum team_color_e
-{
- RED_TEAM = 0,
- BLUE_TEAM = 1
-};
-
-/**
- * Our color.
- */
-extern enum team_color_e bot_color;
-
#endif /* bot_h */
diff --git a/digital/io/src/chrono.c b/digital/io/src/chrono.c
deleted file mode 100644
index b73b627c..00000000
--- a/digital/io/src/chrono.c
+++ /dev/null
@@ -1,156 +0,0 @@
-/* chrono.c */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
- *
- * Copyright (C) 2008 Dufour Jérémy
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}} */
-
-#include "common.h"
-
-#include "bot.h"
-#include "main_timer.h"
-#include "asserv.h"
-#include "twi_master.h"
-
-#include "modules/utils/utils.h"
-#include "modules/host/mex.h"
-
-#include "chrono.h"
-
-/**
- * Implementation notes.
- * This module compute the number of tic of the main loop it should count
- * before the match is over (chrono_init). Every tic of the main loop, it
- * decrements the counter (chrono_update). When the counter is zero, the
- * match is over (chrono_is_match_over, chrono_end_match).
- */
-
-/**
- * Number of overflows of the timer/counter 0 to wait before the match is
- * over.
- * Basically, it is match_duration / timer_counter_0_overflow_duration.
- * Minus one is here for safety reason (because rounding is done at plus one).
- */
-#define CHRONO_MATCH_OVERFLOW_COUNT (MATCH_DURATION_MS / MT_TC0_PERIOD - 1)
-
-/**
- * Duration of a loop to emulate from the original behaviour, in ms.
- */
-#define CHRONO_LOOP_DURATION_MS 4
-
-/**
- * Time to wait before resetting slaves board, in ms.
- */
-#define CHRONO_WAIT_BEFORE_RESET_MS 1000
-
-/**
- * Number of time to overflow before the end of the match.
- */
-static uint32_t chrono_ov_count_;
-
-/**
- * Status of the chrono module.
- * Set to 0 if the module is disabled, otherwise set to a non 0 value.
- */
-static uint8_t chrono_enabled_ = 0;
-
-
-void
-chrono_init (void)
-{
- /* Enable chrono. */
- chrono_enable ();
- /* Set the overflow counter to the maximum of overflow before the end of
- * the match. */
- chrono_ov_count_ = CHRONO_MATCH_OVERFLOW_COUNT;
-}
-
-void
-chrono_update (void)
-{
- /* Decrement overflow counter if it is possible. */
- if (chrono_enabled_ && chrono_ov_count_)
- chrono_ov_count_--;
-}
-
-uint8_t
-chrono_is_match_over (void)
-{
- if (!chrono_enabled_ || chrono_ov_count_)
- return 0;
- else
- return 1;
-}
-
-void
-chrono_enable (void)
-{
- chrono_enabled_ = 1;
-}
-
-void
-chrono_disable (void)
-{
- chrono_enabled_ = 0;
-}
-
-uint8_t
-chrono_enabled (void)
-{
- return chrono_enabled_;
-}
-
-uint32_t
-chrono_remaining_time (void)
-{
- return chrono_ov_count_ * MT_TC0_PERIOD;
-}
-
-void
-chrono_end_match (uint8_t block)
-{
- /* Make sure previous command has been acknowledged. If not, retransmit
- * until acknowledged */
- while (!twi_master_sync ())
- utils_delay_ms (CHRONO_LOOP_DURATION_MS);
-
- /* Make the bot stop moving */
- asserv_stop_motor ();
-
- /* Wait until complete */
- while (!twi_master_sync ())
- utils_delay_ms (CHRONO_LOOP_DURATION_MS);
-
- /* Wait CHRONO_WAIT_BEFORE_RESET ms before reseting */
- utils_delay_ms (CHRONO_WAIT_BEFORE_RESET_MS);
- /* Reset the asserv board */
- asserv_reset ();
- /* Block indefinitely */
- if (block)
- while (42)
-#ifdef HOST
- {
- mex_node_wait ();
- }
-#else
- ;
-#endif
-}
diff --git a/digital/io/src/chrono.h b/digital/io/src/chrono.h
deleted file mode 100644
index e2b09ac8..00000000
--- a/digital/io/src/chrono.h
+++ /dev/null
@@ -1,97 +0,0 @@
-#ifndef chrono_h
-#define chrono_h
-/* chrono.h */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
- *
- * Copyright (C) 2008 Dufour Jérémy
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}} */
-
-/**
- * @file Module to manage the chrono responsible to stop the bot after 90s.
- *
- * It is based on the main timer (time/counter 0) to know when to stop the
- * bot.
- *
- * The main loop should never last more than the 4.44ms defined, otherwise,
- * this module will not be precise at all!
- */
-
-/**
- * Initialize the chrono module.
- * It setups it for a duration of MATCH_DURATION_MS.
- */
-void
-chrono_init (void);
-
-/**
- * Update chrono module.
- * You must call this function every overflow of the main timer.
- */
-void
-chrono_update (void);
-
-/**
- * Enable chrono module.
- * You should call this function when a match start.
- */
-void
-chrono_enable (void);
-
-/**
- * Disable chrono module.
- */
-void
-chrono_disable (void);
-
-/**
- * Is chrono module enabled?
- * @return 0 if not enabled, other values otherwise.
- */
-uint8_t
-chrono_enabled (void);
-
-/**
- * Match over?
- * @return
- * - 0 if the match is not finished yet.
- * - 1 if the match is over.
- */
-uint8_t
-chrono_is_match_over (void);
-
-/**
- * How much time remains before the end of the match.
- * @return remaining time in ms.
- */
-uint32_t
-chrono_remaining_time (void);
-
-/**
- * End the match.
- * This function is responsible of resetting the asserv board to stop the bot
- * from moving and put the io board in a state where it will not do something.
- * @param block blocking function until hardware reset?
- */
-void
-chrono_end_match (uint8_t block);
-
-#endif /* chrono_h */
diff --git a/digital/io/src/defs.h b/digital/io/src/defs.h
deleted file mode 100644
index d80ff96d..00000000
--- a/digital/io/src/defs.h
+++ /dev/null
@@ -1,46 +0,0 @@
-#ifndef defs_h
-#define defs_h
-/* defs.h */
-/* 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 "modules/math/geometry/geometry.h"
-#include "modules/math/geometry/vect.h"
-
-/** General purpose defines. */
-
-/** Structure defining a position with an angle. */
-struct position_t
-{
- /** Cartesian position in millimeters, (0, 0) is at bottom left. */
- vect_t v;
- /** Angle, counter-clockwise, [0, 1), f0.16.
- * For example, 0x8000 means 0.5, which means 180 degrees. */
- uint16_t a;
-};
-typedef struct position_t position_t;
-
-/** Convert degrees to an angle usable in position_t. */
-#define POSITION_A_DEG(a) G_ANGLE_UF016_DEG (a)
-
-#endif /* defs_h */
diff --git a/digital/io/src/fsm.c b/digital/io/src/fsm.c
deleted file mode 100644
index 45eeb5ba..00000000
--- a/digital/io/src/fsm.c
+++ /dev/null
@@ -1,137 +0,0 @@
-/* fsm.c - Finite State Machine code. */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
- *
- * Copyright (C) 2008 Nicolas Schodet
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}} */
-#include "common.h"
-#include "fsm.h"
-
-#include "modules/trace/trace.h"
-#include "modules/proto/proto.h"
-#include "events.h"
-
-#ifdef HOST
-# include <stdio.h>
-#endif
-
-void
-fsm_init (fsm_t *fsm)
-{
- uint8_t i;
- assert (fsm);
- for (i = 0; i < FSM_ACTIVE_STATES_MAX; i++)
- fsm->states_active[i] = fsm->states_init[i];
- for (i = 0; i < fsm->active_states_nb; i++)
- fsm->states_timeout[i] =
- fsm->state_timeout_table[fsm->states_init[i]];
-}
-
-/** Handle an event on the given FSM on a single active state, return 1 if the
- * event was handled. */
-static uint8_t
-fsm_handle_event_single (fsm_t *fsm, uint8_t active, uint8_t event)
-{
- assert (fsm);
- assert (active < fsm->active_states_nb);
- assert (event < fsm->events_nb);
- /* Lookup transition. */
- uint8_t old_state = fsm->states_active[active];
- fsm_transition_t tr = FSM_TRANSITION (fsm, old_state, event);
- /* Ignore unhandled events. */
- if (tr)
- {
- /* Execute transition. */
- uint8_t new_state;
- fsm_branch_t br = tr ();
- /* Change state. */
-#ifdef HOST
- assert (((br >> 16) & 0xff) == fsm->states_active[active]);
- assert (((br >> 8) & 0xff) == event);
- fprintf (stderr, "%s %s =%s=> %s\n", fsm->name,
- fsm->states_names[fsm->states_active[active]],
- fsm->events_names[event], fsm->states_names[br & 0xff]);
- new_state = br & 0xff;
-#else
- new_state = br;
- proto_send3b ('f', old_state, event, new_state);
-#endif
- TRACE (TRACE_FSM__HANDLE_EVENT, (u8) old_state, (u8) event,
- (u8) new_state);
- fsm->states_active[active] = new_state;
- fsm->states_timeout[active] = fsm->state_timeout_table[new_state];
- return 1;
- }
- return 0;
-}
-
-uint8_t
-fsm_handle_event (fsm_t *fsm, uint8_t event)
-{
- uint8_t i, handled = 0;
- assert (fsm);
- assert (event < fsm->events_nb);
- for (i = 0; i < fsm->active_states_nb; i++)
- {
- /* Handle the event for this active state. */
- handled += fsm_handle_event_single (fsm, i, event);
- }
- return handled;
-}
-
-uint8_t
-fsm_handle_timeout (fsm_t *fsm)
-{
- uint8_t i, handled = 0;
- assert (fsm);
- for (i = 0; i < fsm->active_states_nb; i++)
- {
- /* If there is a timeout for this state. */
- if (fsm->states_timeout[i] != 0xffff)
- {
- if (fsm->states_timeout[i])
- fsm->states_timeout[i]--;
- else
- {
- /* Timeout expired, generate corresponding event. */
- handled += fsm_handle_event_single (fsm, i,
- fsm->state_timeout_event);
- }
- }
- }
- return handled;
-}
-
-uint8_t
-fsm_can_handle_event (fsm_t *fsm, uint8_t event)
-{
- uint8_t i;
- assert (fsm);
- assert (event < fsm->events_nb);
- for (i = 0; i < fsm->active_states_nb; i++)
- {
- uint8_t state = fsm->states_active[i];
- if (FSM_TRANSITION (fsm, state, event))
- return 1;
- }
- return 0;
-}
-
diff --git a/digital/io/src/fsm.h b/digital/io/src/fsm.h
deleted file mode 100644
index 2d0e1933..00000000
--- a/digital/io/src/fsm.h
+++ /dev/null
@@ -1,126 +0,0 @@
-#ifndef fsm_h
-#define fsm_h
-/* fsm.h - Finite State Machine code. */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
- *
- * Copyright (C) 2008 Nicolas Schodet
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}} */
-
-#ifndef HOST
-
-# include <avr/pgmspace.h>
-
-#else
-
-# define PROGMEM
-# define pgm_read_word(addr) (*(addr))
-
-#endif
-
-/**
- * The io board includes an FSM with several concurrent active states.
- *
- * The main loop tests for each possible event and generate the corresponding
- * FSM events. For example:
- *
- * if (asserv_move_cmd_status () == success)
- * fsm_handle_event (&ai_fsm, AI_EVENT_position_reached);
- *
- * Any unhandled event will be ignored.
- *
- * To start a sub machine from the top FSM, call the sub machine start
- * function (for example move_start), which will set parameters, then trigger
- * its start event.
- *
- * The sub machine is expected to generate an event for the top machine to
- * make it continue.
- */
-
-#ifdef HOST
-/** On host, check branches. */
-typedef u32 fsm_branch_t;
-#else
-/** On AVR, no check. */
-typedef u8 fsm_branch_t;
-#endif
-
-/** Transition function. */
-typedef fsm_branch_t (*fsm_transition_t) (void);
-
-/** Maximum number of active states. */
-#define FSM_ACTIVE_STATES_MAX 5
-
-/** Context of an FSM. */
-struct fsm_t
-{
- /** Transition table. */
- const fsm_transition_t *transition_table;
- /** Number of events, used to index the right transition. */
- u8 events_nb;
- /** Initial states. */
- u8 states_init[FSM_ACTIVE_STATES_MAX];
- /** Current active states. */
- u8 states_active[FSM_ACTIVE_STATES_MAX];
- /** Nomber of active states. */
- u8 active_states_nb;
- /** State timeout table. */
- const u16 *state_timeout_table;
- /** Current active states timeout if not 0xffff. */
- u16 states_timeout[FSM_ACTIVE_STATES_MAX];
- /** Event to generate on state timeout. */
- u8 state_timeout_event;
-#ifdef HOST
- /** FSM name. */
- const char *name;
- /** States names. */
- const char * const *states_names;
- /** Events names. */
- const char * const *events_names;
-#endif
-};
-typedef struct fsm_t fsm_t;
-
-#define FSM_TRANSITION(fsm, state, event) \
- ((fsm_transition_t) pgm_read_word ( \
- &(fsm)->transition_table[(state) * (fsm)->events_nb + (event)]))
-
-/** Reset a FSM. */
-void
-fsm_init (fsm_t *fsm);
-
-/** Handle state timeout, return non-zero if at least an event was handled. */
-uint8_t
-fsm_handle_timeout (fsm_t *fsm);
-
-/** Handle an event on the given FSM, return non-zero if at least an event was
- * handled. */
-uint8_t
-fsm_handle_event (fsm_t *fsm, uint8_t event);
-
-/** Return non zero if event can be handled. This can be used if event
- * generation requires expensive computations. */
-uint8_t
-fsm_can_handle_event (fsm_t *fsm, uint8_t event);
-
-#include "ai_fsm.h"
-
-#endif /* fsm_h */
diff --git a/digital/io/src/fsm_templates/template_cb.h b/digital/io/src/fsm_templates/template_cb.h
deleted file mode 100644
index 4f4deeec..00000000
--- a/digital/io/src/fsm_templates/template_cb.h
+++ /dev/null
@@ -1,10 +0,0 @@
-#ifndef %(prefix)s_cb_h
-#define %(prefix)s_cb_h
-/*
- * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT!
- *
- * %(name)s callbacks declaration.
- *
-%(*comments)s */
-
-%(states_template,template_cb_decl.h)s#endif /* %(prefix)s_cb_h */
diff --git a/digital/io/src/fsm_templates/template_cb_decl.h b/digital/io/src/fsm_templates/template_cb_decl.h
deleted file mode 100644
index fd72d678..00000000
--- a/digital/io/src/fsm_templates/template_cb_decl.h
+++ /dev/null
@@ -1,6 +0,0 @@
-/*
- * %(state)s =%(event)s=>
-%(*branches_to)s */
-fsm_branch_t
-%(prefix)s__%(state)s__%(event)s (void);
-
diff --git a/digital/io/src/fsm_templates/template_cb_impl.c b/digital/io/src/fsm_templates/template_cb_impl.c
deleted file mode 100644
index 91ff4fbb..00000000
--- a/digital/io/src/fsm_templates/template_cb_impl.c
+++ /dev/null
@@ -1,9 +0,0 @@
-/*
- * %(state)s =%(event)s=>
-%(*branches_to)s */
-fsm_branch_t
-%(prefix)s__%(state)s__%(event)s (void)
-{
-%(returns)s
-}
-
diff --git a/digital/io/src/fsm_templates/template_fsm.c b/digital/io/src/fsm_templates/template_fsm.c
deleted file mode 100644
index 03ce5abf..00000000
--- a/digital/io/src/fsm_templates/template_fsm.c
+++ /dev/null
@@ -1,55 +0,0 @@
-/*
- * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT!
- *
- * %(name)s
- *
-%(*comments)s */
-#include "common.h"
-#include "fsm.h"
-#include "%(prefix)s_cb.h"
-
-#define NULL ((void *)0L)
-
-/* %(name)s transition table. */
-static const fsm_transition_t PROGMEM
-%(prefix)s_transition_table[%(PREFIX)s_STATE_NB][%(PREFIX)s_EVENT_NB] = {
-%(transition_table)s};
-
-/* %(name)s state timeout table. */
-static const uint16_t
-%(prefix)s_state_timeout_table[%(PREFIX)s_STATE_NB] = {
-%(states,%(@timeout|0xffff)s)s};
-
-#ifdef HOST
-
-/* %(name)s state names. */
-static const char *
-%(prefix)s_states_names[] = {
-%(states_names)s};
-
-/* %(name)s event names. */
-static const char *
-%(prefix)s_events_names[] = {
-%(events_names)s};
-
-#endif /* HOST */
-
-/* %(name)s context. */
-fsm_t %(prefix)s_fsm = {
- &%(prefix)s_transition_table[0][0],
- %(PREFIX)s_EVENT_NB,
- {
- %(initials)s},
- {
- %(initials)s},
- %(initials_nb)s,
- &%(prefix)s_state_timeout_table[0],
- { 0 },
- %(PREFIX)s_EVENT_state_timeout,
-#ifdef HOST
- "%(name)s",
- %(prefix)s_states_names,
- %(prefix)s_events_names,
-#endif
-};
-
diff --git a/digital/io/src/fsm_templates/template_fsm.h b/digital/io/src/fsm_templates/template_fsm.h
deleted file mode 100644
index 9ac98895..00000000
--- a/digital/io/src/fsm_templates/template_fsm.h
+++ /dev/null
@@ -1,52 +0,0 @@
-#ifndef %(prefix)s_fsm_h
-#define %(prefix)s_fsm_h
-/*
- * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT!
- *
- * %(name)s
- *
-%(*comments)s */
-
-/* %(name)s states. */
-enum %(prefix)s_state_t
-{
-%(states)s %(PREFIX)s_STATE_NB
-};
-
-/* %(name)s events. */
-enum %(prefix)s_event_t
-{
-%(events)s %(PREFIX)s_EVENT_NB
-};
-
-/* This macro enables checks for branches used in the wrong state/event
- * combination. */
-#ifdef HOST
-# define _BRANCH(state, event, to) \
- ((%(PREFIX)s_STATE_ ## state) << 16 \
- | (%(PREFIX)s_EVENT_ ## event) << 8 \
- | (%(PREFIX)s_STATE_ ## to))
-#else
-# define _BRANCH(state, event, to) \
- ((%(PREFIX)s_STATE_ ## to))
-#endif
-
-/* %(name)s branches. */
-enum %(prefix)s_branch_t
-{
-%(branches)s};
-
-#undef _BRANCH
-
-/* Value to return to follow the only branch. */
-#define %(prefix)s_next(state, event) \
- %(PREFIX)s_BRANCH__ ## state ## __ ## event ## __
-
-/* Value to return to follow a given branch. */
-#define %(prefix)s_next_branch(state, event, branch) \
- %(PREFIX)s_BRANCH__ ## state ## __ ## event ## __ ## branch
-
-/* %(name)s context. */
-extern fsm_t %(prefix)s_fsm;
-
-#endif /* %(prefix)s_fsm_h */
diff --git a/digital/io/src/fsm_templates/template_hola_cb_skel.c b/digital/io/src/fsm_templates/template_hola_cb_skel.c
deleted file mode 100644
index 980718da..00000000
--- a/digital/io/src/fsm_templates/template_hola_cb_skel.c
+++ /dev/null
@@ -1,11 +0,0 @@
-/*
- * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT!
- *
- * Skeleton for %(name)s hola callbacks implementation.
- *
-%(*comments)s */
-#include "common.h"
-#include "fsm.h"
-#include "%(prefix)s_cb.h"
-
-%(states_template,template_cb_impl.c,origin=hola)s
diff --git a/digital/io/src/fsm_templates/template_init_cb_skel.c b/digital/io/src/fsm_templates/template_init_cb_skel.c
deleted file mode 100644
index fea9270e..00000000
--- a/digital/io/src/fsm_templates/template_init_cb_skel.c
+++ /dev/null
@@ -1,11 +0,0 @@
-/*
- * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT!
- *
- * Skeleton for %(name)s init callbacks implementation.
- *
-%(*comments)s */
-#include "common.h"
-#include "fsm.h"
-#include "%(prefix)s_cb.h"
-
-%(states_template,template_cb_impl.c,origin=init)s
diff --git a/digital/io/src/fsm_templates/template_loader_cb_skel.c b/digital/io/src/fsm_templates/template_loader_cb_skel.c
deleted file mode 100644
index 06398952..00000000
--- a/digital/io/src/fsm_templates/template_loader_cb_skel.c
+++ /dev/null
@@ -1,11 +0,0 @@
-/*
- * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT!
- *
- * Skeleton for %(name)s loader callbacks implementation.
- *
-%(*comments)s */
-#include "common.h"
-#include "fsm.h"
-#include "%(prefix)s_cb.h"
-
-%(states_template,template_cb_impl.c,origin=loader)s
diff --git a/digital/io/src/fsm_templates/template_move_cb_skel.c b/digital/io/src/fsm_templates/template_move_cb_skel.c
deleted file mode 100644
index 533ffbfd..00000000
--- a/digital/io/src/fsm_templates/template_move_cb_skel.c
+++ /dev/null
@@ -1,11 +0,0 @@
-/*
- * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT!
- *
- * Skeleton for %(name)s move callbacks implementation.
- *
-%(*comments)s */
-#include "common.h"
-#include "fsm.h"
-#include "%(prefix)s_cb.h"
-
-%(states_template,template_cb_impl.c,origin=move)s
diff --git a/digital/io/src/fsm_templates/template_top_cb_skel.c b/digital/io/src/fsm_templates/template_top_cb_skel.c
deleted file mode 100644
index 8895dc64..00000000
--- a/digital/io/src/fsm_templates/template_top_cb_skel.c
+++ /dev/null
@@ -1,11 +0,0 @@
-/*
- * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT!
- *
- * Skeleton for %(name)s top callbacks implementation.
- *
-%(*comments)s */
-#include "common.h"
-#include "fsm.h"
-#include "%(prefix)s_cb.h"
-
-%(states_template,template_cb_impl.c,origin=top)s
diff --git a/digital/io/src/hola.c b/digital/io/src/hola.c
index 1fd15f18..c7d07e21 100644
--- a/digital/io/src/hola.c
+++ b/digital/io/src/hola.c
@@ -23,3 +23,118 @@
*
* }}} */
+#define FSM_NAME AI
+
+#include "common.h"
+#include "fsm.h"
+#include "asserv.h"
+#include "mimot.h"
+#include "bot.h"
+#include "playground.h"
+
+#include "modules/math/fixed/fixed.h"
+
+uint32_t hola_angle;
+int16_t hola_timeout;
+
+FSM_STATES (
+ /* waiting start */
+ HOLA_IDLE,
+ /* wait hola signal */
+ HOLA_WAIT_HOLA,
+ /* wait until the jack is inserted for the first time */
+ HOLA_WAIT_JACK_IN,
+ /* wait until the jack is removed to initialise robot */
+ HOLA_WAIT_JACK_OUT,
+ /* moving clamps in rounds */
+ HOLA_ROUNDS,
+ /* moving clamp up */
+ HOLA_UP,
+ HOLA_CENTER,
+ HOLA_CLAP_CLOSE,
+ HOLA_CLAP_OPEN)
+
+FSM_EVENTS (hola_start)
+
+FSM_START_WITH (HOLA_IDLE)
+
+FSM_TRANS (HOLA_IDLE, start, HOLA_WAIT_HOLA)
+{
+ return FSM_NEXT (HOLA_IDLE, start);
+}
+
+FSM_TRANS (HOLA_WAIT_HOLA, hola_start, HOLA_WAIT_JACK_IN)
+{
+ return FSM_NEXT (HOLA_WAIT_HOLA, hola_start);
+}
+
+FSM_TRANS (HOLA_WAIT_JACK_IN, jack_inserted_into_bot, HOLA_WAIT_JACK_OUT)
+{
+ return FSM_NEXT (HOLA_WAIT_JACK_IN, jack_inserted_into_bot);
+}
+
+FSM_TRANS (HOLA_WAIT_JACK_OUT, jack_removed_from_bot, HOLA_ROUNDS)
+{
+ return FSM_NEXT (HOLA_WAIT_JACK_OUT, jack_removed_from_bot);
+}
+
+FSM_TRANS (HOLA_ROUNDS, asserv_last_cmd_ack,
+ /* move clamp rounds */
+ no_timeout, HOLA_ROUNDS,
+ /* move up, rotate */
+ timeout, HOLA_UP)
+{
+ if (++hola_timeout < 150)
+ {
+ int32_t cos = fixed_cos_f824 (hola_angle) + 0x1000000;
+ int32_t sin = fixed_sin_f824 (hola_angle) + 0x1000000;
+ int32_t dx = BOT_CLAMP_OPEN_STEP
+ + fixed_mul_f824 (BOT_CLAMP_STROKE_STEP / 3 / 2, cos);
+ int32_t dy = BOT_ELEVATOR_ZERO_SPEED
+ + fixed_mul_f824 (BOT_ELEVATOR_REST_STEP / 3 / 2, sin);
+ mimot_move_motor0_absolute (dx, BOT_CLAMP_SPEED);
+ mimot_move_motor1_absolute (dx, BOT_CLAMP_SPEED);
+ asserv_move_motor0_absolute (dy, BOT_ELEVATOR_SPEED);
+ hola_angle += 0x1000000 / (225 / 12);
+ return FSM_NEXT (HOLA_ROUNDS, asserv_last_cmd_ack, no_timeout);
+ }
+ else
+ {
+ asserv_set_speed (0x10, 0x1c, 0x10, 0x1c);
+ asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED / 3);
+ asserv_move_angularly (POSITION_A_DEG (174));
+ return FSM_NEXT (HOLA_ROUNDS, asserv_last_cmd_ack, timeout);
+ }
+}
+
+FSM_TRANS (HOLA_UP, bot_move_succeed, HOLA_CENTER)
+{
+ asserv_set_speed (0x10, 0x20, 0x10, 0x20);
+ asserv_move_angularly (POSITION_A_DEG (-90));
+ asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP / 2, BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (HOLA_UP, bot_move_succeed);
+}
+
+FSM_TRANS (HOLA_CENTER, bot_move_succeed, HOLA_CLAP_CLOSE)
+{
+ mimot_motor0_clamp (BOT_CLAMP_ZERO_SPEED, 0);
+ mimot_motor1_clamp (BOT_CLAMP_ZERO_SPEED, 0);
+ return FSM_NEXT (HOLA_CENTER, bot_move_succeed);
+}
+
+FSM_TRANS (HOLA_CLAP_CLOSE, clamp_succeed, HOLA_CLAP_OPEN)
+{
+ mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ return FSM_NEXT (HOLA_CLAP_CLOSE, clamp_succeed);
+}
+
+/*
+ * close clamp
+ */
+FSM_TRANS (HOLA_CLAP_OPEN, clamp_succeed, HOLA_CLAP_CLOSE)
+{
+ mimot_motor0_clamp (BOT_CLAMP_SPEED, 0);
+ mimot_motor1_clamp (BOT_CLAMP_SPEED, 0);
+ return FSM_NEXT (HOLA_CLAP_OPEN, clamp_succeed);
+}
diff --git a/digital/io/src/hola.fsm b/digital/io/src/hola.fsm
deleted file mode 100644
index ca219491..00000000
--- a/digital/io/src/hola.fsm
+++ /dev/null
@@ -1,59 +0,0 @@
-# Hola FSM
-hola
- entertain people
-
-States:
- *HOLA_IDLE
- waiting start
- HOLA_WAIT_HOLA
- wait hola signal
- HOLA_WAIT_JACK_IN
- wait until the jack is inserted for the first time
- HOLA_WAIT_JACK_OUT
- wait until the jack is removed to initialise robot
- HOLA_ROUNDS
- moving clamps in rounds
- HOLA_UP
- moving clamp up
- HOLA_CENTER
- HOLA_CLAP_CLOSE
- HOLA_CLAP_OPEN
-
-Events:
- hola_start
- start hola
-
-HOLA_IDLE:
- start -> HOLA_WAIT_HOLA
-
-HOLA_WAIT_HOLA:
- hola_start -> HOLA_WAIT_JACK_IN
-
-HOLA_WAIT_JACK_IN:
- jack_inserted_into_bot -> HOLA_WAIT_JACK_OUT
-
-HOLA_WAIT_JACK_OUT:
- jack_removed_from_bot -> HOLA_ROUNDS
-
-HOLA_ROUNDS:
- asserv_last_cmd_ack: no_timeout -> .
- move clamp rounds
- asserv_last_cmd_ack: timeout -> HOLA_UP
- move up
- rotate
-
-HOLA_UP:
- bot_move_succeed -> HOLA_CENTER
- rotate to center
-
-HOLA_CENTER:
- bot_move_succeed -> HOLA_CLAP_CLOSE
- close clamp
-
-HOLA_CLAP_CLOSE:
- clamp_succeed -> HOLA_CLAP_OPEN
- open clamp
-
-HOLA_CLAP_OPEN:
- clamp_succeed -> HOLA_CLAP_CLOSE
- close clamp
diff --git a/digital/io/src/init.c b/digital/io/src/init.c
index 1d0d8d82..58c94333 100644
--- a/digital/io/src/init.c
+++ b/digital/io/src/init.c
@@ -22,8 +22,212 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
-#include "common.h"
+#define FSM_NAME AI
+#include "common.h"
+#include "fsm.h"
+#include "asserv.h"
#include "init.h"
+#include "playground_2010.h"
+#include "main.h"
+#include "bot.h"
+#include "switch.h"
+#include "chrono.h"
+#include "playground.h"
+#include "modules/trace/trace.h"
+#include "modules/path/path.h"
uint8_t init_match_is_started = 0;
+
+FSM_STATES (
+ /* waiting for the beginning of the top FSM. */
+ INIT_IDLE,
+ /* wait for the jack to be inserted into the bot for the first time. */
+ INIT_WAIT_FIRST_JACK_IN,
+ /* wait the jack to be removed from the bot for the first time. */
+ INIT_WAIT_FIRST_JACK_OUT,
+ /* wait for the jack to be inserted into the bot for the second time. */
+ INIT_WAIT_SECOND_JACK_IN,
+ /* just wait for operator hands to be removed from the jack. */
+ INIT_WAIT_FOR_HANDS_OUT,
+ /* go to the wall for the first time. */
+ INIT_GO_TO_THE_WALL,
+ /* reset the Y position of the bot.
+ * reset the angular position of the bot.
+ * go backward from the wall for a specific distance. */
+ INIT_GO_AWAY_FROM_THE_WALL,
+ /* make the bot face the other wall. */
+ INIT_FACE_OTHER_WALL,
+ /* make sure rotation is really finished before continuing. */
+ INIT_WAIT_AFTER_ROTATION,
+ /* go to the wall for the second time. */
+ INIT_GO_TO_THE_WALL_AGAIN,
+ /* reset the X position of the bot.
+ * go backward from the wall for a specific distance. */
+ INIT_GO_AWAY_FROM_THE_WALL_AGAIN,
+ /* go to the start position. */
+ INIT_GO_TO_START_POSITION,
+ /* wait the jack to be removed from the bot for the second time. */
+ INIT_WAIT_SECOND_JACK_OUT)
+
+FSM_EVENTS (
+ /* the jack is inserted into the bot. */
+ jack_inserted_into_bot,
+ /* the jack is removed from the bot. */
+ jack_removed_from_bot)
+
+FSM_START_WITH (INIT_IDLE)
+
+/*
+ * do nothing.
+ */
+FSM_TRANS (INIT_IDLE, start, INIT_WAIT_FIRST_JACK_IN)
+{
+ return FSM_NEXT (INIT_IDLE, start);
+}
+
+/*
+ * do nothing.
+ */
+FSM_TRANS (INIT_WAIT_FIRST_JACK_IN,
+ jack_inserted_into_bot,
+ INIT_WAIT_FIRST_JACK_OUT)
+{
+ return FSM_NEXT (INIT_WAIT_FIRST_JACK_IN, jack_inserted_into_bot);
+}
+
+/*
+ * start trace module.
+ * get and store the color of the bot.
+ */
+FSM_TRANS (INIT_WAIT_FIRST_JACK_OUT,
+ jack_removed_from_bot,
+ INIT_WAIT_SECOND_JACK_IN)
+{
+ /* Initialize trace module (erase the flash). */
+ trace_init ();
+ /* Get the color. */
+ team_color = switch_get_color ();
+ return FSM_NEXT (INIT_WAIT_FIRST_JACK_OUT, jack_removed_from_bot);
+}
+
+/*
+ * do nothing.
+ */
+FSM_TRANS (INIT_WAIT_SECOND_JACK_IN,
+ jack_inserted_into_bot,
+ INIT_WAIT_FOR_HANDS_OUT)
+{
+ return FSM_NEXT (INIT_WAIT_SECOND_JACK_IN, jack_inserted_into_bot);
+}
+
+/*
+ * fuck the wall in front.
+ */
+FSM_TRANS_TIMEOUT (INIT_WAIT_FOR_HANDS_OUT, 225,
+ INIT_GO_TO_THE_WALL)
+{
+ /* Go to the wall, no backward. */
+ asserv_go_to_the_wall (0);
+ return FSM_NEXT_TIMEOUT (INIT_WAIT_FOR_HANDS_OUT);
+}
+
+/*
+ * reset the Y position of the bot.
+ * reset the angular position of the bot.
+ * move away from the wall (linear move).
+ */
+FSM_TRANS (INIT_GO_TO_THE_WALL,
+ bot_move_succeed,
+ INIT_GO_AWAY_FROM_THE_WALL)
+{
+ asserv_set_y_position (PG_Y (PG_LENGTH - BOT_SIZE_FRONT));
+ asserv_set_angle_position (PG_A_DEG (90));
+ /* Move away from the border. */
+ asserv_move_linearly (- INIT_DIST);
+ return FSM_NEXT (INIT_GO_TO_THE_WALL, bot_move_succeed);
+}
+
+/*
+ * turn to face the other wall.
+ */
+FSM_TRANS (INIT_GO_AWAY_FROM_THE_WALL,
+ bot_move_succeed,
+ INIT_FACE_OTHER_WALL)
+{
+ /* Face the other wall. */
+ asserv_goto_angle (PG_A_DEG (180));
+ return FSM_NEXT (INIT_GO_AWAY_FROM_THE_WALL, bot_move_succeed);
+}
+
+/*
+ * nothing to do.
+ */
+FSM_TRANS (INIT_FACE_OTHER_WALL,
+ bot_move_succeed,
+ INIT_WAIT_AFTER_ROTATION)
+{
+ return FSM_NEXT (INIT_FACE_OTHER_WALL, bot_move_succeed);
+}
+
+/*
+ * fuck the wall in front.
+ */
+FSM_TRANS_TIMEOUT (INIT_WAIT_AFTER_ROTATION, 100,
+ INIT_GO_TO_THE_WALL_AGAIN)
+{
+ /* Go to the wall, no backward. */
+ asserv_go_to_the_wall (0);
+ return FSM_NEXT_TIMEOUT (INIT_WAIT_AFTER_ROTATION);
+}
+
+/*
+ * reset the X position of the bot.
+ * move away from the wall (linear move).
+ */
+FSM_TRANS (INIT_GO_TO_THE_WALL_AGAIN,
+ bot_move_succeed,
+ INIT_GO_AWAY_FROM_THE_WALL_AGAIN)
+{
+ asserv_set_x_position (PG_X (BOT_SIZE_FRONT));
+ /* Move away from the border. */
+ asserv_move_linearly (- INIT_DIST);
+ return FSM_NEXT (INIT_GO_TO_THE_WALL_AGAIN, bot_move_succeed);
+}
+
+/*
+ * go to the start position with a go to movement.
+ */
+FSM_TRANS (INIT_GO_AWAY_FROM_THE_WALL_AGAIN,
+ bot_move_succeed,
+ INIT_GO_TO_START_POSITION)
+{
+ /* Move away from the border. */
+ asserv_goto_xya (BOT_INIT_XYA, 0);
+ return FSM_NEXT (INIT_GO_AWAY_FROM_THE_WALL_AGAIN, bot_move_succeed);
+}
+
+/*
+ * nothing to do, the bot is at the start position.
+ */
+FSM_TRANS (INIT_GO_TO_START_POSITION,
+ bot_move_succeed,
+ INIT_WAIT_SECOND_JACK_OUT)
+{
+ return FSM_NEXT (INIT_GO_TO_START_POSITION, bot_move_succeed);
+}
+
+/*
+ * tell other FSM the match begins.
+ * start the chrono.
+ */
+FSM_TRANS (INIT_WAIT_SECOND_JACK_OUT,
+ jack_removed_from_bot,
+ INIT_IDLE)
+{
+ /* Set the flag to transmit to other FSM. */
+ init_match_is_started = 1;
+ /* Start the chrono. */
+ chrono_init ();
+ return FSM_NEXT (INIT_WAIT_SECOND_JACK_OUT, jack_removed_from_bot);
+}
diff --git a/digital/io/src/init.fsm b/digital/io/src/init.fsm
deleted file mode 100644
index f780ba45..00000000
--- a/digital/io/src/init.fsm
+++ /dev/null
@@ -1,106 +0,0 @@
-# init FSM
-# This FSM is used to place the bot at a "start position" for a match. It is
-# more precise than a human hand and reset the asserv board to the start
-# position.
-# Some remarks:
-# - for security reason, we need to insert the jack then removed it, then
-# insert it back. Why? Because it can already be inserted.
-# - other FSM can do a similar thing by using the first jack out to
-# initialize.
-init
- place the bot in the start position for a match.
-
-States:
- *INIT_IDLE
- waiting for the beginning of the top FSM.
- INIT_WAIT_FIRST_JACK_IN
- wait for the jack to be inserted into the bot for the first time.
- INIT_WAIT_FIRST_JACK_OUT
- wait the jack to be removed from the bot for the first time.
- INIT_WAIT_SECOND_JACK_IN
- wait for the jack to be inserted into the bot for the second time.
- INIT_WAIT_FOR_HANDS_OUT[timeout=225]
- just wait for operator hands to be removed from the jack.
- INIT_GO_TO_THE_WALL
- go to the wall for the first time.
- INIT_GO_AWAY_FROM_THE_WALL
- reset the Y position of the bot.
- reset the angular position of the bot.
- go backward from the wall for a specific distance.
- INIT_FACE_OTHER_WALL
- make the bot face the other wall.
- INIT_WAIT_AFTER_ROTATION [timeout=100]
- make sure rotation is really finished before continuing.
- INIT_GO_TO_THE_WALL_AGAIN
- go to the wall for the second time.
- INIT_GO_AWAY_FROM_THE_WALL_AGAIN
- reset the X position of the bot.
- go backward from the wall for a specific distance.
- INIT_GO_TO_START_POSITION
- go to the start position.
- INIT_WAIT_SECOND_JACK_OUT
- wait the jack to be removed from the bot for the second time.
-
-Events:
- jack_inserted_into_bot
- the jack is inserted into the bot.
- jack_removed_from_bot
- the jack is removed from the bot.
-
-INIT_IDLE:
- start -> INIT_WAIT_FIRST_JACK_IN
- do nothing.
-
-INIT_WAIT_FIRST_JACK_IN:
- jack_inserted_into_bot -> INIT_WAIT_FIRST_JACK_OUT
- do nothing.
-
-INIT_WAIT_FIRST_JACK_OUT:
- jack_removed_from_bot -> INIT_WAIT_SECOND_JACK_IN
- start trace module.
- get and store the color of the bot.
-
-INIT_WAIT_SECOND_JACK_IN:
- jack_inserted_into_bot -> INIT_WAIT_FOR_HANDS_OUT
- do nothing.
-
-INIT_WAIT_FOR_HANDS_OUT:
- state_timeout -> INIT_GO_TO_THE_WALL
- fuck the wall in front.
-
-INIT_GO_TO_THE_WALL:
- bot_move_succeed -> INIT_GO_AWAY_FROM_THE_WALL
- reset the Y position of the bot.
- reset the angular position of the bot.
- move away from the wall (linear move).
-
-INIT_GO_AWAY_FROM_THE_WALL:
- bot_move_succeed -> INIT_FACE_OTHER_WALL
- turn to face the other wall.
-
-INIT_FACE_OTHER_WALL:
- bot_move_succeed -> INIT_WAIT_AFTER_ROTATION
- nothing to do.
-
-INIT_WAIT_AFTER_ROTATION:
- state_timeout -> INIT_GO_TO_THE_WALL_AGAIN
- fuck the wall in front.
-
-INIT_GO_TO_THE_WALL_AGAIN:
- bot_move_succeed -> INIT_GO_AWAY_FROM_THE_WALL_AGAIN
- reset the X position of the bot.
- move away from the wall (linear move).
-
-INIT_GO_AWAY_FROM_THE_WALL_AGAIN:
- bot_move_succeed -> INIT_GO_TO_START_POSITION
- go to the start position with a go to movement.
-
-INIT_GO_TO_START_POSITION:
- bot_move_succeed -> INIT_WAIT_SECOND_JACK_OUT
- nothing to do, the bot is at the start position.
-
-INIT_WAIT_SECOND_JACK_OUT:
- jack_removed_from_bot -> INIT_IDLE
- tell other FSM the match begins.
- start the chrono.
-
diff --git a/digital/io/src/loader.c b/digital/io/src/loader.c
index 4157023e..e8f8c139 100644
--- a/digital/io/src/loader.c
+++ b/digital/io/src/loader.c
@@ -22,10 +22,16 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+#define FSM_NAME AI
+
#include "common.h"
#include "loader.h"
-
#include "fsm.h"
+#include "asserv.h"
+#include "mimot.h"
+#include "bot.h"
+#include "fsm_queue.h"
+#include "food.h"
uint8_t loader_elements;
uint8_t loader_want_up;
@@ -34,13 +40,614 @@ void
loader_up (void)
{
loader_want_up = 1;
- fsm_handle_event (&ai_fsm, AI_EVENT_loader_up);
+ FSM_HANDLE (AI, loader_up);
}
void
loader_down (void)
{
loader_want_up = 0;
- fsm_handle_event (&ai_fsm, AI_EVENT_loader_down);
+ FSM_HANDLE (AI, loader_down);
+}
+
+FSM_STATES (
+ /* wait start */
+ LOADER_IDLE,
+ /* wait until the jack is inserted for the first time */
+ LOADER_WAIT_JACK_IN,
+ /* wait until the jack is removed to initialise robot */
+ LOADER_WAIT_JACK_OUT,
+ /* move the elevator down to find zero position */
+ LOADER_INIT_ELEVATOR_ZERO,
+ /* test clamp closing */
+ LOADER_INIT_CLAMP_CLOSE,
+ /* find clamp zero position */
+ LOADER_INIT_CLAMP_ZERO,
+ /* find gate zero */
+ LOADER_INIT_GATE_ZERO,
+ /* move the elevator up so that the robot can go to the wall */
+ LOADER_INIT_ELEVATOR_UP,
+ /* wait with gate open */
+ LOADER_INIT_GATE_WAIT,
+ /* close gate before start */
+ LOADER_INIT_GATE_CLOSE,
+ /* up position, clamp is protected */
+ LOADER_UP,
+ /* down position, ready to take an element */
+ LOADER_DOWN,
+ /* moving to the up position */
+ LOADER_UPING,
+ /* moving to the down position */
+ LOADER_DOWNING,
+ /* error while moving up, stop in an unknown state */
+ LOADER_ERROR,
+ /* error recovery moving down */
+ LOADER_ERROR_DOWNING,
+ /* error recovery moving down, open clamp */
+ LOADER_ERROR_DOWNING_OPEN,
+ /* error recovery moving up */
+ LOADER_ERROR_UPING,
+ /* error recovery moving up, open clamp */
+ LOADER_ERROR_UPING_OPEN,
+ /* close clamp */
+ LOADER_LOAD_CLOSING,
+ /* move load up to the unload position */
+ LOADER_LOAD_UPING,
+ /* open clamp to unload, wait until unload position */
+ LOADER_LOAD_UNLOADING,
+ /* check clamp is open */
+ LOADER_LOAD_UNLOADING_OPEN,
+ /* open clamp due to no elements to pick */
+ LOADER_LOAD_EMPTY_OPEN)
+
+FSM_EVENTS (
+ /* asserv success result */
+ elevator_succeed,
+ /* asserv failure result */
+ elevator_failed,
+ /* elevator above unloading position */
+ elevator_unload_position,
+ /* asserv failure result */
+ clamp_failed,
+ /* asserv success result */
+ gate_succeed,
+ /* asserv failure result */
+ gate_failed,
+ /* element sensed between clamp to be loaded */
+ loader_element,
+ /* make the loader ready to load an element */
+ loader_down,
+ /* protect the clamp by moving it up */
+ loader_up,
+ /* posted when loader successfully moved down */
+ loader_downed,
+ /* posted when loader successfully moved up */
+ loader_uped,
+ /* posted when loader in an unknown state due to error while moving up */
+ loader_errored,
+ /* loader blocked, due to a black corn */
+ loader_black)
+
+FSM_START_WITH (LOADER_IDLE)
+
+FSM_TRANS (LOADER_IDLE, start, LOADER_WAIT_JACK_IN)
+{
+ return FSM_NEXT (LOADER_IDLE, start);
+}
+
+FSM_TRANS (LOADER_WAIT_JACK_IN,
+ jack_inserted_into_bot,
+ LOADER_WAIT_JACK_OUT)
+{
+ return FSM_NEXT (LOADER_WAIT_JACK_IN, jack_inserted_into_bot);
+}
+
+FSM_TRANS (LOADER_WAIT_JACK_IN,
+ hola_start,
+ LOADER_IDLE)
+{
+ return FSM_NEXT (LOADER_WAIT_JACK_IN, hola_start);
+}
+
+/*
+ * find elevator zero
+ * close clamp
+ * find gate zero
+ */
+FSM_TRANS (LOADER_WAIT_JACK_OUT,
+ jack_removed_from_bot,
+ LOADER_INIT_ELEVATOR_ZERO)
+{
+ asserv_motor0_zero_position (-BOT_ELEVATOR_ZERO_SPEED);
+ mimot_motor0_clamp (BOT_CLAMP_ZERO_SPEED, 0);
+ mimot_motor1_clamp (BOT_CLAMP_ZERO_SPEED, 0);
+ asserv_motor1_zero_position (-BOT_GATE_SPEED);
+ return FSM_NEXT (LOADER_WAIT_JACK_OUT, jack_removed_from_bot);
+}
+
+FSM_TRANS (LOADER_INIT_ELEVATOR_ZERO,
+ elevator_succeed,
+ LOADER_INIT_CLAMP_CLOSE)
+{
+ return FSM_NEXT (LOADER_INIT_ELEVATOR_ZERO, elevator_succeed);
+}
+
+/*
+ * move elevator up
+ * find clamp zero
+ */
+FSM_TRANS (LOADER_INIT_CLAMP_CLOSE,
+ clamp_succeed,
+ LOADER_INIT_CLAMP_ZERO)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP,
+ BOT_ELEVATOR_ZERO_SPEED);
+ mimot_motor0_zero_position (-BOT_CLAMP_ZERO_SPEED);
+ mimot_motor1_zero_position (-BOT_CLAMP_ZERO_SPEED);
+ return FSM_NEXT (LOADER_INIT_CLAMP_CLOSE, clamp_succeed);
+}
+
+FSM_TRANS (LOADER_INIT_CLAMP_ZERO,
+ clamp_succeed,
+ LOADER_INIT_GATE_ZERO)
+{
+ return FSM_NEXT (LOADER_INIT_CLAMP_ZERO, clamp_succeed);
+}
+
+FSM_TRANS (LOADER_INIT_GATE_ZERO,
+ gate_succeed,
+ LOADER_INIT_ELEVATOR_UP)
+{
+ return FSM_NEXT (LOADER_INIT_GATE_ZERO, gate_succeed);
+}
+
+FSM_TRANS (LOADER_INIT_ELEVATOR_UP,
+ elevator_succeed,
+ LOADER_INIT_GATE_WAIT)
+{
+ return FSM_NEXT (LOADER_INIT_ELEVATOR_UP, elevator_succeed);
+}
+
+/*
+ * initialisation failure
+ */
+FSM_TRANS (LOADER_INIT_ELEVATOR_UP,
+ elevator_failed,
+ LOADER_IDLE)
+{
+ /* Move so that the operator notice it. */
+ asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP / 3,
+ BOT_ELEVATOR_ZERO_SPEED);
+ return FSM_NEXT (LOADER_INIT_ELEVATOR_UP, elevator_failed);
}
+/*
+ * close gate
+ */
+FSM_TRANS_TIMEOUT (LOADER_INIT_GATE_WAIT, 225,
+ LOADER_INIT_GATE_CLOSE)
+{
+ asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED);
+ return FSM_NEXT_TIMEOUT (LOADER_INIT_GATE_WAIT);
+}
+
+FSM_TRANS (LOADER_INIT_GATE_CLOSE,
+ gate_succeed,
+ LOADER_UP)
+{
+ return FSM_NEXT (LOADER_INIT_GATE_CLOSE, gate_succeed);
+}
+
+/*
+ * move down
+ */
+FSM_TRANS (LOADER_UP,
+ loader_down,
+ LOADER_DOWNING)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (LOADER_UP, loader_down);
+}
+
+/*
+ * move up
+ */
+FSM_TRANS (LOADER_DOWN,
+ loader_up,
+ LOADER_UPING)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (LOADER_DOWN, loader_up);
+}
+
+/*
+ * clamp
+ */
+FSM_TRANS (LOADER_DOWN,
+ loader_element,
+ LOADER_LOAD_CLOSING)
+{
+ mimot_motor0_clamp (BOT_CLAMP_SPEED, BOT_CLAMP_PWM);
+ mimot_motor1_clamp (BOT_CLAMP_SPEED, BOT_CLAMP_PWM);
+ return FSM_NEXT (LOADER_DOWN, loader_element);
+}
+
+/*
+ * post loader_uped event
+ */
+FSM_TRANS (LOADER_UPING,
+ elevator_succeed,
+ LOADER_UP)
+{
+ fsm_queue_post_event (FSM_EVENT (AI,loader_uped));
+ return FSM_NEXT (LOADER_UPING, elevator_succeed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS (LOADER_UPING,
+ elevator_failed,
+ LOADER_ERROR)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT (LOADER_UPING, elevator_failed);
+}
+
+/*
+ * move down
+ */
+FSM_TRANS (LOADER_UPING,
+ loader_down,
+ LOADER_DOWNING)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (LOADER_UPING, loader_down);
+}
+
+/*
+ * release elevator motor
+ * post loader_downed event
+ */
+FSM_TRANS (LOADER_DOWNING,
+ elevator_succeed,
+ LOADER_DOWN)
+{
+ asserv_motor0_free ();
+ fsm_queue_post_event (FSM_EVENT (AI, loader_downed));
+ return FSM_NEXT (LOADER_DOWNING, elevator_succeed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS (LOADER_DOWNING,
+ elevator_failed,
+ LOADER_ERROR)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT (LOADER_DOWNING, elevator_failed);
+}
+
+/*
+ * emergency stop
+ * post loader_errored event
+ */
+FSM_TRANS (LOADER_DOWNING,
+ loader_element,
+ LOADER_ERROR)
+{
+ asserv_motor0_free ();
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT (LOADER_DOWNING, loader_element);
+}
+
+/*
+ * move up
+ */
+FSM_TRANS (LOADER_DOWNING,
+ loader_up,
+ LOADER_UPING)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (LOADER_DOWNING, loader_up);
+}
+
+/*
+ * move down
+ */
+FSM_TRANS (LOADER_ERROR,
+ loader_down,
+ LOADER_ERROR_DOWNING)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (LOADER_ERROR, loader_down);
+}
+
+/*
+ * move up
+ */
+FSM_TRANS (LOADER_ERROR,
+ loader_up,
+ LOADER_ERROR_UPING)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP, BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (LOADER_ERROR, loader_up);
+}
+
+/*
+ * release elevator motor
+ * open clamp
+ */
+FSM_TRANS (LOADER_ERROR_DOWNING,
+ elevator_succeed,
+ LOADER_ERROR_DOWNING_OPEN)
+{
+ asserv_motor0_free ();
+ mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ return FSM_NEXT (LOADER_ERROR_DOWNING, elevator_succeed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS (LOADER_ERROR_DOWNING,
+ elevator_failed,
+ LOADER_ERROR)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT (LOADER_ERROR_DOWNING, elevator_failed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS_TIMEOUT (LOADER_ERROR_DOWNING, 225,
+ LOADER_ERROR)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT_TIMEOUT (LOADER_ERROR_DOWNING);
+}
+
+/*
+ * post loader_downed event
+ */
+FSM_TRANS (LOADER_ERROR_DOWNING_OPEN,
+ clamp_succeed,
+ LOADER_DOWN)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_downed));
+ return FSM_NEXT (LOADER_ERROR_DOWNING_OPEN, clamp_succeed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS (LOADER_ERROR_DOWNING_OPEN,
+ clamp_failed,
+ LOADER_ERROR)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT (LOADER_ERROR_DOWNING_OPEN, clamp_failed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS_TIMEOUT (LOADER_ERROR_DOWNING_OPEN, 225,
+ LOADER_ERROR)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT_TIMEOUT (LOADER_ERROR_DOWNING_OPEN);
+}
+
+/*
+ * open clamp
+ */
+FSM_TRANS (LOADER_ERROR_UPING,
+ elevator_succeed,
+ LOADER_ERROR_UPING_OPEN)
+{
+ mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ return FSM_NEXT (LOADER_ERROR_UPING, elevator_succeed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS (LOADER_ERROR_UPING,
+ elevator_failed,
+ LOADER_ERROR)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT (LOADER_ERROR_UPING, elevator_failed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS_TIMEOUT (LOADER_ERROR_UPING, 225,
+ LOADER_ERROR)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT_TIMEOUT (LOADER_ERROR_UPING);
+}
+
+/*
+ * post loader_uped event
+ */
+FSM_TRANS (LOADER_ERROR_UPING_OPEN,
+ clamp_succeed,
+ LOADER_UP)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_uped));
+ return FSM_NEXT (LOADER_ERROR_UPING_OPEN, clamp_succeed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS (LOADER_ERROR_UPING_OPEN,
+ clamp_failed,
+ LOADER_ERROR)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT (LOADER_ERROR_UPING_OPEN, clamp_failed);
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS_TIMEOUT (LOADER_ERROR_UPING_OPEN, 225,
+ LOADER_ERROR)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT_TIMEOUT (LOADER_ERROR_UPING_OPEN);
+}
+
+FSM_TRANS (LOADER_LOAD_CLOSING, clamp_succeed,
+ /* move up
+ * count one element */
+ full, LOADER_LOAD_UPING,
+ /* open clamp */
+ empty, LOADER_LOAD_EMPTY_OPEN)
+{
+ /* Measure load using clamp position. */
+ uint16_t tickness = BOT_CLAMP_WIDTH_STEP
+ - mimot_get_motor0_position ()
+ - mimot_get_motor1_position ();
+ if (tickness > BOT_CLAMP_EMPTY_STEP)
+ {
+ asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP,
+ BOT_ELEVATOR_SPEED);
+ position_t robot_position;
+ asserv_get_position (&robot_position);
+ food_taken (robot_position);
+ loader_elements++;
+ return FSM_NEXT (LOADER_LOAD_CLOSING, clamp_succeed, full);
+ }
+ else
+ {
+ mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ return FSM_NEXT (LOADER_LOAD_CLOSING, clamp_succeed, empty);
+ }
+}
+
+/*
+ * open clamp
+ */
+FSM_TRANS (LOADER_LOAD_UPING,
+ elevator_unload_position,
+ LOADER_LOAD_UNLOADING)
+{
+ mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ return FSM_NEXT (LOADER_LOAD_UPING, elevator_unload_position);
+}
+
+/*
+ * post loader_errored or loader_black event
+ * remove one element
+ * open clamp
+ */
+FSM_TRANS (LOADER_LOAD_UPING,
+ elevator_failed,
+ LOADER_ERROR)
+{
+ if (asserv_get_motor0_position () < BOT_ELEVATOR_BLACK_THRESHOLD_STEP)
+ {
+ if (loader_elements)
+ loader_elements--;
+ fsm_queue_post_event (FSM_EVENT (AI, loader_black));
+ }
+ else
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ return FSM_NEXT (LOADER_LOAD_UPING, elevator_failed);
+}
+
+FSM_TRANS (LOADER_LOAD_UNLOADING,
+ elevator_succeed,
+ LOADER_LOAD_UNLOADING_OPEN)
+{
+ return FSM_NEXT (LOADER_LOAD_UNLOADING, elevator_succeed);
+}
+
+/*
+ * post loader_errored event
+ * open clamp
+ */
+FSM_TRANS (LOADER_LOAD_UNLOADING,
+ elevator_failed,
+ LOADER_ERROR)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ return FSM_NEXT (LOADER_LOAD_UNLOADING, elevator_failed);
+}
+
+FSM_TRANS (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed,
+ /* move down */
+ down, LOADER_DOWNING,
+ /* move up */
+ up, LOADER_UPING)
+{
+ if (loader_want_up)
+ {
+ asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP,
+ BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed, up);
+ }
+ else
+ {
+ asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP,
+ BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed, down);
+ }
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS (LOADER_LOAD_UNLOADING_OPEN,
+ clamp_failed,
+ LOADER_ERROR)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT (LOADER_LOAD_UNLOADING_OPEN, clamp_failed);
+}
+
+FSM_TRANS (LOADER_LOAD_EMPTY_OPEN, clamp_succeed,
+ /* post loader_downed event */
+ down, LOADER_DOWN,
+ /* move up */
+ up, LOADER_UPING)
+{
+ if (loader_want_up)
+ {
+ asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP,
+ BOT_ELEVATOR_SPEED);
+ return FSM_NEXT (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, up);
+ }
+ else
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, loader_downed));
+ return FSM_NEXT (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, down);
+ }
+}
+
+/*
+ * post loader_errored event
+ */
+FSM_TRANS (LOADER_LOAD_EMPTY_OPEN,
+ clamp_failed,
+ LOADER_ERROR)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, loader_errored));
+ return FSM_NEXT (LOADER_LOAD_EMPTY_OPEN, clamp_failed);
+}
diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm
deleted file mode 100644
index a188194e..00000000
--- a/digital/io/src/loader.fsm
+++ /dev/null
@@ -1,227 +0,0 @@
-# loader FSM
-loader
- Handle Eurobot 2010 robot clamp, elevator and output door.
-
-States:
- *LOADER_IDLE
- wait start
- LOADER_WAIT_JACK_IN
- wait until the jack is inserted for the first time
- LOADER_WAIT_JACK_OUT
- wait until the jack is removed to initialise robot
- LOADER_INIT_ELEVATOR_ZERO
- move the elevator down to find zero position
- LOADER_INIT_CLAMP_CLOSE
- test clamp closing
- LOADER_INIT_CLAMP_ZERO
- find clamp zero position
- LOADER_INIT_GATE_ZERO
- find gate zero
- LOADER_INIT_ELEVATOR_UP
- move the elevator up so that the robot can go to the wall
- LOADER_INIT_GATE_WAIT[timeout=225]
- wait with gate open
- LOADER_INIT_GATE_CLOSE
- close gate before start
- LOADER_UP
- up position, clamp is protected
- LOADER_DOWN
- down position, ready to take an element
- LOADER_UPING
- moving to the up position
- LOADER_DOWNING
- moving to the down position
- LOADER_ERROR
- error while moving up, stop in an unknown state
- LOADER_ERROR_DOWNING [timeout=225]
- error recovery moving down
- LOADER_ERROR_DOWNING_OPEN [timeout=225]
- error recovery moving down, open clamp
- LOADER_ERROR_UPING [timeout=225]
- error recovery moving up
- LOADER_ERROR_UPING_OPEN [timeout=225]
- error recovery moving up, open clamp
- LOADER_LOAD_CLOSING
- close clamp
- LOADER_LOAD_UPING
- move load up to the unload position
- LOADER_LOAD_UNLOADING
- open clamp to unload, wait until unload position
- LOADER_LOAD_UNLOADING_OPEN
- check clamp is open
- LOADER_LOAD_EMPTY_OPEN
- open clamp due to no elements to pick
-
-Events:
- elevator_succeed
- asserv success result
- elevator_failed
- asserv failure result
- elevator_unload_position
- elevator above unloading position
- clamp_failed
- asserv failure result
- gate_succeed
- asserv success result
- gate_failed
- asserv failure result
- loader_element
- element sensed between clamp to be loaded
- loader_down
- make the loader ready to load an element
- loader_up
- protect the clamp by moving it up
- loader_downed
- posted when loader successfully moved down
- loader_uped
- posted when loader successfully moved up
- loader_errored
- posted when loader in an unknown state due to error while moving up
- loader_black
- loader blocked, due to a black corn
-
-LOADER_IDLE:
- start -> LOADER_WAIT_JACK_IN
-
-LOADER_WAIT_JACK_IN:
- jack_inserted_into_bot -> LOADER_WAIT_JACK_OUT
- hola_start -> LOADER_IDLE
-
-LOADER_WAIT_JACK_OUT:
- jack_removed_from_bot -> LOADER_INIT_ELEVATOR_ZERO
- find elevator zero
- close clamp
- find gate zero
-
-LOADER_INIT_ELEVATOR_ZERO:
- elevator_succeed -> LOADER_INIT_CLAMP_CLOSE
-
-LOADER_INIT_CLAMP_CLOSE:
- clamp_succeed -> LOADER_INIT_CLAMP_ZERO
- move elevator up
- find clamp zero
-
-LOADER_INIT_CLAMP_ZERO:
- clamp_succeed -> LOADER_INIT_GATE_ZERO
-
-LOADER_INIT_GATE_ZERO:
- gate_succeed -> LOADER_INIT_ELEVATOR_UP
-
-LOADER_INIT_ELEVATOR_UP:
- elevator_succeed -> LOADER_INIT_GATE_WAIT
- elevator_failed -> LOADER_IDLE
- initialisation failure
-
-LOADER_INIT_GATE_WAIT:
- state_timeout -> LOADER_INIT_GATE_CLOSE
- close gate
-
-LOADER_INIT_GATE_CLOSE:
- gate_succeed -> LOADER_UP
-
-LOADER_UP:
- loader_down -> LOADER_DOWNING
- move down
-
-LOADER_DOWN:
- loader_up -> LOADER_UPING
- move up
- loader_element -> LOADER_LOAD_CLOSING
- clamp
-
-LOADER_ERROR:
- loader_down -> LOADER_ERROR_DOWNING
- move down
- loader_up -> LOADER_ERROR_UPING
- move up
-
-LOADER_ERROR_DOWNING:
- elevator_succeed -> LOADER_ERROR_DOWNING_OPEN
- release elevator motor
- open clamp
- elevator_failed -> LOADER_ERROR
- post loader_errored event
- state_timeout -> LOADER_ERROR
- post loader_errored event
-
-LOADER_ERROR_DOWNING_OPEN:
- clamp_succeed -> LOADER_DOWN
- post loader_downed event
- clamp_failed -> LOADER_ERROR
- post loader_errored event
- state_timeout -> LOADER_ERROR
- post loader_errored event
-
-LOADER_ERROR_UPING:
- elevator_succeed -> LOADER_ERROR_UPING_OPEN
- open clamp
- elevator_failed -> LOADER_ERROR
- post loader_errored event
- state_timeout -> LOADER_ERROR
- post loader_errored event
-
-LOADER_ERROR_UPING_OPEN:
- clamp_succeed -> LOADER_UP
- post loader_uped event
- clamp_failed -> LOADER_ERROR
- post loader_errored event
- state_timeout -> LOADER_ERROR
- post loader_errored event
-
-LOADER_DOWNING:
- elevator_succeed -> LOADER_DOWN
- release elevator motor
- post loader_downed event
- elevator_failed -> LOADER_ERROR
- post loader_errored event
- loader_element -> LOADER_ERROR
- emergency stop
- post loader_errored event
- loader_up -> LOADER_UPING
- move up
-
-LOADER_UPING:
- elevator_succeed -> LOADER_UP
- post loader_uped event
- elevator_failed -> LOADER_ERROR
- post loader_errored event
- loader_down -> LOADER_DOWNING
- move down
-
-LOADER_LOAD_CLOSING:
- clamp_succeed: full -> LOADER_LOAD_UPING
- move up
- count one element
- clamp_succeed: empty -> LOADER_LOAD_EMPTY_OPEN
- open clamp
-
-LOADER_LOAD_UPING:
- elevator_unload_position -> LOADER_LOAD_UNLOADING
- open clamp
- elevator_failed -> LOADER_ERROR
- post loader_errored or loader_black event
- remove one element
- open clamp
-
-LOADER_LOAD_UNLOADING:
- elevator_succeed -> LOADER_LOAD_UNLOADING_OPEN
- elevator_failed -> LOADER_ERROR
- post loader_errored event
- open clamp
-
-LOADER_LOAD_UNLOADING_OPEN:
- clamp_succeed: down -> LOADER_DOWNING
- move down
- clamp_succeed: up -> LOADER_UPING
- move up
- clamp_failed -> LOADER_ERROR
- post loader_errored event
-
-LOADER_LOAD_EMPTY_OPEN:
- clamp_succeed: down -> LOADER_DOWN
- post loader_downed event
- clamp_succeed: up -> LOADER_UPING
- move up
- clamp_failed -> LOADER_ERROR
- post loader_errored event
-
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index 35ee83c1..22964ddd 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -35,23 +35,27 @@
/* AVR include, non HOST */
#ifndef HOST
# include "switch.h" /* Manage switches (jack, color selector) */
+#else
+#include <string.h>
#endif /* HOST */
-#include "main_timer.h"
+#include "timer.h"
#include "simu.host.h"
#include "asserv.h"
#include "mimot.h"
#include "twi_master.h"
#include "eeprom.h" /* Parameters loaded/stored in the EEPROM */
-#include "fsm.h" /* fsm_* */
+#define FSM_NAME AI
+#include "fsm.h"
+#include "fsm_queue.h"
#include "bot.h"
#include "servo_pos.h"
#include "usdist.h"
#include "radar.h"
#include "chrono.h" /* chrono_end_match */
#include "pwm.h"
-#include "playground.h"
+#include "playground_2010.h"
#include "contact.h"
#include "init.h"
#include "move.h"
@@ -73,19 +77,7 @@ static void main_loop (void);
/**
* Our color.
*/
-enum team_color_e bot_color;
-
-/** Maximum number of events in post queue. */
-#define MAIN_POST_EVENT_QUEUE_SIZE 8
-
-/** Events to post to the FSM in next iteration. */
-uint8_t main_post_event_queue[MAIN_POST_EVENT_QUEUE_SIZE];
-
-/** Number of events in the post queue. */
-uint8_t main_post_event_queue_nb;
-
-/** First event in the post queue. */
-uint8_t main_post_event_queue_head;
+enum team_color_e team_color;
/** Obstacles positions, updated using radar module. */
vect_t main_obstacles_pos[2];
@@ -111,29 +103,6 @@ static uint8_t main_stats_asserv_, main_stats_asserv_cpt_;
*/
static uint8_t main_stats_timer_;
-void
-main_post_event (uint8_t event)
-{
- assert (main_post_event_queue_nb < MAIN_POST_EVENT_QUEUE_SIZE);
- uint8_t tail = (main_post_event_queue_head + main_post_event_queue_nb)
- % MAIN_POST_EVENT_QUEUE_SIZE;
- main_post_event_queue[tail] = event;
- main_post_event_queue_nb++;
-}
-
-/** Pop one event from the event queue. */
-static uint8_t
-main_pop_event (void)
-{
- uint8_t e;
- assert (main_post_event_queue_nb > 0);
- e = main_post_event_queue[main_post_event_queue_head];
- main_post_event_queue_nb--;
- main_post_event_queue_head = (main_post_event_queue_head + 1)
- % MAIN_POST_EVENT_QUEUE_SIZE;
- return e;
-}
-
/**
* Main events management.
* This function is responsible to get all events and send them to the
@@ -144,16 +113,18 @@ main_event_to_fsm (void)
{
/* If an event is handled, stop generating any other event, because a
* transition may have invalidated the current robot state. */
-#define FSM_HANDLE_EVENT(fsm, event) \
- do { if (fsm_handle_event ((fsm), (event))) return; } while (0)
-#define FSM_HANDLE_TIMEOUT(fsm) \
- do { if (fsm_handle_timeout (fsm)) return; } while (0)
+#define FSM_HANDLE_E(fsm, event) \
+ do { if (FSM_HANDLE (fsm, event)) return; } while (0)
+#define FSM_HANDLE_VAR_E(fsm, event) \
+ do { if (FSM_HANDLE_VAR (fsm, event)) return; } while (0)
+#define FSM_HANDLE_TIMEOUT_E(fsm) \
+ do { if (FSM_HANDLE_TIMEOUT (fsm)) return; } while (0)
/* Update FSM timeouts. */
- FSM_HANDLE_TIMEOUT (&ai_fsm);
+ FSM_HANDLE_TIMEOUT_E (AI);
/* If we have entering this function, last command of the asserv board has
* been aquited. */
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_asserv_last_cmd_ack);
+ FSM_HANDLE_E (AI, asserv_last_cmd_ack);
asserv_status_e
move_status = none,
@@ -171,65 +142,65 @@ main_event_to_fsm (void)
/* Check commands move status. */
if (move_status == success)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_bot_move_succeed);
+ FSM_HANDLE_E (AI, bot_move_succeed);
else if (move_status == failure)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_bot_move_failed);
+ FSM_HANDLE_E (AI, bot_move_failed);
if (motor0_status == success)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_elevator_succeed);
+ FSM_HANDLE_E (AI, elevator_succeed);
else if (motor0_status == failure)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_elevator_failed);
+ FSM_HANDLE_E (AI, elevator_failed);
if (motor1_status == success)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_gate_succeed);
+ FSM_HANDLE_E (AI, gate_succeed);
else if (motor1_status == failure)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_gate_failed);
+ FSM_HANDLE_E (AI, gate_failed);
if (motorm0_status == success && motorm1_status == success)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_clamp_succeed);
+ FSM_HANDLE_E (AI, clamp_succeed);
else if (motorm0_status == failure || motorm1_status == failure)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_clamp_failed);
+ FSM_HANDLE_E (AI, clamp_failed);
/* Check positions. */
if (asserv_get_motor0_position () > BOT_ELEVATOR_UNLOAD_STEP)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_elevator_unload_position);
+ FSM_HANDLE_E (AI, elevator_unload_position);
/* Contacts. */
if (asserv_get_motor0_position () < BOT_ELEVATOR_REST_STEP
&& (!IO_GET (CONTACT_BUMPER0) || !IO_GET (CONTACT_BUMPER1)))
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_loader_element);
+ FSM_HANDLE_E (AI, loader_element);
if (!IO_GET (CONTACT_STRATEGY))
{
if (switch_get_color ())
loader_down ();
else
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_hola_start);
+ FSM_HANDLE_E (AI, hola_start);
}
/* Jack */
if (switch_get_jack ())
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_jack_removed_from_bot);
+ FSM_HANDLE_E (AI, jack_removed_from_bot);
else
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_jack_inserted_into_bot);
+ FSM_HANDLE_E (AI, jack_inserted_into_bot);
if (init_match_is_started)
{
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_init_match_is_started);
+ FSM_HANDLE_E (AI, init_match_is_started);
/* This must be done in the last part of this block. */
init_match_is_started = 0;
}
/* Event generated at the end of the sub FSM to post to the top FSM */
- if (main_post_event_queue_nb)
+ if (fsm_queue_poll ())
{
/* We must post the event at the end of this block because it
* will issue a continue and every instruction after will
* never be executed. */
/* We need to save the event before reseting it */
- uint8_t save_event = main_pop_event ();
+ uint8_t save_event = fsm_queue_pop_event ();
/* Post the event */
- FSM_HANDLE_EVENT (&ai_fsm, save_event);
+ FSM_HANDLE_VAR_E (AI, save_event);
}
/* Check obstacles. */
@@ -240,7 +211,7 @@ main_event_to_fsm (void)
position_t robot_pos;
asserv_get_position (&robot_pos);
if (robot_pos.v.y < PG_FIELD_Y_MAX)
- FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_in_field);
+ FSM_HANDLE_E (AI, in_field);
/* TODO: Check other sensors */
}
@@ -256,7 +227,7 @@ main_init (void)
/* Enable interrupts */
sei ();
/* Main timer */
- main_timer_init ();
+ timer_init ();
/* Load parameters */
eeprom_load_param ();
/* Dirty fix */
@@ -276,10 +247,8 @@ main_init (void)
usdist_init ();
/* Top. */
top_init ();
- /* Init FSM. */
- fsm_init (&ai_fsm);
/* Start FSM. */
- fsm_handle_event (&ai_fsm, AI_EVENT_start);
+ FSM_HANDLE (AI, start);
/* PWM module */
pwm_init ();
/* Servo pos init. */
@@ -302,15 +271,15 @@ main_loop (void)
{
/* Wait for an overflow of the main timer (4.444ms) */
{
- uint8_t timer_count = main_timer_wait ();
+ uint8_t timer_count = timer_wait ();
if (main_stats_timer_)
proto_send1b('M', timer_count);
- if (timer_count == 1)
- {
- /* Main timer has reached overflow earlier!
- We are late and this is really bad. */
- TRACE (TRACE_MAIN_TIMER__LATE);
- }
+ if (timer_count == 1)
+ {
+ /* Main timer has reached overflow earlier!
+ We are late and this is really bad. */
+ TRACE (TRACE_TIMER__LATE);
+ }
}
/* Update chrono. */
@@ -706,6 +675,18 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
int
main (int argc, char **argv)
{
+ /* produce AVR's FSM headers. */
+#ifdef HOST
+ int i;
+ if (argc > 1)
+ for (i = 1; i < argc; i++)
+ if (strcmp (argv[i], "--gen") == 0)
+ {
+ FSM_GENERATE (AVR, 0);
+ return 0;
+ }
+#endif
+
avr_init (argc, argv);
/* Initialize the main and its subsystems */
diff --git a/digital/io/src/main.h b/digital/io/src/main.h
index 9096b532..2185b90c 100644
--- a/digital/io/src/main.h
+++ b/digital/io/src/main.h
@@ -29,8 +29,4 @@ extern vect_t main_obstacles_pos[2];
extern uint8_t main_obstacles_nb;
-/** Post an event to be processed in the next main loop. */
-void
-main_post_event (uint8_t event);
-
#endif /* main_h */
diff --git a/digital/io/src/main_timer.avr.c b/digital/io/src/main_timer.avr.c
deleted file mode 100644
index bcb185b6..00000000
--- a/digital/io/src/main_timer.avr.c
+++ /dev/null
@@ -1,60 +0,0 @@
-/* main_timer.avr.c */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
- *
- * Copyright (C) 2009 Dufour Jérémy
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}} */
-
-#include "common.h"
-
-#include "main_timer.h"
-
-#include "modules/utils/utils.h"
-#include "io.h"
-
-void
-main_timer_init (void)
-{
- /* Configuration of the timer/counter 0:
- * - top = 0xff,
- * - prescaler = 256,
- * -> Fov = F_io / (prescaler * (TOP + 1))
- * -> Tov = 1 / Fov = 4.444 ms.
- * Note: if you change the TCCR0 register value, please also update
- * MT_TC0_PRESCALER and MT_TC0_TOP. */
- TCCR0 = regv (FOC0, WGM00, COM01, COM0, WGM01, CS02, CS01, CS00,
- 0, 0, 0, 0, 0, 1, 1, 0);
-}
-
-uint8_t
-main_timer_wait (void)
-{
- /* Let's pretend we have reached overflow before calling this function. */
- uint8_t count_before_ov = 1;
- /* Loop until an overflow of the timer occurs. */
- while (!(TIFR & _BV (TOV0)))
- /* We have not reached overflow. */
- count_before_ov = 0;
- /* Write 1 to clear overflow. */
- TIFR = _BV (TOV0);
-
- return count_before_ov;
-}
diff --git a/digital/io/src/main_timer.h b/digital/io/src/main_timer.h
deleted file mode 100644
index b04c1775..00000000
--- a/digital/io/src/main_timer.h
+++ /dev/null
@@ -1,78 +0,0 @@
-#ifndef main_timer_h
-#define main_timer_h
-/* main_timer.h */
-/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
- *
- * Copyright (C) 2009 Dufour Jérémy
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}} */
-
-/**
- * @file Main timer.
- * The main timer is responsible for making the main loop executed regularly
- * to a specific time (at least).
- *
- * Sometimes, the main loop can be executed less frequently that we want
- * because it takes too much time! This is bad and should be avoided.
- *
- * The main timer used the timer/counter 0 of the AVR.
- */
-
-/**
- * Prescaler configured for timer/counter 0.
- * If you want to change this value, you also need to change the TCCR0
- * register.
- */
-#define MT_TC0_PRESCALER 256L
-/**
- * Top configured for timer/counter 0.
- * If you want to change this value, you also need to change the TCCR0
- * register.
- */
-#define MT_TC0_TOP 255
-/**
- * Period of timer/counter 0 (in millisecond).
- */
-#define MT_TC0_PERIOD \
- (1000.0 / (AC_FREQ / (MT_TC0_PRESCALER * (MT_TC0_TOP + 1))))
-
-/**
- * Initialize the main timer to 4.444 ms.
- * This function setup the timer/counter 0 configuration register.
- */
-void
-main_timer_init (void);
-
-/**
- * Wait until the main timer overflows.
- * @return
- * - 0 if we are on time (we have not reached overflow before calling this
- * function).
- * - 1 if we have already reached overflow.
- * @warning if this function return 1, it means we are late and the main loop
- * is lasting more than the time configured. Consequence, some important
- * functions (like the chronometer for match duration) will not work
- * correctly!
- */
-uint8_t
-main_timer_wait (void);
-
-#endif /* main_timer_h */
diff --git a/digital/io/src/mimot.c b/digital/io/src/mimot.c
deleted file mode 100644
index ba09a3f5..00000000
--- a/digital/io/src/mimot.c
+++ /dev/null
@@ -1,183 +0,0 @@
-/* mimot.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 "mimot.h"
-
-#include "twi_master.h"
-
-#include "modules/utils/byte.h"
-#include "io.h"
-
-/** Flag bit positions in mimot status byte. */
-enum mimot_status_flag_t
-{
- /** Motor0 movement finished with success. */
- mimot_status_flag_motor0_succeed = 0,
- /** Motor0 movement finished with failure. */
- mimot_status_flag_motor0_failed = 1,
- /** Motor1 movement finished with success. */
- mimot_status_flag_motor1_succeed = 2,
- /** Motor1 movement finished with failure. */
- mimot_status_flag_motor1_failed = 3,
-};
-
-/** Status structure. */
-struct mimot_status_t
-{
- /** Status flags. */
- uint8_t status;
- /** Mimot input port. */
- uint8_t input_port;
- /** Motor0 position. */
- uint16_t motor0_position;
- /** Motor1 position. */
- uint16_t motor1_position;
-};
-
-/** Current mimot status. */
-struct mimot_status_t mimot_status;
-
-void
-mimot_init (void)
-{
- /* Nothing to do. */
-}
-
-void
-mimot_status_cb (uint8_t *status)
-{
- /* Parse received data and store them. */
- mimot_status.status = status[0];
- mimot_status.input_port = status[1];
- mimot_status.motor0_position = v8_to_v16 (status[3], status[4]);
- mimot_status.motor1_position = v8_to_v16 (status[5], status[6]);
-}
-
-asserv_status_e
-mimot_motor0_cmd_status (void)
-{
- if (mimot_status.status & _BV (mimot_status_flag_motor0_succeed))
- return success;
- else if (mimot_status.status & _BV (mimot_status_flag_motor0_failed))
- return failure;
- else
- return none;
-}
-
-asserv_status_e
-mimot_motor1_cmd_status (void)
-{
- if (mimot_status.status & _BV (mimot_status_flag_motor1_succeed))
- return success;
- else if (mimot_status.status & _BV (mimot_status_flag_motor1_failed))
- return failure;
- else
- return none;
-}
-
-uint16_t
-mimot_get_motor0_position (void)
-{
- return mimot_status.motor0_position;
-}
-
-uint16_t
-mimot_get_motor1_position (void)
-{
- return mimot_status.motor1_position;
-}
-
-void
-mimot_reset (void)
-{
- uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
- buffer[0] = 'z';
- twi_master_send_buffer (1);
-}
-
-void
-mimot_move_motor0_absolute (uint16_t position, uint8_t speed)
-{
- uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
- buffer[0] = 'b';
- buffer[1] = v16_to_v8 (position, 1);
- buffer[2] = v16_to_v8 (position, 0);
- buffer[3] = speed;
- twi_master_send_buffer (4);
-}
-
-void
-mimot_move_motor1_absolute (uint16_t position, uint8_t speed)
-{
- uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
- buffer[0] = 'c';
- buffer[1] = v16_to_v8 (position, 1);
- buffer[2] = v16_to_v8 (position, 0);
- buffer[3] = speed;
- twi_master_send_buffer (4);
-}
-
-void
-mimot_motor0_zero_position (int8_t speed)
-{
- uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
- buffer[0] = 'B';
- buffer[1] = speed;
- twi_master_send_buffer (2);
-}
-
-void
-mimot_motor1_zero_position (int8_t speed)
-{
- uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
- buffer[0] = 'C';
- buffer[1] = speed;
- twi_master_send_buffer (2);
-}
-
-void
-mimot_motor0_clamp (int8_t speed, int16_t pwm)
-{
- uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
- buffer[0] = 'l';
- buffer[1] = 0;
- buffer[2] = speed;
- buffer[3] = v16_to_v8 (pwm, 1);
- buffer[4] = v16_to_v8 (pwm, 0);
- twi_master_send_buffer (5);
-}
-
-void
-mimot_motor1_clamp (int8_t speed, int16_t pwm)
-{
- uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
- buffer[0] = 'l';
- buffer[1] = 1;
- buffer[2] = speed;
- buffer[3] = v16_to_v8 (pwm, 1);
- buffer[4] = v16_to_v8 (pwm, 0);
- twi_master_send_buffer (5);
-}
-
diff --git a/digital/io/src/mimot.h b/digital/io/src/mimot.h
deleted file mode 100644
index 98709162..00000000
--- a/digital/io/src/mimot.h
+++ /dev/null
@@ -1,95 +0,0 @@
-#ifndef mimot_h
-#define mimot_h
-/* mimot.h */
-/* 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 "asserv.h"
-
-/**
- * Provide interface to mimot board using the TWI protocol.
- */
-
-/** Slave number in the twi_master list. */
-#define MIMOT_SLAVE 1
-
-/** Mimot TWI address. */
-#define MIMOT_TWI_ADDRESS 6
-
-/** Length of status buffer (not including CRC). */
-#define MIMOT_STATUS_LENGTH 7
-
-/** Initialise module. */
-void
-mimot_init (void);
-
-/** Called when a new status buffer is received, update the mimot
- * information. */
-void
-mimot_status_cb (uint8_t *status);
-
-/** Return motor0 last command status. */
-asserv_status_e
-mimot_motor0_cmd_status (void);
-
-/** Return motor1 last command status. */
-asserv_status_e
-mimot_motor1_cmd_status (void);
-
-/** Get motor0 position in steps. */
-uint16_t
-mimot_get_motor0_position (void);
-
-/** Get motor1 position in steps. */
-uint16_t
-mimot_get_motor1_position (void);
-
-/** Reset mimot board. */
-void
-mimot_reset (void);
-
-/** Move motor0 to absolute position in steps. */
-void
-mimot_move_motor0_absolute (uint16_t position, uint8_t speed);
-
-/** Move motor1 to absolute position in steps. */
-void
-mimot_move_motor1_absolute (uint16_t position, uint8_t speed);
-
-/** Reset motor0 to zero position. */
-void
-mimot_motor0_zero_position (int8_t speed);
-
-/** Reset motor1 to zero position. */
-void
-mimot_motor1_zero_position (int8_t speed);
-
-/** Clamp motor0. */
-void
-mimot_motor0_clamp (int8_t speed, int16_t pwm);
-
-/** Clamp motor1. */
-void
-mimot_motor1_clamp (int8_t speed, int16_t pwm);
-
-#endif /* mimot_h */
diff --git a/digital/io/src/move.c b/digital/io/src/move.c
index e4f59c39..60db0272 100644
--- a/digital/io/src/move.c
+++ b/digital/io/src/move.c
@@ -22,6 +22,9 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+
+#define FSM_NAME AI
+
#include "common.h"
#include "move.h"
#include "fsm.h"
@@ -29,9 +32,25 @@
#include "radar.h"
#include "asserv.h"
#include "main.h"
-
+#include "events.h"
+#include "playground.h"
+#include "loader.h"
#include "modules/path/path.h"
#include "modules/utils/utils.h"
+#include "fsm_queue.h"
+#include "modules/math/fixed/fixed.h" /* fixed_* */
+#include "modules/trace/trace.h"
+#include "debug.host.h"
+#include <math.h>
+
+#define MOVE_LOADER_UNBLOCKING_DISTANCE 70
+
+/** Used to define a grid. If robot is outside the grid, be careful. */
+#define MOVE_GRID_X 450
+#define MOVE_GRID_X_ORIGIN 150
+#define MOVE_GRID_Y 250
+#define MOVE_GRID_Y_ORIGIN 128
+#define MOVE_GRID_MARGIN 50
/**
* Internal data used by the move FSM.
@@ -50,7 +69,7 @@ move_start (position_t position, uint8_t backward)
/* Reset try counter. */
move_data.try_again_counter = 3;
/* Start the FSM. */
- fsm_handle_event (&ai_fsm, AI_EVENT_move_start);
+ FSM_HANDLE (AI, move_start);
}
void
@@ -65,7 +84,7 @@ move_start_noangle (vect_t position, uint8_t backward, int16_t shorten)
/* Reset try counter. */
move_data.try_again_counter = 3;
/* Start the FSM. */
- fsm_handle_event (&ai_fsm, AI_EVENT_move_start);
+ FSM_HANDLE (AI, move_start);
}
void
@@ -80,15 +99,591 @@ move_obstacles_update (void)
uint8_t
move_check_obstacles (void)
{
- if (fsm_can_handle_event (&ai_fsm, AI_EVENT_obstacle_in_front))
+ if (FSM_CAN_HANDLE (AI, obstacle_in_front))
{
position_t robot_pos;
asserv_get_position (&robot_pos);
if (radar_blocking (&robot_pos.v, &move_data.step, main_obstacles_pos,
main_obstacles_nb))
- if (fsm_handle_event (&ai_fsm, AI_EVENT_obstacle_in_front))
+ if (FSM_HANDLE (AI, obstacle_in_front))
return 1;
}
return 0;
}
+/* Define FSM */
+FSM_STATES (
+ /* waiting for the beginning of the move FSM. */
+ MOVE_IDLE,
+ /* rotating towards next point. */
+ MOVE_ROTATING,
+ /* moving to a position (intermediate or final). */
+ MOVE_MOVING,
+ /* moving backward to go away from what is blocking the bot. */
+ MOVE_MOVING_BACKWARD_TO_TURN_FREELY,
+ /* waiting for obstacle to disappear. */
+ MOVE_WAIT_FOR_CLEAR_PATH,
+ /* moving loader up and move backward to unblock loader. */
+ MOVE_LOADER_UNBLOCKING_UPING,
+ /* moving loader down. */
+ MOVE_LOADER_UNBLOCKING_DOWNING)
+
+FSM_EVENTS (
+ /* initialize the FSM and start the movement directly. */
+ move_start,
+ /* the bot has seen something (front is the same when going backward). */
+ obstacle_in_front)
+
+FSM_START_WITH (MOVE_IDLE)
+
+/** Go to current step, low level function. */
+static void
+move_go (void)
+{
+ vect_t dst = move_data.step;
+ /* Modify final point if requested. */
+ if (move_data.final_move && move_data.shorten)
+ {
+ /* Compute a vector from destination to robot with lenght
+ * 'shorten'. */
+ position_t robot_position;
+ asserv_get_position (&robot_position);
+ vect_t v = robot_position.v;
+ vect_sub (&v, &move_data.step);
+ int16_t d = vect_norm (&v);
+ if (d > move_data.shorten)
+ {
+ vect_scale_f824 (&v, 0x1000000 / d * move_data.shorten);
+ vect_translate (&dst, &v);
+ }
+ }
+ if (move_data.step_with_angle)
+ asserv_goto_xya (dst.x, dst.y, move_data.step_angle,
+ move_data.step_backward);
+ else
+ asserv_goto (dst.x, dst.y, move_data.step_backward);
+}
+
+/** Go or rotate toward position, returns 1 for linear move, 2 for angular
+ * move. */
+static uint8_t
+move_go_or_rotate (vect_t dst, uint16_t angle, uint8_t with_angle,
+ uint8_t backward)
+{
+ position_t robot_position;
+ /* Remember step. */
+ move_data.step = dst;
+ move_data.step_angle = angle;
+ move_data.step_with_angle = with_angle;
+ move_data.step_backward = backward;
+ /* Compute angle to destination. */
+ asserv_get_position (&robot_position);
+ vect_t v = dst; vect_sub (&v, &robot_position.v);
+ uint16_t dst_angle = atan2 (v.y, v.x) * ((1l << 16) / (2 * M_PI));
+ if (backward & ASSERV_BACKWARD)
+ dst_angle += 0x8000;
+ if ((backward & ASSERV_REVERT_OK)
+ && (dst_angle ^ robot_position.a) & 0x8000)
+ dst_angle += 0x8000;
+ int16_t diff = dst_angle - robot_position.a;
+ /* Move or rotate. */
+ if (UTILS_ABS (diff) < 0x1000)
+ {
+ loader_down ();
+ move_go ();
+ return 1;
+ }
+ else
+ {
+ loader_up ();
+ asserv_goto_angle (dst_angle);
+ return 2;
+ }
+}
+
+/** Go to next position computed by path module, to be called by
+ * move_path_init and move_path_next. Returns 1 for linear move, 2 for angular
+ * move. */
+static uint8_t
+move_go_to_next (vect_t dst)
+{
+ uint8_t r;
+ /* If it is not the last position. */
+ if (dst.x != move_data.final.v.x || dst.y != move_data.final.v.y)
+ {
+ /* Not final position. */
+ move_data.final_move = 0;
+ /* Goto without angle. */
+ r = move_go_or_rotate (dst, 0, 0, move_data.backward_movement_allowed
+ | (move_data.slow ? ASSERV_REVERT_OK : 0));
+ }
+ else
+ {
+ /* Final position. */
+ move_data.final_move = 1;
+ /* Goto with angle if requested. */
+ r = move_go_or_rotate (dst, move_data.final.a, move_data.with_angle,
+ move_data.backward_movement_allowed);
+ }
+ TRACE (TRACE_MOVE__GO_TO, dst.x, dst.y);
+ /* Next time, do not use slow. */
+ move_data.slow = 0;
+ return r;
+}
+
+/** Update and go to first position, return non zero if a path is found, 1 for
+ * linear move, 2 for angular move. */
+static uint8_t
+move_path_init (void)
+{
+ uint8_t found;
+ vect_t dst;
+ /* Get the current position */
+ position_t current_pos;
+ asserv_get_position (&current_pos);
+ /* Give the current position of the bot to the path module */
+ path_endpoints (current_pos.v, move_data.final.v);
+ /* Update the path module */
+ move_data.slow = 0;
+ path_update ();
+ found = path_get_next (&dst);
+ /* If not found, try to escape. */
+ if (!found)
+ {
+ move_data.slow = 1;
+ path_escape (8);
+ path_update ();
+ found = path_get_next (&dst);
+ }
+ /* If not on the grid, slow down. */
+ int16_t mx = current_pos.v.x % MOVE_GRID_X;
+ int16_t my = current_pos.v.y % MOVE_GRID_Y;
+ if (mx < MOVE_GRID_X_ORIGIN - MOVE_GRID_MARGIN
+ || mx > MOVE_GRID_X_ORIGIN + MOVE_GRID_MARGIN
+ || my < MOVE_GRID_Y_ORIGIN - MOVE_GRID_MARGIN
+ || my > MOVE_GRID_Y_ORIGIN + MOVE_GRID_MARGIN
+ )
+ move_data.slow = 1;
+ /* If found, go. */
+ if (found)
+ {
+ return move_go_to_next (dst);
+ }
+ else
+ {
+ /* Error, not final move. */
+ move_data.final_move = 0;
+ return 0;
+ }
+}
+
+/** Go to next position in path. Returns 1 for linear move, 2 for angular
+ * move. */
+static uint8_t
+move_path_next (void)
+{
+ vect_t dst;
+ path_get_next (&dst);
+ return move_go_to_next (dst);
+}
+
+FSM_TRANS (MOVE_IDLE, move_start,
+ /* rotate towards next position. */
+ path_found_rotate, MOVE_ROTATING,
+ /* move to next position. */
+ path_found, MOVE_MOVING,
+ /* post failure event. */
+ no_path_found, MOVE_IDLE)
+{
+ uint8_t next = move_path_init ();
+ if (next)
+ {
+ if (next == 2)
+ return FSM_NEXT (MOVE_IDLE, move_start, path_found_rotate);
+ else
+ return FSM_NEXT (MOVE_IDLE, move_start, path_found);
+ }
+ else
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_fsm_failed));
+ return FSM_NEXT (MOVE_IDLE, move_start, no_path_found);
+ }
+}
+
+/*
+ * move to next position.
+ */
+FSM_TRANS (MOVE_ROTATING,
+ bot_move_succeed,
+ MOVE_MOVING)
+{
+ loader_down ();
+ move_go ();
+ return FSM_NEXT (MOVE_ROTATING, bot_move_succeed);
+}
+
+/*
+ * move to next position.
+ */
+FSM_TRANS (MOVE_ROTATING,
+ bot_move_failed,
+ MOVE_MOVING)
+{
+ loader_down ();
+ move_go ();
+ return FSM_NEXT (MOVE_ROTATING, bot_move_failed);
+}
+
+/*
+ * move to next position.
+ */
+FSM_TRANS_TIMEOUT (MOVE_ROTATING, 1125,
+ MOVE_MOVING)
+{
+ loader_down ();
+ move_go ();
+ return FSM_NEXT_TIMEOUT (MOVE_ROTATING);
+}
+
+/*
+ * move backward
+ * loader up
+ */
+FSM_TRANS (MOVE_ROTATING,
+ loader_errored,
+ MOVE_LOADER_UNBLOCKING_UPING)
+{
+ asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
+ loader_up ();
+ return FSM_NEXT (MOVE_ROTATING, loader_errored);
+}
+
+FSM_TRANS (MOVE_MOVING, bot_move_succeed,
+ /* post success event. */
+ done, MOVE_IDLE,
+ /* rotate towards next position. */
+ path_found_rotate, MOVE_ROTATING,
+ /* move to next position. */
+ path_found, MOVE_MOVING,
+ /* post failure event */
+ no_path_found, MOVE_IDLE)
+{
+ if (move_data.final_move)
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_fsm_succeed));
+ return FSM_NEXT (MOVE_MOVING, bot_move_succeed, done);
+ }
+ else
+ {
+ uint8_t next = move_path_next ();
+ if (next == 2)
+ return FSM_NEXT (MOVE_MOVING, bot_move_succeed, path_found_rotate);
+ else
+ return FSM_NEXT (MOVE_MOVING, bot_move_succeed, path_found);
+ }
+ //return FSM_NEXT (MOVE_MOVING, bot_move_succeed, no_path_found);
+}
+
+
+/*
+ * reset final_move.
+ * move backward to turn freely.
+ */
+void
+move_MOVE_MOVING_bot_move_failed_MOVE_MOVING_BACKWARD_TO_TURN_FREELY ()
+{
+ move_data.final_move = 0;
+ /* Assume there is an obstacle in front of the robot. */
+ position_t robot_pos;
+ asserv_get_position (&robot_pos);
+ vect_t obstacle_pos;
+ int16_t dist = asserv_get_last_moving_direction () == 1
+ ? BOT_SIZE_FRONT + MOVE_REAL_OBSTACLE_RADIUS
+ : -(BOT_SIZE_BACK + MOVE_REAL_OBSTACLE_RADIUS);
+ vect_from_polar_uf016 (&obstacle_pos, dist, robot_pos.a);
+ vect_translate (&obstacle_pos, &robot_pos.v);
+ path_obstacle (0, obstacle_pos, MOVE_OBSTACLE_RADIUS, 0,
+ MOVE_OBSTACLE_VALIDITY);
+ /* Move backward to turn freely. */
+ asserv_move_linearly (asserv_get_last_moving_direction () == 1 ?
+ - 300 : 300);
+}
+
+FSM_TRANS (MOVE_MOVING,
+ bot_move_failed,
+ MOVE_MOVING_BACKWARD_TO_TURN_FREELY)
+{
+ move_MOVE_MOVING_bot_move_failed_MOVE_MOVING_BACKWARD_TO_TURN_FREELY ();
+ return FSM_NEXT (MOVE_MOVING, bot_move_failed);
+}
+
+/*
+ * reset final_move.
+ * move backward to turn freely.
+ */
+FSM_TRANS_TIMEOUT (MOVE_MOVING, 2250,
+ MOVE_MOVING_BACKWARD_TO_TURN_FREELY)
+{
+ move_MOVE_MOVING_bot_move_failed_MOVE_MOVING_BACKWARD_TO_TURN_FREELY ();
+ return FSM_NEXT_TIMEOUT (MOVE_MOVING);
+}
+
+FSM_TRANS (MOVE_MOVING, obstacle_in_front,
+ /* reset final_move.
+ * stop the bot. */
+ tryagain, MOVE_WAIT_FOR_CLEAR_PATH,
+ /* stop the bot.
+ * post failure event. */
+ tryout, MOVE_IDLE)
+{
+ move_data.final_move = 0;
+ asserv_stop_motor ();
+ if (--move_data.try_again_counter == 0)
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_fsm_failed));
+ return FSM_NEXT (MOVE_MOVING, obstacle_in_front, tryout);
+ }
+ else
+ return FSM_NEXT (MOVE_MOVING, obstacle_in_front, tryagain);
+}
+
+/*
+ * move backward
+ * loader up
+ */
+FSM_TRANS (MOVE_MOVING,
+ loader_errored,
+ MOVE_LOADER_UNBLOCKING_UPING)
+{
+ asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
+ loader_up ();
+ return FSM_NEXT (MOVE_MOVING, loader_errored);
+}
+
+FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed,
+ /* post failure event. */
+ tryout, MOVE_IDLE,
+ /* rotate towards next position. */
+ path_found_rotate, MOVE_ROTATING,
+ /* move to next position. */
+ path_found, MOVE_MOVING,
+ /* post failure event. */
+ no_path_found, MOVE_IDLE)
+{
+ if (--move_data.try_again_counter == 0)
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_fsm_failed));
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, tryout);
+ }
+ else
+ {
+ uint8_t next = move_path_init ();
+ if (next)
+ {
+ if (next == 2)
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, path_found_rotate);
+ else
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, path_found);
+ }
+ else
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_fsm_failed));
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, no_path_found);
+ }
+ }
+}
+
+FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed,
+ /* post failure event. */
+ tryout, MOVE_IDLE,
+ /* rotate towards next position. */
+ path_found_rotate, MOVE_ROTATING,
+ /* move to next position. */
+ path_found, MOVE_MOVING,
+ /* nothing to do. */
+ no_path_found_tryagain, MOVE_WAIT_FOR_CLEAR_PATH,
+ /* post failure event. */
+ no_path_found_tryout, MOVE_IDLE)
+{
+ if (--move_data.try_again_counter == 0)
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_fsm_failed));
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, tryout);
+ }
+ else
+ {
+ uint8_t next = move_path_init ();
+ if (next)
+ {
+ if (next == 2)
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, path_found_rotate);
+ else
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, path_found);
+ }
+ else
+ {
+ if (--move_data.try_again_counter == 0)
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_fsm_failed));
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_path_found_tryout);
+ }
+ else
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_path_found_tryagain);
+ }
+ }
+}
+
+FSM_TRANS_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, 255,
+ /* rotate towards next position. */
+ path_found_rotate, MOVE_ROTATING,
+ /* move to next position. */
+ path_found, MOVE_MOVING,
+ /* decrement counter. */
+ no_path_found_tryagain, MOVE_WAIT_FOR_CLEAR_PATH,
+ /* post failure. */
+ no_path_found_tryout, MOVE_IDLE)
+{
+ /* Try to move. */
+ uint8_t next = move_path_init ();
+ if (next)
+ {
+ if (next == 2)
+ return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, path_found_rotate);
+ else
+ return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, path_found);
+ }
+ else
+ {
+ /* Error, no new position, should we try again? */
+ if (--move_data.try_again_counter == 0)
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_fsm_failed));
+ return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, no_path_found_tryout);
+ }
+ else
+ return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, no_path_found_tryagain);
+ }
+}
+
+/*
+ * loader down
+ * reset unblocking retry counter
+ */
+FSM_TRANS (MOVE_LOADER_UNBLOCKING_UPING,
+ bot_move_succeed,
+ MOVE_LOADER_UNBLOCKING_DOWNING)
+{
+ loader_down ();
+ move_data.loader_unblocking_retry = 2;
+ return FSM_NEXT (MOVE_LOADER_UNBLOCKING_UPING, bot_move_succeed);
+}
+
+/*
+ * loader down
+ * reset unblocking retry counter
+ */
+FSM_TRANS (MOVE_LOADER_UNBLOCKING_UPING,
+ bot_move_failed,
+ MOVE_LOADER_UNBLOCKING_DOWNING)
+{
+ loader_down ();
+ move_data.loader_unblocking_retry = 2;
+ return FSM_NEXT (MOVE_LOADER_UNBLOCKING_UPING, bot_move_failed);
+}
+
+FSM_TRANS (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed,
+ /* rotate towards next position. */
+ path_found_rotate, MOVE_ROTATING,
+ /* move to next position. */
+ path_found, MOVE_MOVING,
+ /* post failure. */
+ no_path_found, MOVE_IDLE)
+{
+ /* Try to move. */
+ uint8_t next = move_path_init ();
+ if (next)
+ {
+ if (next == 2)
+ return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, path_found_rotate);
+ else
+ return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, path_found);
+ }
+ else
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_fsm_failed));
+ return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, no_path_found);
+ }
+}
+
+FSM_TRANS (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored,
+ /* move backward.
+ * loader up. */
+ tryagain, MOVE_LOADER_UNBLOCKING_UPING,
+ /* rotate towards next position. */
+ tryout_path_found_rotate, MOVE_ROTATING,
+ /* move to next position. */
+ tryout_path_found, MOVE_ROTATING,
+ /* post failure. */
+ tryout_no_path_found, MOVE_IDLE)
+{
+ if (--move_data.loader_unblocking_retry)
+ {
+ asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
+ loader_up ();
+ return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryagain);
+ }
+ else
+ {
+ /* Try to move. */
+ uint8_t next = move_path_init ();
+ if (next)
+ {
+ if (next == 2)
+ return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_path_found_rotate);
+ else
+ return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_path_found);
+ }
+ else
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_fsm_failed));
+ return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_no_path_found);
+ }
+ }
+}
+
+FSM_TRANS_TIMEOUT (MOVE_LOADER_UNBLOCKING_DOWNING, 450,
+ /* move backward.
+ * loader up. */
+ tryagain, MOVE_LOADER_UNBLOCKING_UPING,
+ /* rotate towards next position. */
+ tryout_path_found_rotate, MOVE_ROTATING,
+ /* move to next position. */
+ tryout_path_found, MOVE_ROTATING,
+ /* post failure. */
+ tryout_no_path_found, MOVE_IDLE)
+{
+ if (--move_data.loader_unblocking_retry)
+ {
+ asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
+ loader_up ();
+ return FSM_NEXT_TIMEOUT (MOVE_LOADER_UNBLOCKING_DOWNING, tryagain);
+ }
+ else
+ {
+ /* Try to move. */
+ uint8_t next = move_path_init ();
+ if (next)
+ {
+ if (next == 2)
+ return FSM_NEXT_TIMEOUT (MOVE_LOADER_UNBLOCKING_DOWNING, tryout_path_found_rotate);
+ else
+ return FSM_NEXT_TIMEOUT (MOVE_LOADER_UNBLOCKING_DOWNING, tryout_path_found);
+ }
+ else
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_fsm_failed));
+ return FSM_NEXT_TIMEOUT (MOVE_LOADER_UNBLOCKING_DOWNING, tryout_no_path_found);
+ }
+ }
+}
+
diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm
deleted file mode 100644
index add29b05..00000000
--- a/digital/io/src/move.fsm
+++ /dev/null
@@ -1,137 +0,0 @@
-# Move FSM
-# This FSM is responsible to move the bot from the current position to the
-# desired one. It will try to avoid obstacle and manage the case when the
-# asserv is blocked.
-move
- move with avoid obstacle.
-
-States:
- *MOVE_IDLE
- waiting for the beginning of the move FSM.
- MOVE_ROTATING [timeout=1125]
- rotating towards next point.
- MOVE_MOVING [timeout=2250]
- moving to a position (intermediate or final).
- MOVE_MOVING_BACKWARD_TO_TURN_FREELY
- moving backward to go away from what is blocking the bot.
- MOVE_WAIT_FOR_CLEAR_PATH [timeout=255]
- waiting for obstacle to disappear.
- MOVE_LOADER_UNBLOCKING_UPING
- moving loader up and move backward to unblock loader.
- MOVE_LOADER_UNBLOCKING_DOWNING [timeout=450]
- moving loader down.
-
-Events:
- move_start
- initialize the FSM and start the movement directly.
- obstacle_in_front
- the bot has seen something (front is the same when going backward).
-
-MOVE_IDLE:
- move_start: path_found_rotate -> MOVE_ROTATING
- rotate towards next position.
- move_start: path_found -> MOVE_MOVING
- move to next position.
- move_start: no_path_found -> MOVE_IDLE
- post failure event.
-
-MOVE_ROTATING:
- bot_move_succeed -> MOVE_MOVING
- move to next position.
- bot_move_failed -> MOVE_MOVING
- move to next position.
- state_timeout -> MOVE_MOVING
- move to next position.
- loader_errored -> MOVE_LOADER_UNBLOCKING_UPING
- move backward
- loader up
-
-MOVE_MOVING:
- bot_move_succeed: done -> MOVE_IDLE
- post success event.
- bot_move_succeed: path_found_rotate -> MOVE_ROTATING
- rotate towards next position.
- bot_move_succeed: path_found -> MOVE_MOVING
- move to next position.
- bot_move_succeed: no_path_found -> MOVE_IDLE
- post failure event.
- bot_move_failed -> MOVE_MOVING_BACKWARD_TO_TURN_FREELY
- reset final_move.
- move backward to turn freely.
- state_timeout -> MOVE_MOVING_BACKWARD_TO_TURN_FREELY
- reset final_move.
- move backward to turn freely.
- obstacle_in_front: tryagain -> MOVE_WAIT_FOR_CLEAR_PATH
- reset final_move.
- stop the bot.
- obstacle_in_front: tryout -> MOVE_IDLE
- stop the bot.
- post failure event.
- loader_errored -> MOVE_LOADER_UNBLOCKING_UPING
- move backward
- loader up
-
-MOVE_MOVING_BACKWARD_TO_TURN_FREELY:
- bot_move_succeed: tryout -> MOVE_IDLE
- post failure event.
- bot_move_succeed: path_found_rotate -> MOVE_ROTATING
- rotate towards next position.
- bot_move_succeed: path_found -> MOVE_MOVING
- move to next position.
- bot_move_succeed: no_path_found -> MOVE_IDLE
- post failure event.
- bot_move_failed: tryout -> MOVE_IDLE
- post failure event.
- bot_move_failed: path_found_rotate -> MOVE_ROTATING
- rotate towards next position.
- bot_move_failed: path_found -> MOVE_MOVING
- move to next position.
- bot_move_failed: no_path_found_tryagain -> MOVE_WAIT_FOR_CLEAR_PATH
- nothing to do.
- bot_move_failed: no_path_found_tryout -> MOVE_IDLE
- post failure event.
-
-MOVE_WAIT_FOR_CLEAR_PATH:
- state_timeout: path_found_rotate -> MOVE_ROTATING
- rotate towards next position.
- state_timeout: path_found -> MOVE_MOVING
- move to next position.
- state_timeout: no_path_found_tryagain -> .
- decrement counter.
- state_timeout: no_path_found_tryout -> MOVE_IDLE
- post failure.
-
-MOVE_LOADER_UNBLOCKING_UPING:
- bot_move_succeed -> MOVE_LOADER_UNBLOCKING_DOWNING
- loader down
- reset unblocking retry counter
- bot_move_failed -> MOVE_LOADER_UNBLOCKING_DOWNING
- loader down
- reset unblocking retry counter
-
-MOVE_LOADER_UNBLOCKING_DOWNING:
- loader_downed: path_found_rotate -> MOVE_ROTATING
- rotate towards next position.
- loader_downed: path_found -> MOVE_MOVING
- move to next position.
- loader_downed: no_path_found -> MOVE_IDLE
- post failure.
- loader_errored: tryagain -> MOVE_LOADER_UNBLOCKING_UPING
- move backward
- loader up
- loader_errored: tryout_path_found_rotate -> MOVE_ROTATING
- rotate towards next position.
- loader_errored: tryout_path_found -> MOVE_ROTATING
- move to next position.
- loader_errored: tryout_no_path_found -> MOVE_IDLE
- post failure.
- state_timeout: tryagain -> MOVE_LOADER_UNBLOCKING_UPING
- move backward
- loader up
- state_timeout: tryout_path_found_rotate -> MOVE_ROTATING
- rotate towards next position.
- state_timeout: tryout_path_found -> MOVE_ROTATING
- move to next position.
- state_timeout: tryout_no_path_found -> MOVE_IDLE
- post failure.
-
diff --git a/digital/io/src/playground.h b/digital/io/src/playground.h
deleted file mode 100644
index 5c251ce5..00000000
--- a/digital/io/src/playground.h
+++ /dev/null
@@ -1,96 +0,0 @@
-#ifndef playground_h
-#define playground_h
-// playground.h
-// io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
-//
-// Copyright (C) 2008 Dufour Jérémy
-//
-// Robot APB Team/Efrei 2004.
-// Web: http://assos.efrei.fr/robot/
-// Email: robot AT efrei DOT fr
-//
-// This program is free software; you can redistribute it and/or modify
-// it under the terms of the GNU General Public License as published by
-// the Free Software Foundation; either version 2 of the License, or
-// (at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU General Public License
-// along with this program; if not, write to the Free Software
-// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
-//
-// }}}
-
-/**
- * @file Some defines for the playground.
- * For example, you can find the size of the table, the positions of the
- * distributors, ...
- * A few important remarks:
- * - the (0,0) position is the bottom left position on the table (when you
- * see it with the two start zone at the top of the scheme).
- */
-
-#include "defs.h"
-#include "bot.h"
-
-/**
- * The width of the table, in millimeters.
- */
-#define PG_WIDTH 3000
-
-/**
- * The length of the table, in millimeters.
- */
-#define PG_LENGTH 2100
-
-/**
- * The distance from table border for movements.
- */
-#define PG_BORDER_DISTANCE 250
-
-/**
- * Considering there is a symmetry axis on X, this macro will compute the
- * value for on the X axis depending on the color.
- */
-#define PG_X(x) (bot_color ? (x) : PG_WIDTH - (x))
-
-/** Same as PG_Y, but for Y coordinate. Actually nothing is done, there is no
- * symmetry. */
-#define PG_Y(y) (y)
-
-/**
- * Considering there is a symmetry axis on X, this macro will compute the
- * value of the angle depending on the color.
- *
- * Takes degrees as input.
- */
-#define PG_A_DEG(a) \
- (bot_color ? POSITION_A_DEG (a) : POSITION_A_DEG (180 - (a)))
-
-/** Initialiser for position_t applying symmetry according to color. Takes
- * degrees for angle. */
-#define PG_POSITION_DEG(x, y, a) \
- { { PG_X (x), PG_Y (y) }, PG_A_DEG (a) }
-
-/** Initialiser for vect_t applying symmetry according to color. */
-#define PG_VECT(x, y) \
- (vect_t) { PG_X (x), PG_Y (y) }
-
-/**
- * Start zone.
- */
-#define PG_START_ZONE_LENGTH 500
-#define PG_START_ZONE_WIDTH 500
-
-/** Size of the unclimbable slope zone (Eurobot 2010). */
-#define PG_SLOPE_WIDTH (500 + 519 + 500)
-#define PG_SLOPE_LENGTH (500 + 22)
-
-/** Start of field zone. */
-#define PG_FIELD_Y_MAX 1128
-
-#endif // playground_h
diff --git a/digital/io/src/debug.host.h b/digital/io/src/playground_2010.h
index 4930b937..11682c5e 100644
--- a/digital/io/src/debug.host.h
+++ b/digital/io/src/playground_2010.h
@@ -1,6 +1,6 @@
-#ifndef debug_host_h
-#define debug_host_h
-/* debug.host.h */
+#ifndef playground_2010_h
+#define playground_2010_h
+/* playground_2010.h */
/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
*
* Copyright (C) 2008 Dufour Jérémy
@@ -24,19 +24,23 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+#include "playground.h"
/**
- * @file Debug printf functions.
- * This module include a macro to add some debug functions to printf a message
- * under host target. It does nothing under avr target.
+ * Eurobot 2010 specific defines.
*/
-#ifdef HOST
-#include <stdio.h>
-#define DPRINTF(format, args...) \
- do { fprintf (stderr, (format), ## args); } while (0)
-#else /* HOST */
-#define DPRINTF(format, args...)
-#endif /* HOST */
+/**
+ * Start zone.
+ */
+#define PG_START_ZONE_LENGTH 500
+#define PG_START_ZONE_WIDTH 500
+
+/** Size of the unclimbable slope zone. */
+#define PG_SLOPE_WIDTH (500 + 519 + 500)
+#define PG_SLOPE_LENGTH (500 + 22)
+
+/** Start of field zone. */
+#define PG_FIELD_Y_MAX 1128
-#endif /* debug_host_h */
+#endif /* playground_2010_h */
diff --git a/digital/io/src/radar.c b/digital/io/src/radar.c
index 1ba2a437..1ac982c7 100644
--- a/digital/io/src/radar.c
+++ b/digital/io/src/radar.c
@@ -25,7 +25,8 @@
#include "common.h"
#include "radar.h"
-#include "playground.h"
+#include "playground_2010.h"
+#include "bot.h"
#include "usdist.h"
#include "modules/math/geometry/geometry.h"
diff --git a/digital/io/src/simu.host.c b/digital/io/src/simu.host.c
index 276c3b14..f1b7e0d2 100644
--- a/digital/io/src/simu.host.c
+++ b/digital/io/src/simu.host.c
@@ -214,13 +214,13 @@ switch_update (void)
}
void
-main_timer_init (void)
+timer_init (void)
{
simu_init ();
}
uint8_t
-main_timer_wait (void)
+timer_wait (void)
{
mex_node_wait_date (mex_node_date () + 4);
simu_step ();
diff --git a/digital/io/src/switch.avr.c b/digital/io/src/switch.avr.c
index 659b597a..9907a86f 100644
--- a/digital/io/src/switch.avr.c
+++ b/digital/io/src/switch.avr.c
@@ -22,10 +22,10 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+#include "common.h"
#include "switch.h"
-#include "common.h"
#include "modules/utils/utils.h" /* set_bit */
#include "io.h" /* PORT/PIN, bit_is_set */
diff --git a/digital/io/src/switch.h b/digital/io/src/switch.h
index ac8d7dbc..9a804d1b 100644
--- a/digital/io/src/switch.h
+++ b/digital/io/src/switch.h
@@ -29,8 +29,7 @@
* @file Module to manage 'switchs'. For example, colors selector and jack.
*/
-#include "bot.h"
-#include "common.h"
+#include "defs.h"
/**
* Initialize the switch module.
diff --git a/digital/io/src/top.c b/digital/io/src/top.c
index 5bfeaa4d..e899d718 100644
--- a/digital/io/src/top.c
+++ b/digital/io/src/top.c
@@ -22,6 +22,10 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+
+#define FSM_NAME AI
+
+#include "fsm.h"
#include "common.h"
#include "defs.h"
#include "top.h"
@@ -31,6 +35,7 @@
#include "move.h"
#include "chrono.h"
#include "playground.h"
+#include "bot.h"
#include "modules/utils/utils.h"
@@ -88,3 +93,358 @@ top_collect (uint8_t force)
}
}
+FSM_INIT
+
+FSM_STATES (IDLE,
+ WAIT_INIT_TO_FINISH,
+ FIRST_GO_BEGIN_OF_LINE_FAST,
+ FIRST_GO_END_OF_LINE_FAST,
+ FIRST_GO_END_OF_LINE_SLOW,
+ UNLOAD,
+ UNLOAD_LOADER_UP,
+ UNLOAD_FACE_BIN,
+ UNLOAD_FACE_BIN_UNBLOCK,
+ UNLOAD_BACK_BIN,
+ UNLOAD_UNLOAD,
+ COLLECT,
+ COLLECT_SLOW_MOTION,
+ COLLECT_BLACK)
+
+FSM_EVENTS (start,
+ move_fsm_succeed,
+ move_fsm_failed,
+ bot_move_succeed,
+ bot_move_failed,
+ init_match_is_started,
+ in_field,
+ asserv_last_cmd_ack,
+ clamp_succeed)
+
+FSM_START_WITH (IDLE)
+
+/* Nothing to do. */
+FSM_TRANS (IDLE, start, WAIT_INIT_TO_FINISH)
+{
+ return FSM_NEXT (IDLE, start);
+}
+
+/*
+ * the match start
+ * set fast speed
+ * go to first sequence begin of line
+ */
+FSM_TRANS (WAIT_INIT_TO_FINISH,
+ init_match_is_started,
+ FIRST_GO_BEGIN_OF_LINE_FAST)
+{
+ asserv_set_speed (BOT_MOVE_FAST);
+ move_start_noangle (PG_VECT (375, 1503), 0, 0);
+ return FSM_NEXT (WAIT_INIT_TO_FINISH, init_match_is_started);
+}
+
+/*
+ * go to end of line
+ * loader down
+ */
+FSM_TRANS (FIRST_GO_BEGIN_OF_LINE_FAST,
+ move_fsm_succeed,
+ FIRST_GO_END_OF_LINE_FAST)
+{
+ move_start_noangle (PG_VECT (2625, 253), 0, 0);
+ loader_down ();
+ return FSM_NEXT (FIRST_GO_BEGIN_OF_LINE_FAST, move_fsm_succeed);
+}
+
+/*
+ * set slow speed
+ * retry
+ */
+FSM_TRANS (FIRST_GO_BEGIN_OF_LINE_FAST,
+ move_fsm_failed,
+ FIRST_GO_BEGIN_OF_LINE_FAST)
+{
+ asserv_set_speed (BOT_MOVE_SLOW);
+ move_start_noangle (PG_VECT (375, 1503), 0, 0);
+ return FSM_NEXT (FIRST_GO_BEGIN_OF_LINE_FAST, move_fsm_failed);
+}
+
+/*
+ * set slow speed
+ */
+FSM_TRANS (FIRST_GO_END_OF_LINE_FAST,
+ in_field,
+ FIRST_GO_END_OF_LINE_SLOW)
+{
+ asserv_set_speed (BOT_MOVE_SLOW);
+ return FSM_NEXT (FIRST_GO_END_OF_LINE_FAST, in_field);
+}
+
+/*
+ * set slow speed
+ * move loader up
+ */
+FSM_TRANS (FIRST_GO_END_OF_LINE_FAST,
+ move_fsm_succeed,
+ UNLOAD_LOADER_UP)
+{
+ asserv_set_speed (BOT_MOVE_SLOW);
+ loader_up ();
+ return FSM_NEXT (FIRST_GO_END_OF_LINE_FAST, move_fsm_succeed);
+}
+
+/*
+ * set slow speed
+ * retry
+ */
+FSM_TRANS (FIRST_GO_END_OF_LINE_FAST,
+ move_fsm_failed,
+ FIRST_GO_END_OF_LINE_SLOW)
+{
+ asserv_set_speed (BOT_MOVE_SLOW);
+ move_start_noangle (PG_VECT (2625, 253), 0, 0);
+ return FSM_NEXT (FIRST_GO_END_OF_LINE_FAST, move_fsm_failed);
+}
+
+/*
+ * move loader up
+ */
+FSM_TRANS (FIRST_GO_END_OF_LINE_SLOW,
+ move_fsm_succeed,
+ UNLOAD_LOADER_UP)
+{
+ loader_up ();
+ return FSM_NEXT (FIRST_GO_END_OF_LINE_SLOW, move_fsm_succeed);
+}
+
+/*
+ * retry
+ */
+FSM_TRANS (FIRST_GO_END_OF_LINE_SLOW,
+ move_fsm_failed,
+ FIRST_GO_END_OF_LINE_SLOW)
+{
+ move_start_noangle (PG_VECT (2625, 253), 0, 0);
+ return FSM_NEXT (FIRST_GO_END_OF_LINE_SLOW, move_fsm_failed);
+}
+
+/*
+ * move loader up
+ */
+FSM_TRANS (UNLOAD, move_fsm_succeed, UNLOAD_LOADER_UP)
+{
+ loader_up ();
+ return FSM_NEXT (UNLOAD, move_fsm_succeed);
+}
+
+/*
+ * retry
+ */
+FSM_TRANS (UNLOAD, move_fsm_failed, UNLOAD)
+{
+ move_start_noangle (PG_VECT (2625, 253), 0, 0);
+ return FSM_NEXT (UNLOAD, move_fsm_failed);
+}
+
+/*
+ * turn toward bin
+ */
+FSM_TRANS (UNLOAD_LOADER_UP, loader_uped, UNLOAD_FACE_BIN)
+{
+ asserv_goto_angle (PG_A_DEG (90));
+ return FSM_NEXT (UNLOAD_LOADER_UP, loader_uped);
+}
+
+/*
+ * turn toward bin
+ */
+FSM_TRANS (UNLOAD_LOADER_UP, loader_errored, UNLOAD_FACE_BIN)
+{
+ asserv_goto_angle (PG_A_DEG (90));
+ return FSM_NEXT (UNLOAD_LOADER_UP, loader_errored);
+}
+
+/*
+ * go backward to bin
+ */
+FSM_TRANS (UNLOAD_FACE_BIN, bot_move_succeed, UNLOAD_BACK_BIN)
+{
+ asserv_move_linearly (-(128 + 250 / 2 - BOT_SIZE_BACK - 50));
+ return FSM_NEXT (UNLOAD_FACE_BIN, bot_move_succeed);
+}
+
+/*
+ * move backward
+ */
+FSM_TRANS (UNLOAD_FACE_BIN, bot_move_failed, UNLOAD_FACE_BIN_UNBLOCK)
+{
+ asserv_move_linearly (-40);
+ return FSM_NEXT (UNLOAD_FACE_BIN, bot_move_failed);
+}
+
+/*
+ * turn toward bin
+ */
+FSM_TRANS (UNLOAD_FACE_BIN_UNBLOCK, bot_move_succeed, UNLOAD_FACE_BIN)
+{
+ asserv_goto_angle (PG_A_DEG (90));
+ return FSM_NEXT (UNLOAD_FACE_BIN_UNBLOCK, bot_move_succeed);
+}
+
+/*
+ * turn toward bin
+ */
+FSM_TRANS (UNLOAD_FACE_BIN_UNBLOCK, bot_move_failed, UNLOAD_FACE_BIN)
+{
+ asserv_goto_angle (PG_A_DEG (90));
+ return FSM_NEXT (UNLOAD_FACE_BIN_UNBLOCK, bot_move_failed);
+}
+
+/*
+ * unload
+ */
+FSM_TRANS (UNLOAD_BACK_BIN, bot_move_succeed, UNLOAD_UNLOAD)
+{
+ asserv_move_motor1_absolute (0, BOT_GATE_SPEED);
+ return FSM_NEXT (UNLOAD_BACK_BIN, bot_move_succeed);
+}
+
+/*
+ * unload
+ */
+FSM_TRANS (UNLOAD_BACK_BIN, bot_move_failed, UNLOAD_UNLOAD)
+{
+ asserv_move_motor1_absolute (0, BOT_GATE_SPEED);
+ return FSM_NEXT (UNLOAD_BACK_BIN, bot_move_failed);
+}
+
+/*
+ * close gate
+ * loader down
+ * choose best food to collect
+ */
+FSM_TRANS_TIMEOUT (UNLOAD_UNLOAD, 255, COLLECT)
+{
+ loader_elements = 0;
+ asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED);
+ loader_down ();
+ top_collect (1);
+ return FSM_NEXT_TIMEOUT (UNLOAD_UNLOAD);
+}
+
+/*
+ * post loader_element event
+ */
+FSM_TRANS (COLLECT, move_fsm_succeed,
+ unload, UNLOAD,
+ collect, COLLECT,
+ slow_motion, COLLECT_SLOW_MOTION)
+{
+ int16_t slow_motion = food_slow_motion (top_food);
+ if (slow_motion)
+ {
+ FSM_HANDLE (AI, loader_element);
+ return FSM_NEXT (COLLECT, move_fsm_succeed, slow_motion);
+ }
+ else if (top_collect (0))
+ return FSM_NEXT (COLLECT, move_fsm_succeed, collect);
+ else
+ return FSM_NEXT (COLLECT, move_fsm_succeed, unload);
+}
+
+/*
+ * cancel this element
+ */
+FSM_TRANS (COLLECT, move_fsm_failed,
+ unload, UNLOAD,
+ collect, COLLECT)
+{
+ if (top_collect (0))
+ {
+ food_cancel (top_food);
+ return FSM_NEXT (COLLECT, move_fsm_failed, collect);
+ }
+ else
+ return FSM_NEXT (COLLECT, move_fsm_failed, unload);
+}
+
+/*
+ * move backward
+ * mark as black
+ */
+FSM_TRANS (COLLECT, loader_black, COLLECT_BLACK)
+{
+ asserv_move_linearly (-90);
+ food_black (top_food);
+ return FSM_NEXT (COLLECT, loader_black);
+}
+
+/*
+ * collect
+ */
+FSM_TRANS (COLLECT_SLOW_MOTION, loader_downed,
+ unload, UNLOAD,
+ collect, COLLECT)
+{
+ if (top_collect (0))
+ return FSM_NEXT (COLLECT_SLOW_MOTION, loader_downed, collect);
+ else
+ return FSM_NEXT (COLLECT_SLOW_MOTION, loader_downed, unload);
+}
+
+/*
+ * same as above
+ */
+FSM_TRANS (COLLECT_SLOW_MOTION, loader_errored,
+ unload, UNLOAD,
+ collect, COLLECT)
+{
+ if (top_collect (0))
+ return FSM_NEXT (COLLECT_SLOW_MOTION, loader_errored, collect);
+ else
+ return FSM_NEXT (COLLECT_SLOW_MOTION, loader_errored, unload);
+}
+
+/*
+ * same as above
+ */
+FSM_TRANS_TIMEOUT (COLLECT_SLOW_MOTION, 450,
+ unload, UNLOAD,
+ collect, COLLECT)
+{
+ if (top_collect (0))
+ return FSM_NEXT_TIMEOUT (COLLECT_SLOW_MOTION, collect);
+ else
+ return FSM_NEXT_TIMEOUT (COLLECT_SLOW_MOTION, unload);
+}
+
+/*
+ * move backward
+ * mark as black
+ */
+FSM_TRANS (COLLECT_SLOW_MOTION, loader_black, COLLECT_BLACK)
+{
+ asserv_move_linearly (-90);
+ food_black (top_food);
+ return FSM_NEXT (COLLECT_SLOW_MOTION, loader_black);
+}
+
+FSM_TRANS (COLLECT_BLACK, bot_move_succeed,
+ unload, UNLOAD,
+ collect, COLLECT)
+{
+ if (top_collect (0))
+ return FSM_NEXT (COLLECT_BLACK, bot_move_succeed, collect);
+ else
+ return FSM_NEXT (COLLECT_BLACK, bot_move_succeed, unload);
+}
+
+FSM_TRANS (COLLECT_BLACK, bot_move_failed,
+ unload, UNLOAD,
+ collect, COLLECT)
+{
+ if (top_collect (0))
+ return FSM_NEXT (COLLECT_BLACK, bot_move_failed, collect);
+ else
+ return FSM_NEXT (COLLECT_BLACK, bot_move_failed, unload);
+}
+
diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm
deleted file mode 100644
index 277f0a5a..00000000
--- a/digital/io/src/top.fsm
+++ /dev/null
@@ -1,162 +0,0 @@
-# Top FSM.
-top
- Main FSM calling other FSM.
-
-States:
- *IDLE
- waiting for the beginning of the top FSM.
- WAIT_INIT_TO_FINISH
- waiting for the end of init.
- FIRST_GO_BEGIN_OF_LINE_FAST
- first sequence, go to start of diagonal line
- FIRST_GO_END_OF_LINE_FAST
- first sequence, go to end of line
- FIRST_GO_END_OF_LINE_SLOW
- slow down to take elements
- UNLOAD
- go to unloading position
- UNLOAD_LOADER_UP
- move loader to up position
- UNLOAD_FACE_BIN
- orient robot to unload angle
- UNLOAD_FACE_BIN_UNBLOCK
- move backward to unblock
- UNLOAD_BACK_BIN
- move back to unload bin
- UNLOAD_UNLOAD[timeout=225]
- unloading, wait
- COLLECT
- collecting elements
- COLLECT_SLOW_MOTION[timeout=450]
- picking element before bumper contact
- COLLECT_BLACK
- moving backward after trying to collect a black corn
-
-Events:
- start
- initialize the FSM.
- move_fsm_succeed
- when the move FSM returns with success.
- move_fsm_failed
- when the move FSM returns with an error.
- bot_move_succeed
- when the bot has succeed its asserv movement.
- bot_move_failed
- when the bot has failed its asserv movement.
- init_match_is_started
- when the init FSM has finished and the match start.
- in_field
- the robot is in the zone containing interesting elements.
- state_timeout
- timeout in cycle count.
- asserv_last_cmd_ack
- last command sent to the asserv board has been acquitted.
- clamp_succeed
- asserv success result
-
-import init.fsm
-import hola.fsm
-import loader.fsm
-import move.fsm
-
-IDLE:
- start -> WAIT_INIT_TO_FINISH
- nothing to do.
-
-WAIT_INIT_TO_FINISH:
- init_match_is_started -> FIRST_GO_BEGIN_OF_LINE_FAST
- the match start
- set fast speed
- go to first sequence begin of line
-
-FIRST_GO_BEGIN_OF_LINE_FAST:
- move_fsm_succeed -> FIRST_GO_END_OF_LINE_FAST
- go to end of line
- loader down
- move_fsm_failed -> .
- set slow speed
- retry
-
-FIRST_GO_END_OF_LINE_FAST:
- in_field -> FIRST_GO_END_OF_LINE_SLOW
- set slow speed
- move_fsm_succeed -> UNLOAD_LOADER_UP
- set slow speed
- move loader up
- move_fsm_failed -> FIRST_GO_END_OF_LINE_SLOW
- set slow speed
- retry
-
-FIRST_GO_END_OF_LINE_SLOW:
- move_fsm_succeed -> UNLOAD_LOADER_UP
- move loader up
- move_fsm_failed -> FIRST_GO_END_OF_LINE_SLOW
- retry
-
-UNLOAD:
- move_fsm_succeed -> UNLOAD_LOADER_UP
- move loader up
- move_fsm_failed -> .
- retry
-
-UNLOAD_LOADER_UP:
- loader_uped -> UNLOAD_FACE_BIN
- turn toward bin
- loader_errored -> UNLOAD_FACE_BIN
- turn toward bin
-
-UNLOAD_FACE_BIN:
- bot_move_succeed -> UNLOAD_BACK_BIN
- go backward to bin
- bot_move_failed -> UNLOAD_FACE_BIN_UNBLOCK
- move backward
-
-UNLOAD_FACE_BIN_UNBLOCK:
- bot_move_succeed -> UNLOAD_FACE_BIN
- turn toward bin
- bot_move_failed -> UNLOAD_FACE_BIN
- turn toward bin
-
-UNLOAD_BACK_BIN:
- bot_move_succeed -> UNLOAD_UNLOAD
- unload
- bot_move_failed -> UNLOAD_UNLOAD
- unload
-
-UNLOAD_UNLOAD:
- state_timeout -> COLLECT
- close gate
- loader down
- choose best food to collect
-
-COLLECT:
- move_fsm_succeed: unload -> UNLOAD
- move_fsm_succeed: collect -> COLLECT
- move_fsm_succeed: slow_motion -> COLLECT_SLOW_MOTION
- post loader_element event
- move_fsm_failed: unload -> UNLOAD
- move_fsm_failed: collect -> COLLECT
- cancel this element
- loader_black -> COLLECT_BLACK
- move backward
- mark as black
-
-COLLECT_SLOW_MOTION:
- loader_downed: unload -> UNLOAD
- loader_downed: collect -> COLLECT
- collect
- loader_errored: unload -> UNLOAD
- loader_errored: collect -> COLLECT
- same as above
- state_timeout: unload -> UNLOAD
- state_timeout: collect -> COLLECT
- same as above
- loader_black -> COLLECT_BLACK
- move backward
- mark as black
-
-COLLECT_BLACK:
- bot_move_succeed: unload -> UNLOAD
- bot_move_succeed: collect -> COLLECT
- bot_move_failed: unload -> UNLOAD
- bot_move_failed: collect -> COLLECT
diff --git a/digital/io/src/trace.trc b/digital/io/src/trace.trc
index 4cdcf8e0..1da5012b 100644
--- a/digital/io/src/trace.trc
+++ b/digital/io/src/trace.trc
@@ -1,4 +1,4 @@
-EVENT main_timer__late "MainTimer: we are late."
+EVENT timer__late "Timer: we are late."
EVENT fsm__handle_event from 1 event 1 to 1 "FSM: %i =%i=> %i."
EVENT move__go_to xd 2 yd 2 "Move: (%d, %d)."
EVENT food__best food 1 "Food best: %d."
diff --git a/digital/io/src/twi_master.c b/digital/io/src/twi_master.c
deleted file mode 100644
index cb600da8..00000000
--- a/digital/io/src/twi_master.c
+++ /dev/null
@@ -1,219 +0,0 @@
-/* twi_master.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 "twi_master.h"
-
-#include "asserv.h"
-#include "mimot.h"
-
-#include "modules/twi/twi.h"
-#include "modules/utils/utils.h"
-#include "modules/utils/crc.h"
-
-/** Interval between retransmissions. */
-#define TWI_MASTER_RETRANSMIT_INTERVAL 10
-
-/** Index of the slave sequence number. */
-#define TWI_MASTER_STATUS_SEQ_INDEX 2
-
-/** Maximum command payload size. */
-#define TWI_MASTER_COMMAND_PAYLOAD_MAX 13
-
-/** Maximum status payload size. */
-#define TWI_MASTER_STATUS_PAYLOAD_MAX 15
-
-/** Maximum number of pending commands. */
-#define TWI_MASTER_PENDING_MAX 16
-
-/** Position of next free command. */
-#define TWI_MASTER_PENDING_TAIL \
- ((twi_master.pending_head + twi_master.pending_nb) \
- % TWI_MASTER_PENDING_MAX)
-
-/** Pending command structure. */
-struct twi_master_command_t
-{
- /** Addressed slave index. */
- uint8_t slave;
- /** Command payload with space for CRC. */
- uint8_t command[TWI_MASTER_COMMAND_PAYLOAD_MAX + 1];
- /** Command length not including CRC. */
- uint8_t length;
-};
-
-/** Global context. */
-struct twi_master_t
-{
- /** Retransmission counter, retransmit when it reach zero. It is also set
- * to zero after an acknowledge so that the next pending command is
- * sent. */
- uint8_t retransmit_counter;
- /** Index of next pending command. */
- uint8_t pending_head;
- /** Number of pending commands. */
- uint8_t pending_nb;
- /** Table of pending commands. The next command is at pending_head.
- * Next commands are stored in a circular buffer way. */
- struct twi_master_command_t pending[TWI_MASTER_PENDING_MAX];
-} twi_master;
-
-/** Callback called when a slave status has been read.
- * - status: status buffer (without CRC). */
-typedef void (*twi_master_slave_status_cb) (uint8_t *status);
-
-/** Information on a slave. */
-struct twi_master_slave_t
-{
- /** Slave address. */
- uint8_t address;
- /** Last command sequence number. */
- uint8_t seq;
- /** Size of the status buffer not including CRC. */
- uint8_t status_length;
- /** Status callback. */
- twi_master_slave_status_cb status_cb;
-};
-
-/** Information on all slaves. */
-static struct twi_master_slave_t twi_master_slaves[] = {
- { ASSERV_TWI_ADDRESS, 0, ASSERV_STATUS_LENGTH, asserv_status_cb },
- { MIMOT_TWI_ADDRESS, 0, MIMOT_STATUS_LENGTH, mimot_status_cb },
-};
-
-/** Send first pending message if available. */
-static void
-twi_master_send_head (void)
-{
- if (twi_master.pending_nb)
- {
- struct twi_master_command_t *c =
- &twi_master.pending[twi_master.pending_head];
- /* Send command. */
- twi_master_send (twi_master_slaves[c->slave].address, c->command,
- c->length + 1);
- twi_master_wait ();
- /* Reset retransmission counter. */
- twi_master.retransmit_counter = TWI_MASTER_RETRANSMIT_INTERVAL;
- }
-}
-
-/** Update slave status, return non zero on success. */
-static uint8_t
-twi_master_update_status (uint8_t slave, uint8_t init)
-{
- uint8_t buffer[TWI_MASTER_STATUS_PAYLOAD_MAX + 1];
- /* Read status. */
- twi_master_recv (twi_master_slaves[slave].address, buffer,
- twi_master_slaves[slave].status_length + 1);
- uint8_t ret = twi_master_wait ();
- if (ret != twi_master_slaves[slave].status_length + 1)
- return 0;
- uint8_t crc = crc_compute (buffer + 1,
- twi_master_slaves[slave].status_length);
- if (crc != buffer[0])
- return 0;
- if (init)
- twi_master_slaves[slave].seq =
- buffer[1 + TWI_MASTER_STATUS_SEQ_INDEX];
- /* Call user callback. */
- twi_master_slaves[slave].status_cb (buffer + 1);
- /* Update pending command list. */
- if (twi_master.pending_nb)
- {
- struct twi_master_command_t *c =
- &twi_master.pending[twi_master.pending_head];
- if (slave == c->slave
- && buffer[1 + TWI_MASTER_STATUS_SEQ_INDEX] == c->command[1])
- {
- /* Successfully acknowledged. */
- twi_master.pending_nb--;
- twi_master.pending_head = (twi_master.pending_head + 1)
- % TWI_MASTER_PENDING_MAX;
- /* Trigger next transmission. */
- twi_master.retransmit_counter = 0;
- }
- }
- /* Success. */
- return 1;
-}
-
-void
-twi_master_init (void)
-{
- uint8_t i;
- /* Initialise hardware. */
- twi_init (AC_IO_TWI_ADDRESS);
- /* Get first status and reset sequence number. */
- for (i = 0; i < UTILS_COUNT (twi_master_slaves); i++)
- while (!twi_master_update_status (i, 1))
- ;
-}
-
-uint8_t
-twi_master_sync (void)
-{
- uint8_t i;
- /* Update all slaves status. */
- for (i = 0; i < UTILS_COUNT (twi_master_slaves); i++)
- twi_master_update_status (i, 0);
- /* If command is to be retransmitted (or transmitted for the first time
- * after a acknowledge). */
- if (twi_master.retransmit_counter == 0)
- twi_master_send_head ();
- else
- twi_master.retransmit_counter--;
- /* Synchronised if no pending command. */
- return twi_master.pending_nb == 0;
-}
-
-uint8_t *
-twi_master_get_buffer (uint8_t slave)
-{
- assert (twi_master.pending_nb < TWI_MASTER_PENDING_MAX);
- struct twi_master_command_t *c =
- &twi_master.pending[TWI_MASTER_PENDING_TAIL];
- /* Store slave. */
- c->slave = slave;
- /* Skip CRC and sequence number. */
- return &c->command[2];
-}
-
-void
-twi_master_send_buffer (uint8_t length)
-{
- assert (length != 0);
- struct twi_master_command_t *c =
- &twi_master.pending[TWI_MASTER_PENDING_TAIL];
- /* Fill sequence number, compute CRC, store length. */
- c->command[1] = ++twi_master_slaves[c->slave].seq;
- c->command[0] = crc_compute (&c->command[1], length + 1);
- c->length = length + 1;
- /* Add to the list of pending command. */
- twi_master.pending_nb++;
- /* Early transmission. */
- if (twi_master.pending_nb == 1)
- twi_master_send_head ();
-}
-
diff --git a/digital/io/src/twi_master.h b/digital/io/src/twi_master.h
deleted file mode 100644
index e7cb9d86..00000000
--- a/digital/io/src/twi_master.h
+++ /dev/null
@@ -1,68 +0,0 @@
-#ifndef twi_master_h
-#define twi_master_h
-/* twi_master.h */
-/* 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.
- *
- * }}} */
-
-/**
- * Handle communication as a master to several slaves.
- *
- * The communication protocol is always based on:
- * - a message sent to the slave with a command to execute,
- * - a status read from the slave containing the current slave state.
- *
- * The first byte of all messages is a CRC of the following bytes, called the
- * message payload.
- *
- * The first byte of payload sent to slave is the command sequence number. It
- * is used by the master to know if its command has been handled. The last
- * handled command sequence number is available in the slave status (third
- * byte).
- *
- * As long as the slave last command sequence number is not equal to the last
- * sent sequence number, the master can not send any other command. If the
- * slave do not acknowledge the command after a time out, the command is sent
- * again.
- *
- * Several commands can be stored by this module, it will defer their
- * transmission until the first command is acknowledged.
- */
-
-/** Initialise module. */
-void
-twi_master_init (void);
-
-/** Synchronise all slaves, return non zero if synchronised. */
-uint8_t
-twi_master_sync (void);
-
-/** Get a buffer to send a command to the corresponding slave. */
-uint8_t *
-twi_master_get_buffer (uint8_t slave);
-
-/** Send previously got buffer with the given length. */
-void
-twi_master_send_buffer (uint8_t length);
-
-#endif /* twi_master_h */
diff --git a/digital/io/src/usdist.c b/digital/io/src/usdist.c
index df26ea79..d6fd9730 100644
--- a/digital/io/src/usdist.c
+++ b/digital/io/src/usdist.c
@@ -24,7 +24,7 @@
* }}} */
#include "common.h"
#include "usdist.h"
-#include "main_timer.h"
+#include "timer.h"
#include "modules/adc/adc.h"
#include "modules/utils/utils.h"
diff --git a/digital/io/src/usdist.h b/digital/io/src/usdist.h
index 541d1c93..9a4a6646 100644
--- a/digital/io/src/usdist.h
+++ b/digital/io/src/usdist.h
@@ -42,7 +42,7 @@
#define USDIST_MM_TOO_FAR 650
/** Measuring period in cycles. */
-#define USDIST_PERIOD_CYCLE (uint8_t) (8.0 / MT_TC0_PERIOD)
+#define USDIST_PERIOD_CYCLE (uint8_t) (8.0 / TIMER_PERIOD_MS)
/** Array containing the last measures in millimeters. */
extern uint16_t usdist_mm[USDIST_NB];
diff --git a/digital/io/tools/dump_usdist.py b/digital/io/tools/dump_usdist.py
index 43e0724e..001a8cc7 100644
--- a/digital/io/tools/dump_usdist.py
+++ b/digital/io/tools/dump_usdist.py
@@ -1,11 +1,8 @@
-import sys
-
import io
import io.init
-import serial
+from utils.init_proto import init_proto
-fd = serial.Serial (sys.argv[1])
-p = io.Proto (fd, **io.init.target)
+p = init_proto ('marcel', io.Proto, io.init)
def cb (*val):
l = [ ]
for v in val:
diff --git a/digital/io/tools/io/init.py b/digital/io/tools/io/init.py
index 059ea408..ebcb1ec9 100644
--- a/digital/io/tools/io/init.py
+++ b/digital/io/tools/io/init.py
@@ -1,6 +1,12 @@
"""Default parameters for io."""
-host = dict (
- servo_pos = ((1, 255), (255, 1), (1, 255), (1, 255), (1, 255),
- (1, 255), (1, 255), (1, 255)),
- )
+host = {
+ 'aquajim': dict (
+ servo_pos = ((1, 255), (255, 1), (1, 255), (1, 255), (1, 255),
+ (1, 255), (1, 255), (1, 255)),
+ ),
+ 'marcel': dict (
+ ),
+ 'robospierre': dict (
+ ),
+ }
target = host
diff --git a/digital/io/tools/io/io.py b/digital/io/tools/io/io.py
index 87471bbd..edefbd9d 100644
--- a/digital/io/tools/io/io.py
+++ b/digital/io/tools/io/io.py
@@ -34,8 +34,9 @@ class Proto:
def send_param (self):
p = self.param
- for i, t in enumerate (p['servo_pos']):
- self.proto.send ('p', 'BBB', i, *t)
+ if ('servo_pos') in p:
+ for i, t in enumerate (p['servo_pos']):
+ self.proto.send ('p', 'BBB', i, *t)
def write_eeprom (self):
self.proto.send ('p', 'cc', 'E', 's')
@@ -53,7 +54,7 @@ class Proto:
self.proto.send ('s', 'BB', servo, high_time)
def pwm_set (self, value, timer):
- self.proto.send ('w', 'Hh', value, timer)
+ self.proto.send ('w', 'hH', value, timer)
def loader (self, command):
self.proto.send ('c', 'c', command)
diff --git a/digital/io/tools/test_simu.py b/digital/io/tools/test_simu.py
deleted file mode 100644
index c6fc24f3..00000000
--- a/digital/io/tools/test_simu.py
+++ /dev/null
@@ -1,156 +0,0 @@
-# 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.
-#
-# }}}
-import math
-
-import mex.hub
-import utils.forked
-
-import asserv
-import asserv.init
-import mimot
-import mimot.init
-import io
-import io.init
-from proto.popen_io import PopenIO
-
-import simu.model.table_eurobot2010 as table_model
-import simu.view.table_eurobot2010 as table
-
-import simu.model.round_obstacle as obstacle_model
-import simu.view.round_obstacle as obstacle_view
-
-import simu.robots.marcel.link.bag as robot_link
-import simu.robots.marcel.model.bag as robot_model
-import simu.robots.marcel.view.bag as robot_view
-
-from simu.inter.inter_node import InterNode
-from Tkinter import *
-
-class ObstacleWithBeacon (obstacle_view.RoundObstacle):
-
- def __init__ (self, onto, model, beacon_model):
- obstacle_view.RoundObstacle.__init__ (self, onto, model)
- self.beacon_model = beacon_model
-
- def __notified (self):
- self.pos = self.model.pos
- self.update ()
-
- def draw (self):
- obstacle_view.RoundObstacle.draw (self)
- if self.pos:
- self.draw_circle ((0, 0), self.beacon_model.radius,
- fill = '#505050')
-
-class TestSimu (InterNode):
- """Interface, with simulated programs."""
-
- robot_start_pos = {
- False: (300, 2100 - 305, math.radians (-270)),
- True: (3000 - 300, 2100 - 305, math.radians (-270))
- }
-
- def __init__ (self, asserv_cmd, mimot_cmd, io_cmd):
- # Hub.
- self.hub = mex.hub.Hub (min_clients = 2)
- self.forked_hub = utils.forked.Forked (self.hub.wait)
- # InterNode.
- InterNode.__init__ (self)
- def time ():
- return self.node.date / self.node.tick
- # Asserv.
- self.asserv = asserv.Proto (PopenIO (asserv_cmd), time,
- **asserv.init.host)
- self.asserv.async = True
- self.tk.createfilehandler (self.asserv, READABLE, self.asserv_read)
- # Mimot.
- self.mimot = mimot.Proto (PopenIO (mimot_cmd), time,
- **mimot.init.host)
- self.mimot.async = True
- self.tk.createfilehandler (self.mimot, READABLE, self.mimot_read)
- # Io.
- self.io = io.Proto (PopenIO (io_cmd), time, **io.init.host)
- self.io.async = True
- self.tk.createfilehandler (self.io, READABLE, self.io_read)
- # Add table.
- self.table_model = table_model.Table ()
- self.table = table.Table (self.table_view, self.table_model)
- self.obstacle = obstacle_model.RoundObstacle (150)
- self.table_model.obstacles.append (self.obstacle)
- self.obstacle_beacon = obstacle_model.RoundObstacle (40, 2)
- self.table_model.obstacles.append (self.obstacle_beacon)
- self.obstacle_view = ObstacleWithBeacon (self.table, self.obstacle,
- self.obstacle_beacon)
- self.table_view.bind ('<2>', self.place_obstacle)
- # Add robot.
- self.robot_link = robot_link.Bag (self.node)
- self.robot_model = robot_model.Bag (self.node, self.table_model,
- self.robot_link)
- self.robot_view = robot_view.Bag (self.table, self.actuator_view,
- self.sensor_frame, self.robot_model)
- # Color switch.
- self.robot_model.color_switch.register (self.change_color)
-
- def close (self):
- self.forked_hub.kill ()
- import time
- time.sleep (1)
-
- def asserv_read (self, file, mask):
- self.asserv.proto.read ()
- self.asserv.proto.sync ()
-
- def mimot_read (self, file, mask):
- self.mimot.proto.read ()
- self.mimot.proto.sync ()
-
- def io_read (self, file, mask):
- self.io.proto.read ()
- self.io.proto.sync ()
-
- def step (self):
- """Overide step to handle retransmissions, could be made cleaner using
- simulated time."""
- InterNode.step (self)
- self.asserv.proto.sync ()
- self.mimot.proto.sync ()
- self.io.proto.sync ()
-
- def change_color (self, *dummy):
- i = self.robot_model.color_switch.state
- self.asserv.set_simu_pos (*self.robot_start_pos[i]);
-
- def place_obstacle (self, ev):
- pos = self.table_view.screen_coord ((ev.x, ev.y))
- self.obstacle.pos = pos
- self.obstacle_beacon.pos = pos
- self.obstacle.notify ()
- self.obstacle_beacon.notify ()
-
-if __name__ == '__main__':
- app = TestSimu (('../../asserv/src/asserv/asserv.host', '-m9', 'marcel'),
- ('../../mimot/src/dirty/dirty.host', '-m9', 'marcel'),
- ('../src/io.host'))
- app.mainloop ()
- app.close ()
diff --git a/digital/io/tools/test_simu_control.py b/digital/io/tools/test_simu_control.py
deleted file mode 100644
index 9365ab2e..00000000
--- a/digital/io/tools/test_simu_control.py
+++ /dev/null
@@ -1,113 +0,0 @@
-# io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
-#
-# Copyright (C) 2009 Nicolas Schodet
-#
-# APBTeam:
-# Web: http://apbteam.org/
-# Email: team AT apbteam DOT org
-#
-# This program is free software; you can redistribute it and/or modify
-# it under the terms of the GNU General Public License as published by
-# the Free Software Foundation; either version 2 of the License, or
-# (at your option) any later version.
-#
-# This program is distributed in the hope that it will be useful,
-# but WITHOUT ANY WARRANTY; without even the implied warranty of
-# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-# GNU General Public License for more details.
-#
-# You should have received a copy of the GNU General Public License
-# along with this program; if not, write to the Free Software
-# Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
-#
-# }}}
-from test_simu import TestSimu
-from Tkinter import *
-import math
-
-class TestSimuControl (TestSimu):
- """Interface with extra control."""
-
- def __init__ (self, asserv_cmd, mimot_cmd, io_cmd):
- TestSimu.__init__ (self, asserv_cmd, mimot_cmd, io_cmd)
-
- def create_widgets (self):
- TestSimu.create_widgets (self)
- self.control_frame = Frame (self)
- self.control_frame.pack (side = 'left', before = self.table_view,
- fill = 'y')
- self.clamp_var = IntVar ()
- self.clamp_button = Checkbutton (self.control_frame, text = 'Clamp',
- indicatoron = False,
- variable = self.clamp_var, command = self.clamp_command)
- self.clamp_button.pack ()
- self.elevator_var = IntVar ()
- self.elevator_button = Checkbutton (self.control_frame,
- text = 'Elevator', indicatoron = False,
- variable = self.elevator_var, command = self.elevator_command)
- self.elevator_button.pack ()
- self.gate_var = IntVar ()
- self.gate_button = Checkbutton (self.control_frame,
- text = 'Gate', indicatoron = False,
- variable = self.gate_var, command = self.gate_command)
- self.gate_button.pack ()
- self.loader_up_button = Button (self.control_frame,
- text = 'Loader up', padx = 0, pady = 0,
- command = self.loader_up_command)
- self.loader_up_button.pack ()
- self.loader_down_button = Button (self.control_frame,
- text = 'Loader down', padx = 0, pady = 0,
- command = self.loader_down_command)
- self.loader_down_button.pack ()
- self.table_view.bind ('<1>', self.move)
- self.table_view.bind ('<3>', self.orient)
-
- def move (self, ev):
- pos = self.table_view.screen_coord ((ev.x, ev.y))
- self.asserv.goto (pos[0], pos[1])
-
- def orient (self, ev):
- x, y = self.table_view.screen_coord ((ev.x, ev.y))
- robot_pos = self.robot_model.position.pos
- if robot_pos is not None:
- a = math.atan2 (y - robot_pos[1], x - robot_pos[0])
- self.asserv.goto_angle (a)
-
- def clamp_command (self):
- if self.clamp_var.get ():
- self.mimot.clamp ('a0', 0x60, 0x100)
- self.mimot.clamp ('a1', 0x60, 0x100)
- else:
- self.mimot.goto_pos ('a0', 0)
- self.mimot.goto_pos ('a1', 0)
-
- def elevator_command (self):
- if self.elevator_var.get ():
- pos = 7089
- else:
- pos = 0
- self.asserv.goto_pos ('a0', pos)
-
- def gate_command (self):
- if self.gate_var.get ():
- pos = -0x1d6b
- else:
- pos = 0
- self.asserv.goto_pos ('a1', pos)
-
- def loader_up_command (self):
- self.io.loader ('u')
-
- def loader_down_command (self):
- self.io.loader ('d')
-
- def change_color (self, *dummy):
- pass
-
-if __name__ == '__main__':
- app = TestSimuControl (('../../asserv/src/asserv/asserv.host', '-m9',
- 'marcel'),
- ('../../mimot/src/dirty/dirty.host', '-m9', 'marcel'),
- ('../src/io.host'))
- app.mainloop ()
- app.close ()