From 2376aeb614d69d7050209f858818bfa7d36be398 Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Mon, 2 May 2011 23:32:51 +0200 Subject: digital/io: adapt FSM to new FSM system (AngFSM) --- digital/io/src/Makefile | 15 +- digital/io/src/ai.conf | 12 - digital/io/src/ai_hola_cb.c | 165 -- digital/io/src/ai_init_cb.c | 213 --- digital/io/src/ai_loader_cb.c | 636 -------- digital/io/src/ai_move_cb.c | 648 -------- digital/io/src/ai_top_cb.c | 434 ------ digital/io/src/fsm.c | 137 -- digital/io/src/fsm.h | 648 ++++++-- digital/io/src/fsm.host.c | 1571 ++++++++++++++++++++ digital/io/src/fsm_templates/template_cb.h | 10 - digital/io/src/fsm_templates/template_cb_decl.h | 6 - digital/io/src/fsm_templates/template_cb_impl.c | 9 - digital/io/src/fsm_templates/template_fsm.c | 55 - digital/io/src/fsm_templates/template_fsm.h | 52 - .../io/src/fsm_templates/template_hola_cb_skel.c | 11 - .../io/src/fsm_templates/template_init_cb_skel.c | 11 - .../io/src/fsm_templates/template_loader_cb_skel.c | 11 - .../io/src/fsm_templates/template_move_cb_skel.c | 11 - .../io/src/fsm_templates/template_top_cb_skel.c | 11 - digital/io/src/hola.c | 115 ++ digital/io/src/hola.fsm | 59 - digital/io/src/init.c | 206 ++- digital/io/src/init.fsm | 106 -- digital/io/src/loader.c | 613 +++++++- digital/io/src/loader.fsm | 227 --- digital/io/src/main.c | 79 +- digital/io/src/move.c | 605 +++++++- digital/io/src/move.fsm | 137 -- digital/io/src/top.c | 359 +++++ digital/io/src/top.fsm | 162 -- 31 files changed, 4060 insertions(+), 3274 deletions(-) delete mode 100644 digital/io/src/ai.conf delete mode 100644 digital/io/src/ai_hola_cb.c delete mode 100644 digital/io/src/ai_init_cb.c delete mode 100644 digital/io/src/ai_loader_cb.c delete mode 100644 digital/io/src/ai_move_cb.c delete mode 100644 digital/io/src/ai_top_cb.c delete mode 100644 digital/io/src/fsm.c create mode 100644 digital/io/src/fsm.host.c delete mode 100644 digital/io/src/fsm_templates/template_cb.h delete mode 100644 digital/io/src/fsm_templates/template_cb_decl.h delete mode 100644 digital/io/src/fsm_templates/template_cb_impl.c delete mode 100644 digital/io/src/fsm_templates/template_fsm.c delete mode 100644 digital/io/src/fsm_templates/template_fsm.h delete mode 100644 digital/io/src/fsm_templates/template_hola_cb_skel.c delete mode 100644 digital/io/src/fsm_templates/template_init_cb_skel.c delete mode 100644 digital/io/src/fsm_templates/template_loader_cb_skel.c delete mode 100644 digital/io/src/fsm_templates/template_move_cb_skel.c delete mode 100644 digital/io/src/fsm_templates/template_top_cb_skel.c delete mode 100644 digital/io/src/hola.fsm delete mode 100644 digital/io/src/init.fsm delete mode 100644 digital/io/src/loader.fsm delete mode 100644 digital/io/src/move.fsm delete mode 100644 digital/io/src/top.fsm (limited to 'digital/io/src') diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile index ceb7c206..8d1d70b8 100644 --- a/digital/io/src/Makefile +++ b/digital/io/src/Makefile @@ -7,7 +7,8 @@ io_SOURCES = main.c servo.avr.c eeprom.avr.c pwm.c \ switch.avr.c chrono.c main_timer.avr.c servo_pos.c \ twi_master.c asserv.c mimot.c \ simu.host.c contact.c usdist.c radar.c \ - path.c food.c events.host.c + path.c food.c events.host.c \ + fsm.host.c init.c move.c top.c hola.c loader.c fsm_AI_gen.avr.c # Modules needed for IO. MODULES = proto uart twi utils adc math/fixed math/geometry path/astar \ trace flash spi @@ -24,11 +25,13 @@ vpath %.c $(AI_MODULES:%=../../ai/src/%) vpath %.h $(AI_MODULES:%=../../ai/src/%) INCLUDES += -I. $(AI_MODULES:%=-I../../ai/src/%) -# FSM. -FSM = ai -FSM_SOURCES := top init move loader hola -# Include FSM makefile. -include $(BASE)/make/Makefile.fsm +include $(BASE)/make/Makefile.gen + +#FSM generation +obj/main.avr.o: fsm_AI_gen.h +fsm_AI_gen.avr.c fsm_AI_gen.h: host + ./io.host --gen + mv fsm_AI_gen.c fsm_AI_gen.avr.c events.h events.host.c: trace.trc PYTHONPATH=$(BASE)/../../host python $(BASE)/../../tools/trace/trace.py -t create -i $< -o diff --git a/digital/io/src/ai.conf b/digital/io/src/ai.conf deleted file mode 100644 index ce7f6374..00000000 --- a/digital/io/src/ai.conf +++ /dev/null @@ -1,12 +0,0 @@ -[user] -template-dir = fsm_templates - -[templates] -template_fsm.h = %_fsm.h -template_fsm.c = %_fsm.c -template_cb.h = %_cb.h -template_top_cb_skel.c = %_top_cb_skel.c -template_init_cb_skel.c = %_init_cb_skel.c -template_move_cb_skel.c = %_move_cb_skel.c -template_loader_cb_skel.c = %_loader_cb_skel.c -template_hola_cb_skel.c = %_hola_cb_skel.c diff --git a/digital/io/src/ai_hola_cb.c b/digital/io/src/ai_hola_cb.c deleted file mode 100644 index 6b76b058..00000000 --- a/digital/io/src/ai_hola_cb.c +++ /dev/null @@ -1,165 +0,0 @@ -/* ai_hola_cb.c */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2010 Nicolas Schodet - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * }}} */ -#include "common.h" -#include "fsm.h" -#include "ai_cb.h" -#include "asserv.h" -#include "mimot.h" -#include "bot.h" -#include "playground.h" - -#include "modules/math/fixed/fixed.h" - -uint32_t hola_angle; -int16_t hola_timeout; - -/* - * HOLA_IDLE =start=> - * => HOLA_WAIT_HOLA - */ -fsm_branch_t -ai__HOLA_IDLE__start (void) -{ - return ai_next (HOLA_IDLE, start); -} - -/* - * HOLA_WAIT_HOLA =hola_start=> - * => HOLA_WAIT_JACK_IN - */ -fsm_branch_t -ai__HOLA_WAIT_HOLA__hola_start (void) -{ - return ai_next (HOLA_WAIT_HOLA, hola_start); -} - -/* - * HOLA_WAIT_JACK_IN =jack_inserted_into_bot=> - * => HOLA_WAIT_JACK_OUT - */ -fsm_branch_t -ai__HOLA_WAIT_JACK_IN__jack_inserted_into_bot (void) -{ - return ai_next (HOLA_WAIT_JACK_IN, jack_inserted_into_bot); -} - -/* - * HOLA_WAIT_JACK_OUT =jack_removed_from_bot=> - * => HOLA_ROUNDS - */ -fsm_branch_t -ai__HOLA_WAIT_JACK_OUT__jack_removed_from_bot (void) -{ - return ai_next (HOLA_WAIT_JACK_OUT, jack_removed_from_bot); -} - -/* - * HOLA_ROUNDS =asserv_last_cmd_ack=> - * no_timeout => HOLA_ROUNDS - * move clamp rounds - * timeout => HOLA_UP - * move up - * rotate - */ -fsm_branch_t -ai__HOLA_ROUNDS__asserv_last_cmd_ack (void) -{ - if (++hola_timeout < 150) - { - int32_t cos = fixed_cos_f824 (hola_angle) + 0x1000000; - int32_t sin = fixed_sin_f824 (hola_angle) + 0x1000000; - int32_t dx = BOT_CLAMP_OPEN_STEP - + fixed_mul_f824 (BOT_CLAMP_STROKE_STEP / 3 / 2, cos); - int32_t dy = BOT_ELEVATOR_ZERO_SPEED - + fixed_mul_f824 (BOT_ELEVATOR_REST_STEP / 3 / 2, sin); - mimot_move_motor0_absolute (dx, BOT_CLAMP_SPEED); - mimot_move_motor1_absolute (dx, BOT_CLAMP_SPEED); - asserv_move_motor0_absolute (dy, BOT_ELEVATOR_SPEED); - hola_angle += 0x1000000 / (225 / 12); - return ai_next_branch (HOLA_ROUNDS, asserv_last_cmd_ack, no_timeout); - } - else - { - asserv_set_speed (0x10, 0x1c, 0x10, 0x1c); - asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED / 3); - asserv_move_angularly (POSITION_A_DEG (174)); - return ai_next_branch (HOLA_ROUNDS, asserv_last_cmd_ack, timeout); - } -} - -/* - * HOLA_UP =bot_move_succeed=> - * => HOLA_CENTER - * rotate to center - */ -fsm_branch_t -ai__HOLA_UP__bot_move_succeed (void) -{ - asserv_set_speed (0x10, 0x20, 0x10, 0x20); - asserv_move_angularly (POSITION_A_DEG (-90)); - asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP / 2, BOT_ELEVATOR_SPEED); - return ai_next (HOLA_UP, bot_move_succeed); -} - -/* - * HOLA_CENTER =bot_move_succeed=> - * => HOLA_CLAP_CLOSE - * close clamp - */ -fsm_branch_t -ai__HOLA_CENTER__bot_move_succeed (void) -{ - mimot_motor0_clamp (BOT_CLAMP_ZERO_SPEED, 0); - mimot_motor1_clamp (BOT_CLAMP_ZERO_SPEED, 0); - return ai_next (HOLA_CENTER, bot_move_succeed); -} - -/* - * HOLA_CLAP_CLOSE =clamp_succeed=> - * => HOLA_CLAP_OPEN - * open clamp - */ -fsm_branch_t -ai__HOLA_CLAP_CLOSE__clamp_succeed (void) -{ - mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); - mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); - return ai_next (HOLA_CLAP_CLOSE, clamp_succeed); -} - -/* - * HOLA_CLAP_OPEN =clamp_succeed=> - * => HOLA_CLAP_CLOSE - * close clamp - */ -fsm_branch_t -ai__HOLA_CLAP_OPEN__clamp_succeed (void) -{ - mimot_motor0_clamp (BOT_CLAMP_SPEED, 0); - mimot_motor1_clamp (BOT_CLAMP_SPEED, 0); - return ai_next (HOLA_CLAP_OPEN, clamp_succeed); -} - - diff --git a/digital/io/src/ai_init_cb.c b/digital/io/src/ai_init_cb.c deleted file mode 100644 index 444b867b..00000000 --- a/digital/io/src/ai_init_cb.c +++ /dev/null @@ -1,213 +0,0 @@ -/* init_cb.c - init FSM callbacks. */ -/* {{{ - * - * Copyright (C) 2009 Nicolas Haller - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * }}} - * init: - * place the bot in the start position for a match. - */ -#include "common.h" -#include "fsm.h" -#include "ai_cb.h" -#include "asserv.h" -#include "init.h" -#include "playground.h" -#include "main.h" -#include "bot.h" -#include "switch.h" -#include "chrono.h" -#include "playground.h" -#include "modules/trace/trace.h" -#include "modules/path/path.h" - -/* - * INIT_IDLE =start=> - * => INIT_WAIT_FIRST_JACK_IN - * do nothing. - */ -fsm_branch_t -ai__INIT_IDLE__start (void) -{ - return ai_next (INIT_IDLE, start); -} - -/* - * INIT_WAIT_FIRST_JACK_IN =jack_inserted_into_bot=> - * => INIT_WAIT_FIRST_JACK_OUT - * do nothing. - */ -fsm_branch_t -ai__INIT_WAIT_FIRST_JACK_IN__jack_inserted_into_bot (void) -{ - return ai_next (INIT_WAIT_FIRST_JACK_IN, jack_inserted_into_bot); -} - -/* - * INIT_WAIT_FIRST_JACK_OUT =jack_removed_from_bot=> - * => INIT_WAIT_SECOND_JACK_IN - * start trace module. - * get and store the color of the bot. - */ -fsm_branch_t -ai__INIT_WAIT_FIRST_JACK_OUT__jack_removed_from_bot (void) -{ - /* Initialize trace module (erase the flash). */ - trace_init (); - /* Get the color. */ - bot_color = switch_get_color (); - return ai_next (INIT_WAIT_FIRST_JACK_OUT, jack_removed_from_bot); -} - -/* - * INIT_WAIT_SECOND_JACK_IN =jack_inserted_into_bot=> - * => INIT_WAIT_FOR_HANDS_OUT - * do nothing. - */ -fsm_branch_t -ai__INIT_WAIT_SECOND_JACK_IN__jack_inserted_into_bot (void) -{ - return ai_next (INIT_WAIT_SECOND_JACK_IN, jack_inserted_into_bot); -} - -/* - * INIT_WAIT_FOR_HANDS_OUT =state_timeout=> - * => INIT_GO_TO_THE_WALL - * fuck the wall in front. - */ -fsm_branch_t -ai__INIT_WAIT_FOR_HANDS_OUT__state_timeout (void) -{ - /* Go to the wall, no backward. */ - asserv_go_to_the_wall (0); - return ai_next (INIT_WAIT_FOR_HANDS_OUT, state_timeout); -} - -/* - * INIT_GO_TO_THE_WALL =bot_move_succeed=> - * => INIT_GO_AWAY_FROM_THE_WALL - * reset the Y position of the bot. - * reset the angular position of the bot. - * move away from the wall (linear move). - */ -fsm_branch_t -ai__INIT_GO_TO_THE_WALL__bot_move_succeed (void) -{ - asserv_set_y_position (PG_Y (PG_LENGTH - BOT_SIZE_FRONT)); - asserv_set_angle_position (PG_A_DEG (90)); - /* Move away from the border. */ - asserv_move_linearly (- INIT_DIST); - return ai_next (INIT_GO_TO_THE_WALL, bot_move_succeed); -} - -/* - * INIT_GO_AWAY_FROM_THE_WALL =bot_move_succeed=> - * => INIT_FACE_OTHER_WALL - * turn to face the other wall. - */ -fsm_branch_t -ai__INIT_GO_AWAY_FROM_THE_WALL__bot_move_succeed (void) -{ - /* Face the other wall. */ - asserv_goto_angle (PG_A_DEG (180)); - return ai_next (INIT_GO_AWAY_FROM_THE_WALL, bot_move_succeed); -} - -/* - * INIT_FACE_OTHER_WALL =bot_move_succeed=> - * => INIT_WAIT_AFTER_ROTATION - * nothing to do. - */ -fsm_branch_t -ai__INIT_FACE_OTHER_WALL__bot_move_succeed (void) -{ - return ai_next (INIT_FACE_OTHER_WALL, bot_move_succeed); -} - -/* - * INIT_WAIT_AFTER_ROTATION =state_timeout=> - * => INIT_GO_TO_THE_WALL_AGAIN - * fuck the wall in front. - */ -fsm_branch_t -ai__INIT_WAIT_AFTER_ROTATION__state_timeout (void) -{ - /* Go to the wall, no backward. */ - asserv_go_to_the_wall (0); - return ai_next (INIT_WAIT_AFTER_ROTATION, state_timeout); -} - -/* - * INIT_GO_TO_THE_WALL_AGAIN =bot_move_succeed=> - * => INIT_GO_AWAY_FROM_THE_WALL_AGAIN - * reset the X position of the bot. - * move away from the wall (linear move). - */ -fsm_branch_t -ai__INIT_GO_TO_THE_WALL_AGAIN__bot_move_succeed (void) -{ - asserv_set_x_position (PG_X (BOT_SIZE_FRONT)); - /* Move away from the border. */ - asserv_move_linearly (- INIT_DIST); - return ai_next (INIT_GO_TO_THE_WALL_AGAIN, bot_move_succeed); -} - -/* - * INIT_GO_AWAY_FROM_THE_WALL_AGAIN =bot_move_succeed=> - * => INIT_GO_TO_START_POSITION - * go to the start position with a go to movement. - */ -fsm_branch_t -ai__INIT_GO_AWAY_FROM_THE_WALL_AGAIN__bot_move_succeed (void) -{ - /* Move away from the border. */ - asserv_goto_xya (BOT_INIT_XYA, 0); - return ai_next (INIT_GO_AWAY_FROM_THE_WALL_AGAIN, bot_move_succeed); -} - -/* - * INIT_GO_TO_START_POSITION =bot_move_succeed=> - * => INIT_WAIT_SECOND_JACK_OUT - * nothing to do, the bot is at the start position. - */ -fsm_branch_t -ai__INIT_GO_TO_START_POSITION__bot_move_succeed (void) -{ - return ai_next (INIT_GO_TO_START_POSITION, bot_move_succeed); -} - -/* - * INIT_WAIT_SECOND_JACK_OUT =jack_removed_from_bot=> - * => INIT_IDLE - * tell other FSM the match begins. - * start the chrono. - */ -fsm_branch_t -ai__INIT_WAIT_SECOND_JACK_OUT__jack_removed_from_bot (void) -{ - /* Set the flag to transmit to other FSM. */ - init_match_is_started = 1; - /* Start the chrono. */ - chrono_init (); - return ai_next (INIT_WAIT_SECOND_JACK_OUT, jack_removed_from_bot); -} - - diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c deleted file mode 100644 index fdb628a9..00000000 --- a/digital/io/src/ai_loader_cb.c +++ /dev/null @@ -1,636 +0,0 @@ -/* ai_loader_cb.c */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2010 Nicolas Schodet - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * }}} */ -#include "common.h" -#include "fsm.h" -#include "ai_cb.h" -#include "asserv.h" -#include "mimot.h" -#include "bot.h" -#include "main.h" -#include "food.h" -#include "loader.h" - -/* - * LOADER_IDLE =start=> - * => LOADER_WAIT_JACK_IN - */ -fsm_branch_t -ai__LOADER_IDLE__start (void) -{ - return ai_next (LOADER_IDLE, start); -} - -/* - * LOADER_WAIT_JACK_IN =jack_inserted_into_bot=> - * => LOADER_WAIT_JACK_OUT - */ -fsm_branch_t -ai__LOADER_WAIT_JACK_IN__jack_inserted_into_bot (void) -{ - return ai_next (LOADER_WAIT_JACK_IN, jack_inserted_into_bot); -} - -/* - * LOADER_WAIT_JACK_IN =hola_start=> - * => LOADER_IDLE - */ -fsm_branch_t -ai__LOADER_WAIT_JACK_IN__hola_start (void) -{ - return ai_next (LOADER_WAIT_JACK_IN, hola_start); -} - -/* - * LOADER_WAIT_JACK_OUT =jack_removed_from_bot=> - * => LOADER_INIT_ELEVATOR_ZERO - * find elevator zero - * close clamp - * find gate zero - */ -fsm_branch_t -ai__LOADER_WAIT_JACK_OUT__jack_removed_from_bot (void) -{ - asserv_motor0_zero_position (-BOT_ELEVATOR_ZERO_SPEED); - mimot_motor0_clamp (BOT_CLAMP_ZERO_SPEED, 0); - mimot_motor1_clamp (BOT_CLAMP_ZERO_SPEED, 0); - asserv_motor1_zero_position (-BOT_GATE_SPEED); - return ai_next (LOADER_WAIT_JACK_OUT, jack_removed_from_bot); -} - -/* - * LOADER_INIT_ELEVATOR_ZERO =elevator_succeed=> - * => LOADER_INIT_CLAMP_CLOSE - */ -fsm_branch_t -ai__LOADER_INIT_ELEVATOR_ZERO__elevator_succeed (void) -{ - return ai_next (LOADER_INIT_ELEVATOR_ZERO, elevator_succeed); -} - -/* - * LOADER_INIT_CLAMP_CLOSE =clamp_succeed=> - * => LOADER_INIT_CLAMP_ZERO - * move elevator up - * find clamp zero - */ -fsm_branch_t -ai__LOADER_INIT_CLAMP_CLOSE__clamp_succeed (void) -{ - asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, - BOT_ELEVATOR_ZERO_SPEED); - mimot_motor0_zero_position (-BOT_CLAMP_ZERO_SPEED); - mimot_motor1_zero_position (-BOT_CLAMP_ZERO_SPEED); - return ai_next (LOADER_INIT_CLAMP_CLOSE, clamp_succeed); -} - -/* - * LOADER_INIT_CLAMP_ZERO =clamp_succeed=> - * => LOADER_INIT_GATE_ZERO - */ -fsm_branch_t -ai__LOADER_INIT_CLAMP_ZERO__clamp_succeed (void) -{ - return ai_next (LOADER_INIT_CLAMP_ZERO, clamp_succeed); -} - -/* - * LOADER_INIT_GATE_ZERO =gate_succeed=> - * => LOADER_INIT_ELEVATOR_UP - */ -fsm_branch_t -ai__LOADER_INIT_GATE_ZERO__gate_succeed (void) -{ - return ai_next (LOADER_INIT_GATE_ZERO, gate_succeed); -} - -/* - * LOADER_INIT_ELEVATOR_UP =elevator_succeed=> - * => LOADER_INIT_GATE_WAIT - */ -fsm_branch_t -ai__LOADER_INIT_ELEVATOR_UP__elevator_succeed (void) -{ - return ai_next (LOADER_INIT_ELEVATOR_UP, elevator_succeed); -} - -/* - * LOADER_INIT_ELEVATOR_UP =elevator_failed=> - * => LOADER_IDLE - * initialisation failure - */ -fsm_branch_t -ai__LOADER_INIT_ELEVATOR_UP__elevator_failed (void) -{ - /* Move so that the operator notice it. */ - asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP / 3, - BOT_ELEVATOR_ZERO_SPEED); - return ai_next (LOADER_INIT_ELEVATOR_UP, elevator_failed); -} - -/* - * LOADER_INIT_GATE_WAIT =state_timeout=> - * => LOADER_INIT_GATE_CLOSE - * close gate - */ -fsm_branch_t -ai__LOADER_INIT_GATE_WAIT__state_timeout (void) -{ - asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED); - return ai_next (LOADER_INIT_GATE_WAIT, state_timeout); -} - -/* - * LOADER_INIT_GATE_CLOSE =gate_succeed=> - * => LOADER_UP - */ -fsm_branch_t -ai__LOADER_INIT_GATE_CLOSE__gate_succeed (void) -{ - return ai_next (LOADER_INIT_GATE_CLOSE, gate_succeed); -} - -/* - * LOADER_UP =loader_down=> - * => LOADER_DOWNING - * move down - */ -fsm_branch_t -ai__LOADER_UP__loader_down (void) -{ - asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED); - return ai_next (LOADER_UP, loader_down); -} - -/* - * LOADER_DOWN =loader_up=> - * => LOADER_UPING - * move up - */ -fsm_branch_t -ai__LOADER_DOWN__loader_up (void) -{ - asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED); - return ai_next (LOADER_DOWN, loader_up); -} - -/* - * LOADER_DOWN =loader_element=> - * => LOADER_LOAD_CLOSING - * clamp - */ -fsm_branch_t -ai__LOADER_DOWN__loader_element (void) -{ - mimot_motor0_clamp (BOT_CLAMP_SPEED, BOT_CLAMP_PWM); - mimot_motor1_clamp (BOT_CLAMP_SPEED, BOT_CLAMP_PWM); - return ai_next (LOADER_DOWN, loader_element); -} - -/* - * LOADER_UPING =elevator_succeed=> - * => LOADER_UP - * post loader_uped event - */ -fsm_branch_t -ai__LOADER_UPING__elevator_succeed (void) -{ - main_post_event (AI_EVENT_loader_uped); - return ai_next (LOADER_UPING, elevator_succeed); -} - -/* - * LOADER_UPING =elevator_failed=> - * => LOADER_ERROR - * post loader_errored event - */ -fsm_branch_t -ai__LOADER_UPING__elevator_failed (void) -{ - main_post_event (AI_EVENT_loader_errored); - return ai_next (LOADER_UPING, elevator_failed); -} - -/* - * LOADER_UPING =loader_down=> - * => LOADER_DOWNING - * move down - */ -fsm_branch_t -ai__LOADER_UPING__loader_down (void) -{ - asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED); - return ai_next (LOADER_UPING, loader_down); -} - -/* - * LOADER_DOWNING =elevator_succeed=> - * => LOADER_DOWN - * release elevator motor - * post loader_downed event - */ -fsm_branch_t -ai__LOADER_DOWNING__elevator_succeed (void) -{ - asserv_motor0_free (); - main_post_event (AI_EVENT_loader_downed); - return ai_next (LOADER_DOWNING, elevator_succeed); -} - -/* - * LOADER_DOWNING =elevator_failed=> - * => LOADER_ERROR - * post loader_errored event - */ -fsm_branch_t -ai__LOADER_DOWNING__elevator_failed (void) -{ - main_post_event (AI_EVENT_loader_errored); - return ai_next (LOADER_DOWNING, elevator_failed); -} - -/* - * LOADER_DOWNING =loader_element=> - * => LOADER_ERROR - * emergency stop - * post loader_errored event - */ -fsm_branch_t -ai__LOADER_DOWNING__loader_element (void) -{ - asserv_motor0_free (); - main_post_event (AI_EVENT_loader_errored); - return ai_next (LOADER_DOWNING, loader_element); -} - -/* - * LOADER_DOWNING =loader_up=> - * => LOADER_UPING - * move up - */ -fsm_branch_t -ai__LOADER_DOWNING__loader_up (void) -{ - asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED); - return ai_next (LOADER_DOWNING, loader_up); -} - -/* - * LOADER_ERROR =loader_down=> - * => LOADER_ERROR_DOWNING - * move down - */ -fsm_branch_t -ai__LOADER_ERROR__loader_down (void) -{ - asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED); - return ai_next (LOADER_ERROR, loader_down); -} - -/* - * LOADER_ERROR =loader_up=> - * => LOADER_ERROR_UPING - * move up - */ -fsm_branch_t -ai__LOADER_ERROR__loader_up (void) -{ - asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP, BOT_ELEVATOR_SPEED); - return ai_next (LOADER_ERROR, loader_up); -} - -/* - * LOADER_ERROR_DOWNING =elevator_succeed=> - * => LOADER_ERROR_DOWNING_OPEN - * release elevator motor - * open clamp - */ -fsm_branch_t -ai__LOADER_ERROR_DOWNING__elevator_succeed (void) -{ - asserv_motor0_free (); - mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); - mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); - return ai_next (LOADER_ERROR_DOWNING, elevator_succeed); -} - -/* - * LOADER_ERROR_DOWNING =elevator_failed=> - * => LOADER_ERROR - * post loader_errored event - */ -fsm_branch_t -ai__LOADER_ERROR_DOWNING__elevator_failed (void) -{ - main_post_event (AI_EVENT_loader_errored); - return ai_next (LOADER_ERROR_DOWNING, elevator_failed); -} - -/* - * LOADER_ERROR_DOWNING =state_timeout=> - * => LOADER_ERROR - * post loader_errored event - */ -fsm_branch_t -ai__LOADER_ERROR_DOWNING__state_timeout (void) -{ - main_post_event (AI_EVENT_loader_errored); - return ai_next (LOADER_ERROR_DOWNING, state_timeout); -} - -/* - * LOADER_ERROR_DOWNING_OPEN =clamp_succeed=> - * => LOADER_DOWN - * post loader_downed event - */ -fsm_branch_t -ai__LOADER_ERROR_DOWNING_OPEN__clamp_succeed (void) -{ - main_post_event (AI_EVENT_loader_downed); - return ai_next (LOADER_ERROR_DOWNING_OPEN, clamp_succeed); -} - -/* - * LOADER_ERROR_DOWNING_OPEN =clamp_failed=> - * => LOADER_ERROR - * post loader_errored event - */ -fsm_branch_t -ai__LOADER_ERROR_DOWNING_OPEN__clamp_failed (void) -{ - main_post_event (AI_EVENT_loader_errored); - return ai_next (LOADER_ERROR_DOWNING_OPEN, clamp_failed); -} - -/* - * LOADER_ERROR_DOWNING_OPEN =state_timeout=> - * => LOADER_ERROR - * post loader_errored event - */ -fsm_branch_t -ai__LOADER_ERROR_DOWNING_OPEN__state_timeout (void) -{ - main_post_event (AI_EVENT_loader_errored); - return ai_next (LOADER_ERROR_DOWNING_OPEN, state_timeout); -} - -/* - * LOADER_ERROR_UPING =elevator_succeed=> - * => LOADER_ERROR_UPING_OPEN - * open clamp - */ -fsm_branch_t -ai__LOADER_ERROR_UPING__elevator_succeed (void) -{ - mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); - mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); - return ai_next (LOADER_ERROR_UPING, elevator_succeed); -} - -/* - * LOADER_ERROR_UPING =elevator_failed=> - * => LOADER_ERROR - * post loader_errored event - */ -fsm_branch_t -ai__LOADER_ERROR_UPING__elevator_failed (void) -{ - main_post_event (AI_EVENT_loader_errored); - return ai_next (LOADER_ERROR_UPING, elevator_failed); -} - -/* - * LOADER_ERROR_UPING =state_timeout=> - * => LOADER_ERROR - * post loader_errored event - */ -fsm_branch_t -ai__LOADER_ERROR_UPING__state_timeout (void) -{ - main_post_event (AI_EVENT_loader_errored); - return ai_next (LOADER_ERROR_UPING, state_timeout); -} - -/* - * LOADER_ERROR_UPING_OPEN =clamp_succeed=> - * => LOADER_UP - * post loader_uped event - */ -fsm_branch_t -ai__LOADER_ERROR_UPING_OPEN__clamp_succeed (void) -{ - main_post_event (AI_EVENT_loader_uped); - return ai_next (LOADER_ERROR_UPING_OPEN, clamp_succeed); -} - -/* - * LOADER_ERROR_UPING_OPEN =clamp_failed=> - * => LOADER_ERROR - * post loader_errored event - */ -fsm_branch_t -ai__LOADER_ERROR_UPING_OPEN__clamp_failed (void) -{ - main_post_event (AI_EVENT_loader_errored); - return ai_next (LOADER_ERROR_UPING_OPEN, clamp_failed); -} - -/* - * LOADER_ERROR_UPING_OPEN =state_timeout=> - * => LOADER_ERROR - * post loader_errored event - */ -fsm_branch_t -ai__LOADER_ERROR_UPING_OPEN__state_timeout (void) -{ - main_post_event (AI_EVENT_loader_errored); - return ai_next (LOADER_ERROR_UPING_OPEN, state_timeout); -} - -/* - * LOADER_LOAD_CLOSING =clamp_succeed=> - * full => LOADER_LOAD_UPING - * move up - * count one element - * empty => LOADER_LOAD_EMPTY_OPEN - * open clamp - */ -fsm_branch_t -ai__LOADER_LOAD_CLOSING__clamp_succeed (void) -{ - /* Measure load using clamp position. */ - uint16_t tickness = BOT_CLAMP_WIDTH_STEP - - mimot_get_motor0_position () - - mimot_get_motor1_position (); - if (tickness > BOT_CLAMP_EMPTY_STEP) - { - asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP, - BOT_ELEVATOR_SPEED); - position_t robot_position; - asserv_get_position (&robot_position); - food_taken (robot_position); - loader_elements++; - return ai_next_branch (LOADER_LOAD_CLOSING, clamp_succeed, full); - } - else - { - mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); - mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); - return ai_next_branch (LOADER_LOAD_CLOSING, clamp_succeed, empty); - } -} - -/* - * LOADER_LOAD_UPING =elevator_unload_position=> - * => LOADER_LOAD_UNLOADING - * open clamp - */ -fsm_branch_t -ai__LOADER_LOAD_UPING__elevator_unload_position (void) -{ - mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); - mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); - return ai_next (LOADER_LOAD_UPING, elevator_unload_position); -} - -/* - * LOADER_LOAD_UPING =elevator_failed=> - * => LOADER_ERROR - * post loader_errored or loader_black event - * remove one element - * open clamp - */ -fsm_branch_t -ai__LOADER_LOAD_UPING__elevator_failed (void) -{ - if (asserv_get_motor0_position () < BOT_ELEVATOR_BLACK_THRESHOLD_STEP) - { - if (loader_elements) - loader_elements--; - main_post_event (AI_EVENT_loader_black); - } - else - main_post_event (AI_EVENT_loader_errored); - mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); - mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); - return ai_next (LOADER_LOAD_UPING, elevator_failed); -} - -/* - * LOADER_LOAD_UNLOADING =elevator_succeed=> - * => LOADER_LOAD_UNLOADING_OPEN - */ -fsm_branch_t -ai__LOADER_LOAD_UNLOADING__elevator_succeed (void) -{ - return ai_next (LOADER_LOAD_UNLOADING, elevator_succeed); -} - -/* - * LOADER_LOAD_UNLOADING =elevator_failed=> - * => LOADER_ERROR - * post loader_errored event - * open clamp - */ -fsm_branch_t -ai__LOADER_LOAD_UNLOADING__elevator_failed (void) -{ - main_post_event (AI_EVENT_loader_errored); - mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); - mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); - return ai_next (LOADER_LOAD_UNLOADING, elevator_failed); -} - -/* - * LOADER_LOAD_UNLOADING_OPEN =clamp_succeed=> - * down => LOADER_DOWNING - * move down - * up => LOADER_UPING - * move up - */ -fsm_branch_t -ai__LOADER_LOAD_UNLOADING_OPEN__clamp_succeed (void) -{ - if (loader_want_up) - { - asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, - BOT_ELEVATOR_SPEED); - return ai_next_branch (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed, up); - } - else - { - asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, - BOT_ELEVATOR_SPEED); - return ai_next_branch (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed, down); - } -} - -/* - * LOADER_LOAD_UNLOADING_OPEN =clamp_failed=> - * => LOADER_ERROR - * post loader_errored event - */ -fsm_branch_t -ai__LOADER_LOAD_UNLOADING_OPEN__clamp_failed (void) -{ - main_post_event (AI_EVENT_loader_errored); - return ai_next (LOADER_LOAD_UNLOADING_OPEN, clamp_failed); -} - -/* - * LOADER_LOAD_EMPTY_OPEN =clamp_succeed=> - * down => LOADER_DOWN - * post loader_downed event - * up => LOADER_UPING - * move up - */ -fsm_branch_t -ai__LOADER_LOAD_EMPTY_OPEN__clamp_succeed (void) -{ - if (loader_want_up) - { - asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, - BOT_ELEVATOR_SPEED); - return ai_next_branch (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, up); - } - else - { - main_post_event (AI_EVENT_loader_downed); - return ai_next_branch (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, down); - } -} - -/* - * LOADER_LOAD_EMPTY_OPEN =clamp_failed=> - * => LOADER_ERROR - * post loader_errored event - */ -fsm_branch_t -ai__LOADER_LOAD_EMPTY_OPEN__clamp_failed (void) -{ - main_post_event (AI_EVENT_loader_errored); - return ai_next (LOADER_LOAD_EMPTY_OPEN, clamp_failed); -} - - diff --git a/digital/io/src/ai_move_cb.c b/digital/io/src/ai_move_cb.c deleted file mode 100644 index 32cdd221..00000000 --- a/digital/io/src/ai_move_cb.c +++ /dev/null @@ -1,648 +0,0 @@ -/* move_cb.c */ -/* {{{ - * - * Copyright (C) 2008 Nélio Laranjeiro - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * }}} - * move: - * move with avoid obstacle. - */ -#include "common.h" -#include "fsm.h" -#include "ai_cb.h" - -#include "asserv.h" -#include "playground.h" -#include "move.h" -#include "bot.h" -#include "radar.h" -#include "events.h" -#include "loader.h" - -#include "main.h" /* main_post_event_for_top_fsm */ -#include "modules/math/fixed/fixed.h" /* fixed_* */ -#include "modules/path/path.h" -#include "modules/utils/utils.h" -#include "modules/trace/trace.h" - -#include "debug.host.h" - -#include - -#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 (¤t_pos); - /* Give the current position of the bot to the path module */ - path_endpoints (current_pos.v, move_data.final.v); - /* Update the path module */ - move_data.slow = 0; - path_update (); - found = path_get_next (&dst); - /* If not found, try to escape. */ - if (!found) - { - move_data.slow = 1; - path_escape (8); - path_update (); - found = path_get_next (&dst); - } - /* If not on the grid, slow down. */ - int16_t mx = current_pos.v.x % MOVE_GRID_X; - int16_t my = current_pos.v.y % MOVE_GRID_Y; - if (mx < MOVE_GRID_X_ORIGIN - MOVE_GRID_MARGIN - || mx > MOVE_GRID_X_ORIGIN + MOVE_GRID_MARGIN - || my < MOVE_GRID_Y_ORIGIN - MOVE_GRID_MARGIN - || my > MOVE_GRID_Y_ORIGIN + MOVE_GRID_MARGIN - ) - move_data.slow = 1; - /* If found, go. */ - if (found) - { - return move_go_to_next (dst); - } - else - { - /* Error, not final move. */ - move_data.final_move = 0; - return 0; - } -} - -/** Go to next position in path. Returns 1 for linear move, 2 for angular - * move. */ -static uint8_t -move_path_next (void) -{ - vect_t dst; - path_get_next (&dst); - return move_go_to_next (dst); -} - -/* - * MOVE_IDLE =move_start=> - * path_found_rotate => MOVE_ROTATING - * rotate towards next position. - * path_found => MOVE_MOVING - * move to next position. - * no_path_found => MOVE_IDLE - * post failure event. - */ -fsm_branch_t -ai__MOVE_IDLE__move_start (void) -{ - uint8_t next = move_path_init (); - if (next) - { - if (next == 2) - return ai_next_branch (MOVE_IDLE, move_start, path_found_rotate); - else - return ai_next_branch (MOVE_IDLE, move_start, path_found); - } - else - { - main_post_event (AI_EVENT_move_fsm_failed); - return ai_next_branch (MOVE_IDLE, move_start, no_path_found); - } -} - -/* - * MOVE_ROTATING =bot_move_succeed=> - * => MOVE_MOVING - * move to next position. - */ -fsm_branch_t -ai__MOVE_ROTATING__bot_move_succeed (void) -{ - loader_down (); - move_go (); - return ai_next (MOVE_ROTATING, bot_move_succeed); -} - -/* - * MOVE_ROTATING =bot_move_failed=> - * => MOVE_MOVING - * move to next position. - */ -fsm_branch_t -ai__MOVE_ROTATING__bot_move_failed (void) -{ - ai__MOVE_ROTATING__bot_move_succeed (); - return ai_next (MOVE_ROTATING, bot_move_failed); -} - -/* - * MOVE_ROTATING =state_timeout=> - * => MOVE_MOVING - * move to next position. - */ -fsm_branch_t -ai__MOVE_ROTATING__state_timeout (void) -{ - ai__MOVE_ROTATING__bot_move_succeed (); - return ai_next (MOVE_ROTATING, state_timeout); -} - -/* - * MOVE_ROTATING =loader_errored=> - * => MOVE_LOADER_UNBLOCKING_UPING - * move backward - * loader up - */ -fsm_branch_t -ai__MOVE_ROTATING__loader_errored (void) -{ - asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE); - loader_up (); - return ai_next (MOVE_ROTATING, loader_errored); -} - -/* - * MOVE_MOVING =bot_move_succeed=> - * done => MOVE_IDLE - * post success event. - * path_found_rotate => MOVE_ROTATING - * rotate towards next position. - * path_found => MOVE_MOVING - * move to next position. - * no_path_found => MOVE_IDLE - * post failure event. - */ -fsm_branch_t -ai__MOVE_MOVING__bot_move_succeed (void) -{ - if (move_data.final_move) - { - main_post_event (AI_EVENT_move_fsm_succeed); - return ai_next_branch (MOVE_MOVING, bot_move_succeed, done); - } - else - { - uint8_t next = move_path_next (); - if (next == 2) - return ai_next_branch (MOVE_MOVING, bot_move_succeed, path_found_rotate); - else - return ai_next_branch (MOVE_MOVING, bot_move_succeed, path_found); - } - //return ai_next_branch (MOVE_MOVING, bot_move_succeed, no_path_found); -} - -/* - * MOVE_MOVING =bot_move_failed=> - * => MOVE_MOVING_BACKWARD_TO_TURN_FREELY - * reset final_move. - * move backward to turn freely. - */ -fsm_branch_t -ai__MOVE_MOVING__bot_move_failed (void) -{ - move_data.final_move = 0; - /* Assume there is an obstacle in front of the robot. */ - position_t robot_pos; - asserv_get_position (&robot_pos); - vect_t obstacle_pos; - int16_t dist = asserv_get_last_moving_direction () == 1 - ? BOT_SIZE_FRONT + MOVE_REAL_OBSTACLE_RADIUS - : -(BOT_SIZE_BACK + MOVE_REAL_OBSTACLE_RADIUS); - vect_from_polar_uf016 (&obstacle_pos, dist, robot_pos.a); - vect_translate (&obstacle_pos, &robot_pos.v); - path_obstacle (0, obstacle_pos, MOVE_OBSTACLE_RADIUS, 0, - MOVE_OBSTACLE_VALIDITY); - /* Move backward to turn freely. */ - asserv_move_linearly (asserv_get_last_moving_direction () == 1 ? - - 300 : 300); - return ai_next (MOVE_MOVING, bot_move_failed); -} - -/* - * MOVE_MOVING =state_timeout=> - * => MOVE_MOVING_BACKWARD_TO_TURN_FREELY - * reset final_move. - * move backward to turn freely. - */ -fsm_branch_t -ai__MOVE_MOVING__state_timeout (void) -{ - ai__MOVE_MOVING__bot_move_failed (); - return ai_next (MOVE_MOVING, state_timeout); -} - -/* - * MOVE_MOVING =obstacle_in_front=> - * tryagain => MOVE_WAIT_FOR_CLEAR_PATH - * reset final_move. - * stop the bot. - * tryout => MOVE_IDLE - * stop the bot. - * post failure event. - */ -fsm_branch_t -ai__MOVE_MOVING__obstacle_in_front (void) -{ - move_data.final_move = 0; - asserv_stop_motor (); - if (--move_data.try_again_counter == 0) - { - main_post_event (AI_EVENT_move_fsm_failed); - return ai_next_branch (MOVE_MOVING, obstacle_in_front, tryout); - } - else - return ai_next_branch (MOVE_MOVING, obstacle_in_front, tryagain); -} - -/* - * MOVE_MOVING =loader_errored=> - * => MOVE_LOADER_UNBLOCKING_UPING - * move backward - * loader up - */ -fsm_branch_t -ai__MOVE_MOVING__loader_errored (void) -{ - asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE); - loader_up (); - return ai_next (MOVE_MOVING, loader_errored); -} - -/* - * MOVE_MOVING_BACKWARD_TO_TURN_FREELY =bot_move_succeed=> - * tryout => MOVE_IDLE - * post failure event. - * path_found_rotate => MOVE_ROTATING - * rotate towards next position. - * path_found => MOVE_MOVING - * move to next position. - * no_path_found => MOVE_IDLE - * post failure event. - */ -fsm_branch_t -ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_succeed (void) -{ - if (--move_data.try_again_counter == 0) - { - main_post_event (AI_EVENT_move_fsm_failed); - return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, tryout); - } - else - { - uint8_t next = move_path_init (); - if (next) - { - if (next == 2) - return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, path_found_rotate); - else - return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, path_found); - } - else - { - main_post_event (AI_EVENT_move_fsm_failed); - return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, no_path_found); - } - } -} - -/* - * MOVE_MOVING_BACKWARD_TO_TURN_FREELY =bot_move_failed=> - * tryout => MOVE_IDLE - * post failure event. - * path_found_rotate => MOVE_ROTATING - * rotate towards next position. - * path_found => MOVE_MOVING - * move to next position. - * no_path_found_tryagain => MOVE_WAIT_FOR_CLEAR_PATH - * nothing to do. - * no_path_found_tryout => MOVE_IDLE - * post failure event. - */ -fsm_branch_t -ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_failed (void) -{ - if (--move_data.try_again_counter == 0) - { - main_post_event (AI_EVENT_move_fsm_failed); - return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, tryout); - } - else - { - uint8_t next = move_path_init (); - if (next) - { - if (next == 2) - return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, path_found_rotate); - else - return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, path_found); - } - else - { - if (--move_data.try_again_counter == 0) - { - main_post_event (AI_EVENT_move_fsm_failed); - return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_path_found_tryout); - } - else - return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_path_found_tryagain); - } - } -} - -/* - * MOVE_WAIT_FOR_CLEAR_PATH =state_timeout=> - * path_found_rotate => MOVE_ROTATING - * rotate towards next position. - * path_found => MOVE_MOVING - * move to next position. - * no_path_found_tryagain => MOVE_WAIT_FOR_CLEAR_PATH - * decrement counter. - * no_path_found_tryout => MOVE_IDLE - * post failure. - */ -fsm_branch_t -ai__MOVE_WAIT_FOR_CLEAR_PATH__state_timeout (void) -{ - /* Try to move. */ - uint8_t next = move_path_init (); - if (next) - { - if (next == 2) - return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, path_found_rotate); - else - return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, path_found); - } - else - { - /* Error, no new position, should we try again? */ - if (--move_data.try_again_counter == 0) - { - main_post_event (AI_EVENT_move_fsm_failed); - return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, no_path_found_tryout); - } - else - return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, no_path_found_tryagain); - } -} - -/* - * MOVE_LOADER_UNBLOCKING_UPING =bot_move_succeed=> - * => MOVE_LOADER_UNBLOCKING_DOWNING - * loader down - * reset unblocking retry counter - */ -fsm_branch_t -ai__MOVE_LOADER_UNBLOCKING_UPING__bot_move_succeed (void) -{ - loader_down (); - move_data.loader_unblocking_retry = 2; - return ai_next (MOVE_LOADER_UNBLOCKING_UPING, bot_move_succeed); -} - -/* - * MOVE_LOADER_UNBLOCKING_UPING =bot_move_failed=> - * => MOVE_LOADER_UNBLOCKING_DOWNING - * loader down - * reset unblocking retry counter - */ -fsm_branch_t -ai__MOVE_LOADER_UNBLOCKING_UPING__bot_move_failed (void) -{ - loader_down (); - move_data.loader_unblocking_retry = 2; - return ai_next (MOVE_LOADER_UNBLOCKING_UPING, bot_move_failed); -} - -/* - * MOVE_LOADER_UNBLOCKING_DOWNING =loader_downed=> - * path_found_rotate => MOVE_ROTATING - * rotate towards next position. - * path_found => MOVE_MOVING - * move to next position. - * no_path_found => MOVE_IDLE - * post failure. - */ -fsm_branch_t -ai__MOVE_LOADER_UNBLOCKING_DOWNING__loader_downed (void) -{ - /* Try to move. */ - uint8_t next = move_path_init (); - if (next) - { - if (next == 2) - return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, path_found_rotate); - else - return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, path_found); - } - else - { - main_post_event (AI_EVENT_move_fsm_failed); - return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, no_path_found); - } -} - -/* - * MOVE_LOADER_UNBLOCKING_DOWNING =loader_errored=> - * tryagain => MOVE_LOADER_UNBLOCKING_UPING - * move backward - * loader up - * tryout_path_found_rotate => MOVE_ROTATING - * rotate towards next position. - * tryout_path_found => MOVE_ROTATING - * move to next position. - * tryout_no_path_found => MOVE_IDLE - * post failure. - */ -fsm_branch_t -ai__MOVE_LOADER_UNBLOCKING_DOWNING__loader_errored (void) -{ - if (--move_data.loader_unblocking_retry) - { - asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE); - loader_up (); - return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryagain); - } - else - { - /* Try to move. */ - uint8_t next = move_path_init (); - if (next) - { - if (next == 2) - return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_path_found_rotate); - else - return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_path_found); - } - else - { - main_post_event (AI_EVENT_move_fsm_failed); - return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_no_path_found); - } - } -} - -/* - * MOVE_LOADER_UNBLOCKING_DOWNING =state_timeout=> - * tryagain => MOVE_LOADER_UNBLOCKING_UPING - * move backward - * loader up - * tryout_path_found_rotate => MOVE_ROTATING - * rotate towards next position. - * tryout_path_found => MOVE_ROTATING - * move to next position. - * tryout_no_path_found => MOVE_IDLE - * post failure. - */ -fsm_branch_t -ai__MOVE_LOADER_UNBLOCKING_DOWNING__state_timeout (void) -{ - if (--move_data.loader_unblocking_retry) - { - asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE); - loader_up (); - return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, state_timeout, tryagain); - } - else - { - /* Try to move. */ - uint8_t next = move_path_init (); - if (next) - { - if (next == 2) - return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, state_timeout, tryout_path_found_rotate); - else - return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, state_timeout, tryout_path_found); - } - else - { - main_post_event (AI_EVENT_move_fsm_failed); - return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, state_timeout, tryout_no_path_found); - } - } -} - - diff --git a/digital/io/src/ai_top_cb.c b/digital/io/src/ai_top_cb.c deleted file mode 100644 index ed1ae038..00000000 --- a/digital/io/src/ai_top_cb.c +++ /dev/null @@ -1,434 +0,0 @@ -/* ai_top_cb.c */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2010 Nicolas Schodet - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * }}} - * Main FSM calling other FSM. - */ -#include "common.h" -#include "fsm.h" -#include "ai_cb.h" -#include "top.h" -#include "move.h" -#include "chrono.h" -#include "playground.h" -#include "asserv.h" -#include "loader.h" -#include "food.h" - -/* - * IDLE =start=> - * => WAIT_INIT_TO_FINISH - * nothing to do. - */ -fsm_branch_t -ai__IDLE__start (void) -{ - return ai_next (IDLE, start); -} - -/* - * WAIT_INIT_TO_FINISH =init_match_is_started=> - * => FIRST_GO_BEGIN_OF_LINE_FAST - * the match start - * set fast speed - * go to first sequence begin of line - */ -fsm_branch_t -ai__WAIT_INIT_TO_FINISH__init_match_is_started (void) -{ - asserv_set_speed (BOT_MOVE_FAST); - move_start_noangle (PG_VECT (375, 1503), 0, 0); - return ai_next (WAIT_INIT_TO_FINISH, init_match_is_started); -} - -/* - * FIRST_GO_BEGIN_OF_LINE_FAST =move_fsm_succeed=> - * => FIRST_GO_END_OF_LINE_FAST - * go to end of line - * loader down - */ -fsm_branch_t -ai__FIRST_GO_BEGIN_OF_LINE_FAST__move_fsm_succeed (void) -{ - move_start_noangle (PG_VECT (2625, 253), 0, 0); - loader_down (); - return ai_next (FIRST_GO_BEGIN_OF_LINE_FAST, move_fsm_succeed); -} - -/* - * FIRST_GO_BEGIN_OF_LINE_FAST =move_fsm_failed=> - * => FIRST_GO_BEGIN_OF_LINE_FAST - * set slow speed - * retry - */ -fsm_branch_t -ai__FIRST_GO_BEGIN_OF_LINE_FAST__move_fsm_failed (void) -{ - asserv_set_speed (BOT_MOVE_SLOW); - move_start_noangle (PG_VECT (375, 1503), 0, 0); - return ai_next (FIRST_GO_BEGIN_OF_LINE_FAST, move_fsm_failed); -} - -/* - * FIRST_GO_END_OF_LINE_FAST =in_field=> - * => FIRST_GO_END_OF_LINE_SLOW - * set slow speed - */ -fsm_branch_t -ai__FIRST_GO_END_OF_LINE_FAST__in_field (void) -{ - asserv_set_speed (BOT_MOVE_SLOW); - return ai_next (FIRST_GO_END_OF_LINE_FAST, in_field); -} - -/* - * FIRST_GO_END_OF_LINE_FAST =move_fsm_succeed=> - * => UNLOAD_LOADER_UP - * set slow speed - * move loader up - */ -fsm_branch_t -ai__FIRST_GO_END_OF_LINE_FAST__move_fsm_succeed (void) -{ - asserv_set_speed (BOT_MOVE_SLOW); - ai__FIRST_GO_END_OF_LINE_SLOW__move_fsm_succeed (); - return ai_next (FIRST_GO_END_OF_LINE_FAST, move_fsm_succeed); -} - -/* - * FIRST_GO_END_OF_LINE_FAST =move_fsm_failed=> - * => FIRST_GO_END_OF_LINE_SLOW - * set slow speed - * retry - */ -fsm_branch_t -ai__FIRST_GO_END_OF_LINE_FAST__move_fsm_failed (void) -{ - asserv_set_speed (BOT_MOVE_SLOW); - ai__FIRST_GO_END_OF_LINE_SLOW__move_fsm_failed (); - return ai_next (FIRST_GO_END_OF_LINE_FAST, move_fsm_failed); -} - -/* - * FIRST_GO_END_OF_LINE_SLOW =move_fsm_succeed=> - * => UNLOAD_LOADER_UP - * move loader up - */ -fsm_branch_t -ai__FIRST_GO_END_OF_LINE_SLOW__move_fsm_succeed (void) -{ - loader_up (); - return ai_next (FIRST_GO_END_OF_LINE_SLOW, move_fsm_succeed); -} - -/* - * FIRST_GO_END_OF_LINE_SLOW =move_fsm_failed=> - * => FIRST_GO_END_OF_LINE_SLOW - * retry - */ -fsm_branch_t -ai__FIRST_GO_END_OF_LINE_SLOW__move_fsm_failed (void) -{ - move_start_noangle (PG_VECT (2625, 253), 0, 0); - return ai_next (FIRST_GO_END_OF_LINE_SLOW, move_fsm_failed); -} - -/* - * UNLOAD =move_fsm_succeed=> - * => UNLOAD_LOADER_UP - * move loader up - */ -fsm_branch_t -ai__UNLOAD__move_fsm_succeed (void) -{ - loader_up (); - return ai_next (UNLOAD, move_fsm_succeed); -} - -/* - * UNLOAD =move_fsm_failed=> - * => UNLOAD - * retry - */ -fsm_branch_t -ai__UNLOAD__move_fsm_failed (void) -{ - move_start_noangle (PG_VECT (2625, 253), 0, 0); - return ai_next (UNLOAD, move_fsm_failed); -} - -/* - * UNLOAD_LOADER_UP =loader_uped=> - * => UNLOAD_FACE_BIN - * turn toward bin - */ -fsm_branch_t -ai__UNLOAD_LOADER_UP__loader_uped (void) -{ - asserv_goto_angle (PG_A_DEG (90)); - return ai_next (UNLOAD_LOADER_UP, loader_uped); -} - -/* - * UNLOAD_LOADER_UP =loader_errored=> - * => UNLOAD_FACE_BIN - * turn toward bin - */ -fsm_branch_t -ai__UNLOAD_LOADER_UP__loader_errored (void) -{ - asserv_goto_angle (PG_A_DEG (90)); - return ai_next (UNLOAD_LOADER_UP, loader_errored); -} - -/* - * UNLOAD_FACE_BIN =bot_move_succeed=> - * => UNLOAD_BACK_BIN - * go backward to bin - */ -fsm_branch_t -ai__UNLOAD_FACE_BIN__bot_move_succeed (void) -{ - asserv_move_linearly (-(128 + 250 / 2 - BOT_SIZE_BACK - 50)); - return ai_next (UNLOAD_FACE_BIN, bot_move_succeed); -} - -/* - * UNLOAD_FACE_BIN =bot_move_failed=> - * => UNLOAD_FACE_BIN_UNBLOCK - * move backward - */ -fsm_branch_t -ai__UNLOAD_FACE_BIN__bot_move_failed (void) -{ - asserv_move_linearly (-40); - return ai_next (UNLOAD_FACE_BIN, bot_move_failed); -} - -/* - * UNLOAD_FACE_BIN_UNBLOCK =bot_move_succeed=> - * => UNLOAD_FACE_BIN - * turn toward bin - */ -fsm_branch_t -ai__UNLOAD_FACE_BIN_UNBLOCK__bot_move_succeed (void) -{ - asserv_goto_angle (PG_A_DEG (90)); - return ai_next (UNLOAD_FACE_BIN_UNBLOCK, bot_move_succeed); -} - -/* - * UNLOAD_FACE_BIN_UNBLOCK =bot_move_failed=> - * => UNLOAD_FACE_BIN - * turn toward bin - */ -fsm_branch_t -ai__UNLOAD_FACE_BIN_UNBLOCK__bot_move_failed (void) -{ - asserv_goto_angle (PG_A_DEG (90)); - return ai_next (UNLOAD_FACE_BIN_UNBLOCK, bot_move_failed); -} - -/* - * UNLOAD_BACK_BIN =bot_move_succeed=> - * => UNLOAD_UNLOAD - * unload - */ -fsm_branch_t -ai__UNLOAD_BACK_BIN__bot_move_succeed (void) -{ - asserv_move_motor1_absolute (0, BOT_GATE_SPEED); - return ai_next (UNLOAD_BACK_BIN, bot_move_succeed); -} - -/* - * UNLOAD_BACK_BIN =bot_move_failed=> - * => UNLOAD_UNLOAD - * unload - */ -fsm_branch_t -ai__UNLOAD_BACK_BIN__bot_move_failed (void) -{ - ai__UNLOAD_BACK_BIN__bot_move_succeed (); - return ai_next (UNLOAD_BACK_BIN, bot_move_failed); -} - -/* - * UNLOAD_UNLOAD =state_timeout=> - * => COLLECT - * close gate - * loader down - * choose best food to collect - */ -fsm_branch_t -ai__UNLOAD_UNLOAD__state_timeout (void) -{ - loader_elements = 0; - asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED); - loader_down (); - top_collect (1); - return ai_next (UNLOAD_UNLOAD, state_timeout); -} - -/* - * COLLECT =move_fsm_succeed=> - * unload => UNLOAD - * collect => COLLECT - * slow_motion => COLLECT_SLOW_MOTION - * post loader_element event - */ -fsm_branch_t -ai__COLLECT__move_fsm_succeed (void) -{ - int16_t slow_motion = food_slow_motion (top_food); - if (slow_motion) - { - fsm_handle_event (&ai_fsm, AI_EVENT_loader_element); - return ai_next_branch (COLLECT, move_fsm_succeed, slow_motion); - } - else if (top_collect (0)) - return ai_next_branch (COLLECT, move_fsm_succeed, collect); - else - return ai_next_branch (COLLECT, move_fsm_succeed, unload); -} - -/* - * COLLECT =move_fsm_failed=> - * unload => UNLOAD - * collect => COLLECT - * cancel this element - */ -fsm_branch_t -ai__COLLECT__move_fsm_failed (void) -{ - if (top_collect (0)) - { - food_cancel (top_food); - return ai_next_branch (COLLECT, move_fsm_failed, collect); - } - else - return ai_next_branch (COLLECT, move_fsm_failed, unload); -} - -/* - * COLLECT =loader_black=> - * => COLLECT_BLACK - * move backward - * mark as black - */ -fsm_branch_t -ai__COLLECT__loader_black (void) -{ - asserv_move_linearly (-90); - food_black (top_food); - return ai_next (COLLECT, loader_black); -} - -/* - * COLLECT_SLOW_MOTION =loader_downed=> - * unload => UNLOAD - * collect => COLLECT - * collect - */ -fsm_branch_t -ai__COLLECT_SLOW_MOTION__loader_downed (void) -{ - if (top_collect (0)) - return ai_next_branch (COLLECT_SLOW_MOTION, loader_downed, collect); - else - return ai_next_branch (COLLECT_SLOW_MOTION, loader_downed, unload); -} - -/* - * COLLECT_SLOW_MOTION =loader_errored=> - * unload => UNLOAD - * collect => COLLECT - * same as above - */ -fsm_branch_t -ai__COLLECT_SLOW_MOTION__loader_errored (void) -{ - if (top_collect (0)) - return ai_next_branch (COLLECT_SLOW_MOTION, loader_errored, collect); - else - return ai_next_branch (COLLECT_SLOW_MOTION, loader_errored, unload); -} - -/* - * COLLECT_SLOW_MOTION =state_timeout=> - * unload => UNLOAD - * collect => COLLECT - * same as above - */ -fsm_branch_t -ai__COLLECT_SLOW_MOTION__state_timeout (void) -{ - if (top_collect (0)) - return ai_next_branch (COLLECT_SLOW_MOTION, state_timeout, collect); - else - return ai_next_branch (COLLECT_SLOW_MOTION, state_timeout, unload); -} - -/* - * COLLECT_SLOW_MOTION =loader_black=> - * => COLLECT_BLACK - * move backward - * mark as black - */ -fsm_branch_t -ai__COLLECT_SLOW_MOTION__loader_black (void) -{ - asserv_move_linearly (-90); - food_black (top_food); - return ai_next (COLLECT_SLOW_MOTION, loader_black); -} - -/* - * COLLECT_BLACK =bot_move_succeed=> - * unload => UNLOAD - * collect => COLLECT - */ -fsm_branch_t -ai__COLLECT_BLACK__bot_move_succeed (void) -{ - if (top_collect (0)) - return ai_next_branch (COLLECT_BLACK, bot_move_succeed, collect); - else - return ai_next_branch (COLLECT_BLACK, bot_move_succeed, unload); -} - -/* - * COLLECT_BLACK =bot_move_failed=> - * unload => UNLOAD - * collect => COLLECT - */ -fsm_branch_t -ai__COLLECT_BLACK__bot_move_failed (void) -{ - if (top_collect (0)) - return ai_next_branch (COLLECT_BLACK, bot_move_failed, collect); - else - return ai_next_branch (COLLECT_BLACK, bot_move_failed, unload); -} - - diff --git a/digital/io/src/fsm.c b/digital/io/src/fsm.c deleted file mode 100644 index 45eeb5ba..00000000 --- a/digital/io/src/fsm.c +++ /dev/null @@ -1,137 +0,0 @@ -/* fsm.c - Finite State Machine code. */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2008 Nicolas Schodet - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * }}} */ -#include "common.h" -#include "fsm.h" - -#include "modules/trace/trace.h" -#include "modules/proto/proto.h" -#include "events.h" - -#ifdef HOST -# include -#endif - -void -fsm_init (fsm_t *fsm) -{ - uint8_t i; - assert (fsm); - for (i = 0; i < FSM_ACTIVE_STATES_MAX; i++) - fsm->states_active[i] = fsm->states_init[i]; - for (i = 0; i < fsm->active_states_nb; i++) - fsm->states_timeout[i] = - fsm->state_timeout_table[fsm->states_init[i]]; -} - -/** Handle an event on the given FSM on a single active state, return 1 if the - * event was handled. */ -static uint8_t -fsm_handle_event_single (fsm_t *fsm, uint8_t active, uint8_t event) -{ - assert (fsm); - assert (active < fsm->active_states_nb); - assert (event < fsm->events_nb); - /* Lookup transition. */ - uint8_t old_state = fsm->states_active[active]; - fsm_transition_t tr = FSM_TRANSITION (fsm, old_state, event); - /* Ignore unhandled events. */ - if (tr) - { - /* Execute transition. */ - uint8_t new_state; - fsm_branch_t br = tr (); - /* Change state. */ -#ifdef HOST - assert (((br >> 16) & 0xff) == fsm->states_active[active]); - assert (((br >> 8) & 0xff) == event); - fprintf (stderr, "%s %s =%s=> %s\n", fsm->name, - fsm->states_names[fsm->states_active[active]], - fsm->events_names[event], fsm->states_names[br & 0xff]); - new_state = br & 0xff; -#else - new_state = br; - proto_send3b ('f', old_state, event, new_state); -#endif - TRACE (TRACE_FSM__HANDLE_EVENT, (u8) old_state, (u8) event, - (u8) new_state); - fsm->states_active[active] = new_state; - fsm->states_timeout[active] = fsm->state_timeout_table[new_state]; - return 1; - } - return 0; -} - -uint8_t -fsm_handle_event (fsm_t *fsm, uint8_t event) -{ - uint8_t i, handled = 0; - assert (fsm); - assert (event < fsm->events_nb); - for (i = 0; i < fsm->active_states_nb; i++) - { - /* Handle the event for this active state. */ - handled += fsm_handle_event_single (fsm, i, event); - } - return handled; -} - -uint8_t -fsm_handle_timeout (fsm_t *fsm) -{ - uint8_t i, handled = 0; - assert (fsm); - for (i = 0; i < fsm->active_states_nb; i++) - { - /* If there is a timeout for this state. */ - if (fsm->states_timeout[i] != 0xffff) - { - if (fsm->states_timeout[i]) - fsm->states_timeout[i]--; - else - { - /* Timeout expired, generate corresponding event. */ - handled += fsm_handle_event_single (fsm, i, - fsm->state_timeout_event); - } - } - } - return handled; -} - -uint8_t -fsm_can_handle_event (fsm_t *fsm, uint8_t event) -{ - uint8_t i; - assert (fsm); - assert (event < fsm->events_nb); - for (i = 0; i < fsm->active_states_nb; i++) - { - uint8_t state = fsm->states_active[i]; - if (FSM_TRANSITION (fsm, state, event)) - return 1; - } - return 0; -} - diff --git a/digital/io/src/fsm.h b/digital/io/src/fsm.h index 2d0e1933..6b376d30 100644 --- a/digital/io/src/fsm.h +++ b/digital/io/src/fsm.h @@ -1,126 +1,566 @@ -#ifndef fsm_h -#define fsm_h -/* fsm.h - Finite State Machine code. */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2008 Nicolas Schodet - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * }}} */ +/* + AngFSM - AngFSM - Almost Non Generated Finite State Machine + + Copyright 2011 Jerome Jutteau + +Contact: + * email: j.jutteau _AT_ gmail _DOT_ com + * website: http://fuu.im/angfsm/ -#ifndef HOST + This file is part of AngFSM. -# include + AngFSM is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. -#else + AngFSM is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with AngFSM. If not, see . + */ -# define PROGMEM -# define pgm_read_word(addr) (*(addr)) +#include +#ifndef _FSM_GEN_ +#define _FSM_GEN_ + +#ifndef FSM_NAME +#define FSM_NAME FSM #endif -/** - * The io board includes an FSM with several concurrent active states. - * - * The main loop tests for each possible event and generate the corresponding - * FSM events. For example: - * - * if (asserv_move_cmd_status () == success) - * fsm_handle_event (&ai_fsm, AI_EVENT_position_reached); - * - * Any unhandled event will be ignored. - * - * To start a sub machine from the top FSM, call the sub machine start - * function (for example move_start), which will set parameters, then trigger - * its start event. +#define XSTR(s) STR(s) +#define STR(s) #s +#define FSM_PASTE(a, b) a ## b +#define FSM_PASTE_EXPAND(a, b) FSM_PASTE (a, b) +#define FSM_PASTE3_EXPAND(a, b, c)\ + FSM_PASTE_EXPAND (a, FSM_PASTE_EXPAND (b, c)) +#define FIRST(first, others...) first + +#ifdef __AVR_ARCH__ +/* Include generated header. */ +#include XSTR (FSM_PASTE_EXPAND (fsm_, FSM_NAME)_gen.h) + +/* Disable init macro. */ +#define FSM_INIT + +/* Disable code generation macro. */ +#define FSM_GENERATE(arch, string) + +/* Disable FSM_IMPORT macro. */ +#define FSM_IMPORT(fsm_name) + +/** Immediatly handle an event by the fsm. */ +#define FSM_HANDLE(fsm, event) fsm_##fsm##_handle (FSM_EVENT_##fsm##_##event ) + +/** Says if an event can actually be handle or not by the fsm. */ +#define FSM_CAN_HANDLE(fsm, event) fsm_##fsm##_can_handle \ + (FSM_EVENT_##fsm##_##event) + +/** Reset fsm to it's initial state. */ +#define FSM_RESET(fsm) fsm_##fsm##_init (); + +/** Disable dot generation. */ +#define FSM_GEN_DOT(fsm) + +/** Disable state defining * - * The sub machine is expected to generate an event for the top machine to - * make it continue. + * Define states of the fsm, can be called several times to add other states. */ +#define FSM_STATES(states...) + +/** Disable event defining */ +#define FSM_EVENTS(events...) + +/** Disable macro. */ +#define FSM_START_WITH(starters...) + +#define FSM_TRANS(state, event, output_branches...)\ + FSM_PASTE3_EXPAND (fsm_, FSM_NAME,_branch_t)\ +FSM_PASTE3_EXPAND (fsm_, FSM_NAME,_trans_func_##state##_##event) () + +#define FSM_NEXT(state, event, branch...)\ + FSM_PASTE3_EXPAND (FSM_BRANCH_, FSM_NAME,_##state##_##event##_##branch) + +/** Transform an event in uint16_t. */ +#define FSM_EVENT(fsm, event) FSM_EVENT_##fsm##_##event + +/** Handle event from uint16_t. */ +#define FSM_HANDLE_VAR(fsm, event) fsm_##fsm##_handle (event) + +/* Can we handle event from uint16_t ? */ +#define FSM_CAN_HANDLE_VAR(fsm, event) fsm_##fsm##_can_handle (event) + +/* Time out macros. */ +#define FSM_TRANS_TIMEOUT(state, timeout, output_branches...)\ + FSM_TRANS (state, state##_TIMEOUT, output_branches) + +#define FSM_NEXT_TIMEOUT(state, branch...)\ + FSM_NEXT (state, state##_TIMEOUT, branch) + +#define FSM_HANDLE_TIMEOUT(fsm_name) fsm_##fsm_name##_handle_timeout () + +#else /* Compiling for HOST */ +#include + +/* Describe an event. */ +typedef struct +{ + /* Name of the event. */ + char *var_name; + /* Unique code of the event. */ + uint code; +} fsm_build_event_t; + +/* Chain of events. */ +typedef struct fsm_build_event_chain_t +{ + fsm_build_event_t event; + struct fsm_build_event_chain_t *next; +} fsm_build_event_chain_t; + +/* Describe a state. Same as event. */ +typedef fsm_build_event_t fsm_build_state_t; + +/* Chain of state */ +typedef struct fsm_build_state_chain_t +{ + fsm_build_state_t state; + struct fsm_build_state_chain_t *next; +} fsm_build_state_chain_t; + +/** + * Describe a branch. It is the output of a transition. + * Branches can be chained when there are several possible output state for + * one event. + */ +typedef struct fsm_build_branch_chain_t +{ + /* Pointer to the output state of the branch. */ + fsm_build_state_t *state; + /* Name of the branch. */ + char *name; + /* Pointer to the next branch when we have the choice of the output */ + struct fsm_build_branch_chain_t *next; +} fsm_build_branch_chain_t; + +/** + * Describe a transition. + * When an event occurs, an active state may react to this event an execute a + * transition. The transition function returns the new state which become + * active. */ +typedef struct +{ + /* Active state. */ + fsm_build_state_t *state; + /* Active event. */ + fsm_build_event_t *event; + /* Possible states the transition will return. */ + fsm_build_branch_chain_t *output_branches; +} fsm_build_trans_t; -#ifdef HOST -/** On host, check branches. */ -typedef u32 fsm_branch_t; -#else -/** On AVR, no check. */ -typedef u8 fsm_branch_t; -#endif +/* Chain of transitions. */ +typedef struct fsm_build_trans_chain_t +{ + fsm_build_trans_t trans; + struct fsm_build_trans_chain_t *next; +} fsm_build_trans_chain_t; -/** Transition function. */ -typedef fsm_branch_t (*fsm_transition_t) (void); +/* Pointer to a transition function. */ +typedef fsm_build_state_t* (*fsm_build_run_strans_func_t)(void); -/** Maximum number of active states. */ -#define FSM_ACTIVE_STATES_MAX 5 +/* Chain of transitions with associated function's pointer. */ +typedef struct fsm_trans_func_chain_t +{ + fsm_build_run_strans_func_t func; + fsm_build_trans_t trans; + struct fsm_trans_func_chain_t *next; +} fsm_trans_func_chain_t; -/** Context of an FSM. */ -struct fsm_t +/* Timeout structure. */ +typedef struct fsm_build_timeout_t { - /** Transition table. */ - const fsm_transition_t *transition_table; - /** Number of events, used to index the right transition. */ - u8 events_nb; - /** Initial states. */ - u8 states_init[FSM_ACTIVE_STATES_MAX]; - /** Current active states. */ - u8 states_active[FSM_ACTIVE_STATES_MAX]; - /** Nomber of active states. */ - u8 active_states_nb; - /** State timeout table. */ - const u16 *state_timeout_table; - /** Current active states timeout if not 0xffff. */ - u16 states_timeout[FSM_ACTIVE_STATES_MAX]; - /** Event to generate on state timeout. */ - u8 state_timeout_event; -#ifdef HOST - /** FSM name. */ - const char *name; - /** States names. */ - const char * const *states_names; - /** Events names. */ - const char * const *events_names; -#endif -}; -typedef struct fsm_t fsm_t; + uint timeout; + fsm_build_trans_t *trans; +} fsm_build_timeout_t; + +/* Chain of fsm_timeout_t. */ +typedef struct fsm_build_timeout_chain_t +{ + fsm_build_timeout_t timeout; + struct fsm_build_timeout_chain_t *next; +} fsm_build_timeout_chain_t; + +/* Data needed for fsm execution (for host). */ +typedef struct +{ + /* This table store pointers of aff all transition functions. The first + * dimension correspond to the events and the second to the states. If + * there is no transition of a event/state couple, NULL is set. This + * permit to react quickly to an event during execution but is not the + * best for memory cost. + */ + fsm_build_run_strans_func_t **trans_table; + /* Store pointer to active states. */ + fsm_build_state_t **active_states; + /* This array has the size of active_states and store the event who has + * triggered the corresponding active state. This is used for dot + * generation for visual purposes. + */ + fsm_build_event_t **events_before_active_state; + /* Store all pointers of transition functions. */ + fsm_trans_func_chain_t *func_pool; + /* Array of counters for timeout events. + * -1 mean counter is off. */ + int *timeout_counters; +} fsm_build_run_t; + +/* Store all Finite State Machine (fsm) informations. */ +typedef struct +{ + /* All events. */ + fsm_build_event_chain_t *events; + /* All states. */ + fsm_build_state_chain_t *states; + /* All transitions. */ + fsm_build_trans_chain_t *trans; + /* Name of the fsm. */ + char *name; + /* Maximal number of active states. */ + uint max_active_states; + /* Total number of events. */ + uint event_nb; + /* Total number of states. */ + uint state_nb; + /* First active states. */ + fsm_build_state_chain_t *starters; + /* All timeout. */ + fsm_build_timeout_chain_t *timeouts; + /* Data for running purposes. */ + fsm_build_run_t run; +} fsm_build_t; + +/* Store all fsm in a chain. */ +typedef struct fsm_build_chain_t +{ + fsm_build_t *fsm; + struct fsm_build_chain_t *next; +} fsm_build_chain_t; + +/* Store all fsm. */ +extern fsm_build_chain_t *fsm_build_all_fsm; +/* Function to initialize fsm_build_all_fsm. */ +void fsm_build_init_all_fsm () __attribute__((constructor(101))); + +/** Create the fsm and functions who will be executed before and after main + * function. This permits initilialization of the fsm. */ +extern fsm_build_t FSM_PASTE_EXPAND (fsm_, FSM_NAME); +void FSM_PASTE_EXPAND (fsm_build_init_, FSM_NAME)() __attribute__((constructor(102))); +void FSM_PASTE_EXPAND (fsm_build_run_init_, FSM_NAME)() __attribute__((constructor(107))); +void FSM_PASTE_EXPAND (fsm_build_free_, FSM_NAME)() __attribute__((destructor)); + +#define FSM_INIT\ + fsm_build_t FSM_PASTE_EXPAND (fsm_, FSM_NAME);\ +void FSM_PASTE_EXPAND (fsm_build_init_, FSM_NAME)()\ +{\ + fsm_build_init (FSM_PASTE_EXPAND (&fsm_, FSM_NAME), XSTR(FSM_NAME));\ +}\ +void FSM_PASTE_EXPAND (fsm_build_run_init_, FSM_NAME)()\ +{\ + fsm_build_run_init (& FSM_PASTE_EXPAND (fsm_, FSM_NAME));\ +}\ +void FSM_PASTE_EXPAND (fsm_build_free_, FSM_NAME)()\ +{\ + fsm_build_free (& FSM_PASTE_EXPAND (fsm_, FSM_NAME));\ +} + +#define FSM_IMPORT(fsm_name)\ + extern fsm_build_t fsm_##fsm_name; + +/** Generate source ou header files for the specific architecture */ +#define FSM_GENERATE(arch, string) fsm_build_gen (#arch, string); + +/** Immediatly handle an event by the fsm. */ +#define FSM_HANDLE(fsm, event) fsm_build_handle_string (&fsm_##fsm , #event) + +/** Says if an event can actually be handle or not by the fsm. */ +#define FSM_CAN_HANDLE(fsm, event) fsm_build_can_handle_string (&fsm_##fsm, #event) + +/** Reset fsm to it's initial state. */ +#define FSM_RESET(fsm) fsm_build_reset (&fsm_##fsm) + +/** Generate dot. */ +#define FSM_GEN_DOT(fsm) fsm_build_gen_dot (&fsm_##fsm) + +/** Define states of the fsm, can be called several times to add other states. */ +#define FSM_STATES(states...)\ + void FSM_PASTE3_EXPAND (fsm_build_states_, FSM_NAME, FIRST (states)) () __attribute__((constructor(103)));\ +void FSM_PASTE3_EXPAND (fsm_build_states_, FSM_NAME, FIRST (states)) ()\ +{\ + fsm_build_states (& FSM_PASTE_EXPAND (fsm_, FSM_NAME), #states);\ +} + +/** Define events of the fsm, can be called several times to add other events. */ +#define FSM_EVENTS(events...)\ + void FSM_PASTE3_EXPAND (fsm_build_events_, FSM_NAME, FIRST (events)) () __attribute__((constructor(103)));\ +void FSM_PASTE3_EXPAND (fsm_build_events_, FSM_NAME, FIRST (events)) ()\ +{\ + fsm_build_events (& FSM_PASTE_EXPAND (fsm_, FSM_NAME), #events);\ +} -#define FSM_TRANSITION(fsm, state, event) \ - ((fsm_transition_t) pgm_read_word ( \ - &(fsm)->transition_table[(state) * (fsm)->events_nb + (event)])) +/** Define wich state(s) will be active at the beginning. */ +#define FSM_START_WITH(starters...)\ + void FSM_PASTE3_EXPAND (fsm_build_start_with_, FSM_NAME, FIRST (starters))() __attribute__((constructor(104)));\ +void FSM_PASTE3_EXPAND (fsm_build_start_with_, FSM_NAME, FIRST (starters))()\ +{\ + fsm_build_start_with (& FSM_PASTE_EXPAND (fsm_, FSM_NAME), #starters);\ +} -/** Reset a FSM. */ +/** + * Define a transition by giving a state, an event and: + * - a event if the transition returns only one state. + * - branches if the transition returns different states. + * See examples for reel usage. + */ +#define FSM_TRANS(state, event, output_branches...)\ + fsm_build_state_t* FSM_PASTE3_EXPAND (fsm_trans_, FSM_NAME,_##state##_##event) ();\ +void FSM_PASTE3_EXPAND (fsm_build_trans_, FSM_NAME,_##state##_##event)() __attribute__((constructor(105)));\ +void FSM_PASTE3_EXPAND (fsm_build_trans_, FSM_NAME,_##state##_##event)()\ +{\ + fsm_build_trans (& FSM_PASTE_EXPAND(fsm_, FSM_NAME), #state, #event,\ +#output_branches,\ + & FSM_PASTE3_EXPAND (fsm_trans_, FSM_NAME,_##state##_##event));\ +}\ +fsm_build_state_t* FSM_PASTE3_EXPAND (fsm_trans_, FSM_NAME,_##state##_##event) () + +/** + * Used to return next state by giving the actual transition informations and + * the branch (if there are several branches). + * Not directly returning the state can avoid some errors. * + */ +#define FSM_NEXT(state, event, branch...)\ + fsm_build_get_next_state (& FSM_PASTE_EXPAND (fsm_, FSM_NAME), #state, #event, #branch) + +/** + * Define a transition when a state times out. + * You have to provide a state, a timeout value and: + * - a event if the transition returns only one state. + * - branches if the transition returns different states. + * See examples for reel usage. + */ +#define FSM_TRANS_TIMEOUT(state, timeout, output_branches...)\ + void FSM_PASTE3_EXPAND (fsm_build_timeout_, FSM_NAME,_##state##_TIMEOUT)() __attribute__((constructor(106)));\ +void FSM_PASTE3_EXPAND (fsm_build_timeout_, FSM_NAME,_##state##_TIMEOUT)()\ +{\ + fsm_build_timeout (& FSM_PASTE_EXPAND(fsm_, FSM_NAME), #state, XSTR(state##_TIMEOUT), timeout);\ +}\ +FSM_EVENTS (state##_TIMEOUT)\ +FSM_TRANS (state, state##_TIMEOUT, output_branches) + +/** + * Used to return next state after a timeout. + * Same as FSM_NEXT but without specifying an event. + */ +#define FSM_NEXT_TIMEOUT(state, branch...)\ + FSM_NEXT (state, state##_TIMEOUT, branch) + +/** Used to handle timeout events. */ +#define FSM_HANDLE_TIMEOUT(fsm_name) fsm_build_handle_timeout (& FSM_PASTE_EXPAND (fsm_, FSM_NAME)) + +/** Transform an event in uint16_t. */ +#define FSM_EVENT(fsm_name, event) fsm_build_get_event_code (& FSM_PASTE_EXPAND (fsm_, FSM_NAME), #event) + +/** Handle event from uint16_t. */ +#define FSM_HANDLE_VAR(fsm, event) fsm_build_handle_integer (& FSM_PASTE_EXPAND (fsm_, FSM_NAME), event) + +/* Can we handle event from uint16_t ? */ +#define FSM_CAN_HANDLE_VAR(fSM, event) fsm_build_can_handle_integer (& FSM_PASTE_EXPAND (fsm_, FSM_NAME), event) + +/** + * Parse a string who contains a list of arguments seperated by comma. + * It will create and store each parameters (without space and ") in an array + * and give the number of parsed arguments. + * \param string string to parse + * \param tab pointer to a table of strings where to store the array + * \param nb pointer where to store the number of parsed arguments + */ +void +fsm_build_arg_parse (char *string, char ***tab, int *nb); + +/** + * Free an array of strings generated by fsm_build_arg_parse. + * \param tab pointer to the array of strings + * \param nb number of arguments stored in the array. + */ +void +fsm_build_arg_free (char ***tab, int nb); + +/** + * This function is executed when a state is returned by a transition. + * \param fsm fsm + * \param trans transition where the return occurs + * \param branch branch to transition has chosen. + */ +void +fsm_build_print (fsm_build_t *fsm, + fsm_build_trans_t* trans, + fsm_build_branch_chain_t* branch); + +/** + * Test the fsm and search for errors. + */ +void +fsm_build_sanity_check (fsm_build_t *fsm); + +/** Reset the fsm to it's initial state. */ +void +fsm_build_reset (fsm_build_t *fsm); + +/** Generate the dot file of the actual fsm state. */ +void +fsm_build_gen_dot (fsm_build_t *fsm); + +/** Initialize the fsm. */ void -fsm_init (fsm_t *fsm); +fsm_build_init (fsm_build_t *fsm, char *name); -/** Handle state timeout, return non-zero if at least an event was handled. */ -uint8_t -fsm_handle_timeout (fsm_t *fsm); +/** Initialize the running data of the fsm. */ +void +fsm_build_run_init (fsm_build_t *fsm); + +/** + * Add some states to the fsm. + * \param fsm fsm + * \param states states separated by comma + **/ +void +fsm_build_states (fsm_build_t *fsm, char *states); + +/** + * Add some events to the fsm. + * \param fsm fsm + * \param events events separated by comma + **/ +void +fsm_build_events (fsm_build_t *fsm, char *events); + +/** Get event's pointer by giving it's name. */ +fsm_build_event_t* +fsm_build_get_event (fsm_build_t *fsm, char *event); + +/** Get state's pointer by giving it's name. */ +fsm_build_state_t* +fsm_build_get_state (fsm_build_t *fsm, char *state); + +/** Get event's pointer by giving it's code. */ +fsm_build_event_t* +fsm_build_get_event_by_code (fsm_build_t *fsm, uint event); + +/** Get state's pointer by giving it's code. */ +fsm_build_state_t* +fsm_build_get_state_by_code (fsm_build_t *fsm, uint state); + +/** Get event code as uint16_t */ +uint16_t +fsm_build_get_event_code (fsm_build_t *fsm, char *event); + +/** + * Add a transition to the fsm. + * \param fsm fsm + * \param state state who will be active for the transition + * \param event event to react + * \param output_branches list of output branches seperated by comma or a + * single state (if there is only one branche). + * For example: + * - If the transition return a single state, it will be "stateX" + * - If the transition can return different states, it will be + * "branch1, state1, branch2, state2, branch3, state3" [...] + * \param trans_func pointer to the transition function. + */ +void +fsm_build_trans (fsm_build_t *fsm, + char *state, + char *event, + char *output_branches, + fsm_build_run_strans_func_t trans_func); + +/* Create transition based on a timeout. + * \param fsm fsm + * \param state state who times out + * \param event name of the timeout (specific to this state and described in macro) + * \param timeout value of timeout + */ +void +fsm_build_timeout (fsm_build_t *fsm, char *state, char *event, uint timeout); + +/** + * Define with which states the fsm will start. + * \param fsm fsm + * \param starters list of states seperated by comma + */ +void +fsm_build_start_with (fsm_build_t *fsm, char *starters); + +/** Handle an event by the fsm. + * \param fsm fsm + * \param event event to handle with fsm + * \return zero if event is not handled at all, one otherwise + **/ +int +fsm_build_handle (fsm_build_t *fsm, fsm_build_event_t *event); +int +fsm_build_handle_string (fsm_build_t *fsm, char *event); +int +fsm_build_handle_integer (fsm_build_t *fsm, uint16_t event); + +/** + * Say if the event can be handled or not. + * \param fsm fsm + * \param event event to test with the fsm + * \return zero if the event can be handled, non-zero otherwhise + */ +int +fsm_build_can_handle (fsm_build_t *fsm, fsm_build_event_t *event); +int +fsm_build_can_handle_string (fsm_build_t *fsm, char *event); +int +fsm_build_can_handle_integer (fsm_build_t *fsm, uint16_t event); -/** Handle an event on the given FSM, return non-zero if at least an event was - * handled. */ -uint8_t -fsm_handle_event (fsm_t *fsm, uint8_t event); +/** + * Handle timeout events of the fsm. + * \param fsm fsm + * \return one if an event has been handled, zero otherwise + */ +int +fsm_build_handle_timeout (fsm_build_t *fsm); + +/** Give the state at the transition output. */ +fsm_build_state_t* +fsm_build_get_next_state (fsm_build_t *fsm, + char *state, + char *event, + char *branch); -/** Return non zero if event can be handled. This can be used if event - * generation requires expensive computations. */ -uint8_t -fsm_can_handle_event (fsm_t *fsm, uint8_t event); +/** Generate header file for target which provides more optimised version of + * fsm execution + * \param arch specify generated architecture + * \param embedded_strings ask to embed states and events strings (0 = not + * embedded) + */ +void +fsm_build_gen (char *arch, uint embedded_strings); +void +fsm_build_gen_avr_h (fsm_build_t *fsm, uint embedded_strings); +void +fsm_build_gen_avr_c (fsm_build_t *fsm, uint embedded_strings); + +/** Free fsm allocated data. */ +void +fsm_build_free (fsm_build_t *fsm); -#include "ai_fsm.h" +#endif /* Architectures */ -#endif /* fsm_h */ +#endif /* #ifdef _FSM_GEN_ */ diff --git a/digital/io/src/fsm.host.c b/digital/io/src/fsm.host.c new file mode 100644 index 00000000..bf8b761d --- /dev/null +++ b/digital/io/src/fsm.host.c @@ -0,0 +1,1571 @@ +/* + AngFSM - AngFSM - Almost Non Generated Finite State Machine + + Copyright 2011 Jerome Jutteau + +Contact: + * email: j.jutteau _AT_ gmail _DOT_ com + * website: http://fuu.im/angfsm/ + + This file is part of AngFSM. + + AngFSM is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + AngFSM is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with AngFSM. If not, see . + */ + +#include "fsm.h" + +#include +#include +#include +#include + +void +fsm_build_arg_parse (char *string, char ***tab, int *nb) +{ + long i = 0; + long buff_cnt = 0; + char *buff; + int nb_cur = 0; + int param = 0; + + assert (string && tab && nb); + if (strlen (string) == 0) + { + *nb = 0; + **tab = NULL; + return; + } + buff = strdup (string); + + /* Count. */ + *nb = 1; + for (i = 0; string[i] != '\0'; i++) + if (string[i] == ',') + (*nb)++; + *tab = (char **) malloc ((*nb) * sizeof (char *)); + + /* Fill. */ + for (i = 0; i < (long) strlen (string) + 1; i++) + { + if (string[i] == ',' || string[i] == '\0') + { + param = 0; + buff[buff_cnt] = '\0'; + (*tab)[nb_cur] = strdup (buff); + buff_cnt=0; + nb_cur++; + } + else if (string[i] != '"' && string[i] != ' ') + { + if (param == 0) + param = 1; + if (param == 2) + printf ("Parse error in \"%s\"\n", string); + buff[buff_cnt++] = string[i]; + } + else if (string[i] == ' ') + { + if (param == 1) + param = 2; + } + } + free (buff); +} + +void +fsm_build_arg_free (char ***tab, int nb) +{ + assert (*tab); + int i; + for (i = 0; i < nb; i++) + { + assert ((*tab)[i]); + free ((*tab)[i]); + } + free (*tab); +} + +void +fsm_build_print (fsm_build_t *fsm, + fsm_build_trans_t *trans, + fsm_build_branch_chain_t *branch) +{ + //XXX What to do here ? + if (branch->name == NULL) + fprintf (stderr, "Transition: %s -- %s --> %s\n", + trans->state->var_name, + trans->event->var_name, + branch->state->var_name); + else + fprintf (stderr, "Transition: %s -- %s --> %s (%s)\n", + trans->state->var_name, + trans->event->var_name, + branch->name, + branch->state->var_name); +} + +void +fsm_build_sanity_check (fsm_build_t *fsm) +{ + fsm_build_event_chain_t *ec; + fsm_build_event_chain_t *ec_tmp; + fsm_build_state_chain_t *sc; + fsm_build_state_chain_t *sc_tmp; + fsm_build_trans_chain_t *tc; + fsm_build_trans_chain_t *tc_tmp; + fsm_build_branch_chain_t *bc; + uint b; + + /* Sanity check 1: are all states has a different name ? */ + sc = fsm->states; + while (sc != NULL) + { + sc_tmp = fsm->states; + while (sc_tmp != NULL) + { + if (sc != sc_tmp + && strcmp (sc->state.var_name, sc_tmp->state.var_name) + == 0) + assert (!"All states must have a different name."); + sc_tmp = sc_tmp->next; + } + sc = sc->next; + } + + /* Sanity check 2: are all events has a different name ? */ + ec = fsm->events; + while (ec != NULL) + { + ec_tmp = fsm->events; + while (ec_tmp != NULL) + { + if (ec != ec_tmp + && strcmp (ec->event.var_name, ec_tmp->event.var_name) + == 0) + assert (!"All events must have a different name."); + ec_tmp = ec_tmp->next; + } + ec = ec->next; + } + + /* Sanity check 3: are all events used in a transition ? */ + ec = fsm->events; + while (ec != NULL) + { + b = 0; + tc = fsm->trans; + while (tc != NULL) + { + if (&ec->event == tc->trans.event) + { + b = 1; + break; + } + tc = tc->next; + } + if (!b) + fprintf (stderr, "Warning, some events are not used in a transition (%s).\n", + ec->event.var_name); + ec = ec->next; + } + + /* Sanity check 4: are all states used in a transition ? */ + sc = fsm->states; + while (sc != NULL) + { + b = 0; + tc = fsm->trans; + while (tc != NULL) + { + if (&sc->state == tc->trans.state) + { + b = 1; + break; + } + /* This state may be a final state. */ + bc = tc->trans.output_branches; + while (bc != NULL) + { + if (&sc->state == bc->state) + { + b = 1; + break; + } + bc = bc->next; + } + + tc = tc->next; + } + if (!b) + fprintf (stderr, "Warning, some states are not used in a transition (%s).\n", + sc->state.var_name); + sc = sc->next; + } + + /* Sanity check 5: are all states can be reached during execution ? */ + fsm_build_state_chain_t *stack = NULL; + fsm_build_state_chain_t *tail = NULL; + uint i; + /* Initialise check array. */ + uint *check_tab = malloc (fsm->state_nb * sizeof (uint)); + memset (check_tab, 0, fsm->state_nb * sizeof (uint)); + /* Initialize by adding first active states the stack. */ + sc = fsm->starters; + while (sc != NULL) + { + sc_tmp = (fsm_build_state_chain_t *) + malloc (sizeof (fsm_build_state_chain_t)); + sc_tmp->state = sc->state; + if (stack == NULL) + stack = sc_tmp; + if (tail != NULL) + tail->next = sc_tmp; + sc_tmp->next = NULL; + tail = sc_tmp; + sc = sc->next; + } + /* Get next states from the stack and check them. */ + while (stack != NULL) + { + /* Check state. */ + check_tab[stack->state.code] = 1; + /* Add to the tail of the stack th next states if they are not checked + * yet. */ + tc = fsm->trans; + while (tc != NULL) + { + if (tc->trans.state->code == stack->state.code) + { + bc = tc->trans.output_branches; + while (bc != NULL) + { + if (!check_tab[bc->state->code]) + { + check_tab[bc->state->code] = 1; + /* Add at the end of the stack. */ + sc_tmp = (fsm_build_state_chain_t *) + malloc (sizeof (fsm_build_state_chain_t)); + sc_tmp->state = *bc->state; + sc_tmp->next = NULL; + tail->next = sc_tmp; + tail = sc_tmp; + } + bc = bc->next; + } + } + tc = tc->next; + } + sc_tmp = stack->next; + free (stack); + stack = sc_tmp; + } + /* Check if some states are not reached. */ + for (i = 0; i < fsm->state_nb; i++) + if (!check_tab[i]) + fprintf (stderr, "Warning, some states won't be reachable during execution (%s).\n", + fsm_build_get_state_by_code (fsm, i)->var_name); + /* Free. */ + free (check_tab); + + /* Sanity check 6: Any transition how have the same state and same event ? + * Useless test ? Making the same state/event couple will cause a + * compilation error. + */ + tc = fsm->trans; + while (tc != NULL) + { + tc_tmp = fsm->trans; + while (tc_tmp != NULL) + { + if (tc != tc_tmp + && tc->trans.state == tc_tmp->trans.state + && tc->trans.event == tc_tmp->trans.event) + assert (!"All transitions must have a different state/event\ + couple."); + tc_tmp = tc_tmp->next; + } + tc = tc->next; + } + + /* Sanity check 7: check that all first active states are unique. */ + sc = fsm->starters; + while (sc != NULL) + { + uint b = 0; + sc_tmp = fsm->starters; + while (sc_tmp != NULL) + { + if (sc_tmp->state.code == sc->state.code) + b++; + if (b > 1) + assert (!"Your first active states must be unique."); + sc_tmp = sc_tmp->next; + } + sc = sc->next; + } + + /* Sanity check 8: any transition output error ? + * for example, as we are in state s42, making a FSM_NEXT (s1, e2) will + * work but this is a user's mistake. + * + * TODO Find a way to check this. + **/ +} + +void +fsm_build_reset (fsm_build_t *fsm) +{ + uint i; + fsm_build_state_chain_t *curs = fsm->starters; + for (i = 0; i < fsm->max_active_states; i++) + { + fsm->run.active_states[i] = NULL; + fsm->run.events_before_active_state[i] = NULL; + } + i = 0; + while (curs != NULL) + { + fsm->run.active_states[i++] = &curs->state; + curs = curs->next; + } +} + +void +fsm_build_gen_dot (fsm_build_t *fsm) +{ + static uint cpt = 1; + uint j; + fsm_build_trans_chain_t *tc; + fsm_build_branch_chain_t *bc; + + uint fns = strlen (fsm->name) + strlen ("dot__.dot") + cpt + 1; + char *fn = malloc (fns * sizeof (char)); + snprintf (fn, fns, "dot_%s_%u.dot", fsm->name, cpt++); + FILE *f = fopen(fn,"w"); + + /* Gen graph. */ + fprintf (f, "digraph %s {\n", fsm->name); + tc = fsm->trans; + while (tc != NULL) + { + bc = tc->trans.output_branches; + while (bc != NULL) + { + fprintf (f, "\t%s -> %s [label=\"%s", + tc->trans.state->var_name, + bc->state->var_name, + tc->trans.event->var_name); + if (bc->name != NULL) + fprintf (f, " (%s)", bc->name); + fprintf (f, "\""); + /* If the next state is an active state we look if this this event + * was handled before. */ + for (j = 0; j < fsm->max_active_states; j++) + if (fsm->run.active_states[j] + && fsm->run.active_states[j]->code == bc->state->code) + if (fsm->run.events_before_active_state[j] + && fsm->run.events_before_active_state[j]->code + == tc->trans.event->code) + fprintf (f, ", color=red"); + + fprintf (f, "];\n"); + bc = bc->next; + } + tc = tc->next; + } + /* Colorise active states. */ + for (j = 0; j < fsm->max_active_states; j++) + if (fsm->run.active_states[j]) + fprintf (f, "\t%s [color=blue, fontcolor=white, style=filled];\n", + fsm->run.active_states[j]->var_name); + + fprintf (f, "}\n\n"); + + /* Close file. */ + fclose (f); + + /* Free. */ + free (fn); +} + +/* All fsm. */ +fsm_build_chain_t *fsm_build_all_fsm; +void fsm_build_init_all_fsm () +{ + fsm_build_all_fsm = NULL; +} + +/* Just initialise structures. */ +void +fsm_build_init (fsm_build_t *fsm, char *name) +{ + assert (fsm); + fsm->events = NULL; + fsm->states = NULL; + fsm->trans = NULL; + fsm->name = name; + fsm->max_active_states = 0; + fsm->event_nb = 0; + fsm->state_nb = 0; + fsm->starters = NULL; + fsm->timeouts = NULL; + + fsm->run.trans_table = NULL; + fsm->run.active_states = NULL; + fsm->run.events_before_active_state = NULL; + fsm->run.func_pool = NULL; + fsm->run.timeout_counters = NULL; + + /* Store fsm. */ + fsm_build_chain_t *niou = (fsm_build_chain_t *) + malloc (sizeof (fsm_build_chain_t)); + niou->fsm = fsm; + niou->next = fsm_build_all_fsm; + fsm_build_all_fsm = niou; +} + +/* Prepare the fsm to run (at very end). */ +void +fsm_build_run_init (fsm_build_t *fsm) +{ + uint i,j; + + /* Sanity check. */ + fsm_build_sanity_check (fsm); + + /* Create empty transition table. */ + fsm->run.trans_table = (fsm_build_run_strans_func_t **) + malloc (fsm->event_nb * sizeof (fsm_build_run_strans_func_t *)); + for (i = 0; i < fsm->event_nb; i++) + { + fsm->run.trans_table[i] = (fsm_build_run_strans_func_t *) + malloc (fsm->state_nb * sizeof (fsm_build_run_strans_func_t)); + for (j = 0; j < fsm->state_nb; j++) + fsm->run.trans_table[i][j] = NULL; + } + + /* Fill with transitions. */ + fsm_trans_func_chain_t *fc = fsm->run.func_pool; + while (fc != NULL) + { + fsm->run.trans_table[fc->trans.event->code][fc->trans.state->code] = + fc->func; + fc = fc->next; + } + + /* Free function pool. */ + fsm_trans_func_chain_t *old_fc; + fc = fsm->run.func_pool; + while (fc != NULL) + { + old_fc = fc; + fc = fc->next; + free (old_fc); + } + fsm->run.func_pool = NULL; + + /* Initialize active states. */ + fsm->run.active_states = (fsm_build_state_t **) malloc + (fsm->max_active_states * sizeof (fsm_build_state_t *)); + for (i = 0; i < fsm->max_active_states; i++) + fsm->run.active_states[i] = NULL; + fsm_build_state_chain_t *s = fsm->starters; + i = 0; + while (s != NULL) + { + fsm->run.active_states[i++] = &s->state; + s = s->next; + } + + /* Initialize last events before active states. */ + fsm->run.events_before_active_state = (fsm_build_event_t **) malloc + (fsm->max_active_states * sizeof (fsm_build_event_t *)); + for (i = 0; i < fsm->max_active_states; i++) + fsm->run.events_before_active_state[i] = NULL; + + /* Initialize timeout counters. */ + fsm->run.timeout_counters = (int *) + malloc (fsm->max_active_states * sizeof (int)); + for (i = 0; i < fsm->max_active_states; i++) + fsm->run.timeout_counters[i] = -1; + fsm_build_timeout_chain_t *toc = fsm->timeouts; + for (i = 0; i < fsm->max_active_states; i++) + while (toc != NULL) + { + if (fsm->run.active_states[i]->code + == toc->timeout.trans->state->code) + fsm->run.timeout_counters[i] = toc->timeout.timeout; + toc = toc->next; + } +} + +void +fsm_build_states (fsm_build_t *fsm, char *states) +{ + fsm_build_state_chain_t *head = fsm->states; + char **args; + int i, nb; + + fsm_build_arg_parse (states, &args, &nb); + for (i = 0; i < nb; i++) + { + fsm_build_state_chain_t *s = (fsm_build_state_chain_t *) + malloc (sizeof (fsm_build_state_chain_t)); + s->state.var_name = strdup (args[i]); + s->state.code = fsm->state_nb++; + s->next = head; + head = s; + } + fsm_build_arg_free (&args, nb); + fsm->states = head; +} + +void +fsm_build_events (fsm_build_t *fsm, char *events) +{ + fsm_build_event_chain_t *head = fsm->events; + char **args; + int i, nb; + + if (strcmp (events, "_TIMEOUT_") == 0 + && fsm_build_get_event (fsm, events) != NULL) + return; + fsm_build_arg_parse (events, &args, &nb); + for (i = 0; i < nb; i++) + { + fsm_build_event_chain_t *e = (fsm_build_event_chain_t *) + malloc (sizeof (fsm_build_event_chain_t)); + e->event.var_name = strdup (args[i]); + e->event.code = fsm->event_nb++; + e->next = head; + head = e; + } + fsm_build_arg_free (&args, nb); + fsm->events = head; +} + +fsm_build_event_t * +fsm_build_get_event (fsm_build_t *fsm, char *event) +{ + fsm_build_event_chain_t *curs = fsm->events; + while (curs != NULL) + { + if (strcmp (curs->event.var_name, event) == 0) + return &(curs->event); + curs = curs->next; + } + return NULL; +} + +fsm_build_state_t * +fsm_build_get_state (fsm_build_t *fsm, char *state) +{ + fsm_build_state_chain_t *curs = fsm->states; + while (curs != NULL) + { + if (strcmp (curs->state.var_name, state) == 0) + return &(curs->state); + curs = curs->next; + } + return NULL; +} + +fsm_build_event_t* +fsm_build_get_event_by_code (fsm_build_t *fsm, uint event) +{ + fsm_build_event_chain_t *curs = fsm->events; + while (curs != NULL) + { + if (curs->event.code == event) + return &(curs->event); + curs = curs->next; + } + return NULL; +} + +fsm_build_state_t* +fsm_build_get_state_by_code (fsm_build_t *fsm, uint state) +{ + fsm_build_state_chain_t *curs = fsm->states; + while (curs != NULL) + { + if (curs->state.code == state) + return &(curs->state); + curs = curs->next; + } + return NULL; +} + +uint16_t +fsm_build_get_event_code (fsm_build_t *fsm, char *event) +{ + assert (fsm); + fsm_build_event_t *e = fsm_build_get_event (fsm, event); + assert (e); + return e->code; +} + +void +fsm_build_trans (fsm_build_t *fsm, + char *state, + char *event, + char *output_branches, + fsm_build_run_strans_func_t trans_func) +{ + fsm_build_trans_t t; + fsm_build_branch_chain_t *b; + char **args; + int i, nb; + + t.output_branches = NULL; + /* Check state and event exists. */ + t.state = fsm_build_get_state (fsm, state); + t.event = fsm_build_get_event (fsm, event); + assert (t.state); + assert (t.event); + + fsm_build_arg_parse (output_branches, &args, &nb); + /* Only one output state. */ + if (nb == 1) + { + t.output_branches = (fsm_build_branch_chain_t *) + malloc (sizeof (fsm_build_branch_chain_t)); + t.output_branches->name = NULL; + t.output_branches->state = fsm_build_get_state (fsm, args[0]); + assert (t.output_branches->state); + t.output_branches->next = NULL; + } + else + for (i = 0; i < nb; i = i + 2) + { + b = (fsm_build_branch_chain_t *) + malloc (sizeof (fsm_build_branch_chain_t)); + b->name = strdup (args[i]); + b->state = fsm_build_get_state (fsm, args[i+1]); + assert (b->state); + b->next = t.output_branches; + t.output_branches = b; + } + fsm_build_arg_free (&args, nb); + + /* Add trans to fsm. */ + fsm_build_trans_chain_t *tc = (fsm_build_trans_chain_t *) + malloc (sizeof (fsm_build_trans_chain_t)); + tc->trans = t; + tc->next = fsm->trans; + fsm->trans = tc; + + /* Add trans function to run context. */ + fsm_trans_func_chain_t *fc = (fsm_trans_func_chain_t *) + malloc (sizeof (fsm_trans_func_chain_t)); + fc->func = trans_func; + fc->trans = tc->trans; + fc->next = fsm->run.func_pool; + fsm->run.func_pool = fc; +} + +void +fsm_build_timeout (fsm_build_t *fsm, char *state, char *event, uint timeout) +{ + fsm_build_trans_chain_t *tc = NULL; + fsm_build_trans_t *t = NULL; + fsm_build_state_t *s = NULL; + fsm_build_event_t *e = NULL; + fsm_build_timeout_chain_t *to = NULL; + + assert (fsm); + assert (state); + assert (event); + + /* Find the corresponding transition. */ + tc = fsm->trans; + s = fsm_build_get_state (fsm, state); + e = fsm_build_get_event (fsm, event); + assert (tc); + assert (s); + assert (e); + while (tc != NULL) + { + if (tc->trans.state == s && tc->trans.event == e) + { + t = &tc->trans; + break; + } + tc = tc->next; + } + assert (t); + + /* Fill a new timeout. */ + to = (fsm_build_timeout_chain_t *) + malloc (sizeof (fsm_build_timeout_chain_t)); + to->timeout.timeout = timeout; + to->timeout.trans = t; + + /* Add timeout to chain. */ + to->next = fsm->timeouts; + fsm->timeouts = to; +} + +void +fsm_build_start_with (fsm_build_t *fsm, char *starters) +{ + char **args; + int nb, i; + fsm_build_arg_parse (starters, &args, &nb); + fsm->max_active_states += nb; + for (i=0; i < nb; i++) + { + fsm_build_state_chain_t *sc = (fsm_build_state_chain_t *) + malloc (sizeof (fsm_build_state_chain_t)); + fsm_build_state_t *s = fsm_build_get_state (fsm, args[i]); + assert (s); + sc->state = *s; + sc->next = fsm->starters; + fsm->starters = sc; + } + fsm_build_arg_free (&args, nb); +} + +int +fsm_build_handle (fsm_build_t *fsm, fsm_build_event_t *e) +{ + fsm_build_state_t *s = NULL; + fsm_build_timeout_chain_t *toc = NULL; + assert (e); + uint i; + int handled = 0; + for (i = 0; i < fsm->max_active_states; i++) + { + s = fsm->run.active_states[i]; + if (s && fsm->run.trans_table[e->code][s->code]) + { + fsm->run.events_before_active_state[i] = e; + fsm->run.active_states[i] = fsm->run.trans_table[e->code][s->code](); + /* Check the new state has a timeout or not. */ + toc = fsm->timeouts; + fsm->run.timeout_counters[i] = -1; + while (toc != NULL) + { + if (toc->timeout.trans->state->code + == fsm->run.active_states[i]->code) + { + fsm->run.timeout_counters[i] = toc->timeout.timeout; + break; + } + toc = toc->next; + } + + handled = 1; + } + } + return handled; +} + +int +fsm_build_handle_string (fsm_build_t *fsm, char *event) +{ + fsm_build_event_t *e = fsm_build_get_event (fsm, event); + return fsm_build_handle (fsm, e); +} + +int +fsm_build_handle_integer (fsm_build_t *fsm, uint16_t event) +{ + fsm_build_event_t *e = fsm_build_get_event_by_code (fsm, event); + return fsm_build_handle (fsm, e); +} + +int +fsm_build_can_handle (fsm_build_t *fsm, fsm_build_event_t *e) +{ + fsm_build_state_t *s = NULL; + assert (e); + uint i; + for (i = 0; i < fsm->max_active_states; i++) + { + s = fsm->run.active_states[i]; + if (s && fsm->run.trans_table[e->code][s->code]) + return 1; + } + return 0; +} + +int +fsm_build_can_handle_string (fsm_build_t *fsm, char *event) +{ + fsm_build_event_t *e = fsm_build_get_event (fsm, event); + return fsm_build_can_handle (fsm, e); +} + +int +fsm_build_can_handle_integer (fsm_build_t *fsm, uint16_t event) +{ + fsm_build_event_t *e = fsm_build_get_event_by_code (fsm, event); + return fsm_build_can_handle (fsm, e); +} + +int +fsm_build_handle_timeout (fsm_build_t *fsm) +{ + int out = 0; + int i; + char *event = NULL; + for (i = 0; i < (int) fsm->max_active_states; i++) + { + if (fsm->run.timeout_counters[i] > 0) + fsm->run.timeout_counters[i]--; + /* We have a timeout event. */ + if (fsm->run.timeout_counters[i] == 0) + { + fsm->run.timeout_counters[i] = -1; + /* build event string */ + event = (char *) malloc ( + (strlen (fsm->run.active_states[i]->var_name) + + strlen ("_TIMEOUT") + 1) * sizeof (char)); + sprintf (event, "%s_TIMEOUT", fsm->run.active_states[i]->var_name); + fsm_build_handle_string (fsm, event); + out = 1; + } + } + return out; +} + +fsm_build_state_t* +fsm_build_get_next_state (fsm_build_t *fsm, + char *state, + char *event, + char *branch) +{ + fsm_build_state_t *s; + fsm_build_event_t *e; + fsm_build_trans_chain_t *t_curs; + fsm_build_branch_chain_t *b_curs; + + /* Convert input data. */ + s = fsm_build_get_state (fsm, state); + e = fsm_build_get_event (fsm, event); + assert (s && e); + + /* Get transition. */ + t_curs = fsm->trans; + while (t_curs != NULL) + { + if (s == t_curs->trans.state && e == t_curs->trans.event) + break; + t_curs = t_curs->next; + } + assert (t_curs); + assert (t_curs->trans.output_branches); + + /* If we have only one branch. */ + if (strlen (branch) == 0) + { + /* Branch has to be given is there are multiple branches. */ + assert (t_curs->trans.output_branches->next == NULL); + fsm_build_print (fsm, &t_curs->trans, t_curs->trans.output_branches); + return t_curs->trans.output_branches->state; + } + + /* Find correct branch. */ + b_curs = t_curs->trans.output_branches; + while (b_curs != NULL) + { + if (strcmp (b_curs->name, branch) == 0) + break; + b_curs = b_curs->next; + } + assert (b_curs); + fsm_build_print (fsm, &t_curs->trans, b_curs); + return b_curs->state; +} + +void +fsm_build_gen_avr_h (fsm_build_t *fsm, uint embedded_strings) +{ + fsm_build_state_chain_t *sc; + fsm_build_event_chain_t *ec; + fsm_build_trans_chain_t *tc; + fsm_build_branch_chain_t *bc; + fsm_build_state_t *s; + fsm_build_event_t *e; + fsm_build_chain_t *all_fsm; + uint i, j; + + /* Open file. */ + char *fn = (char *) malloc ((strlen (fsm->name) + + strlen ("fsm_gen_.h") + 1) * sizeof (char)); + sprintf (fn, "fsm_%s_gen.h", fsm->name); + FILE *f = fopen (fn, "w"); + + /* Introduction. */ + fprintf (f, "/* This file has been generated, do not edit. */\n\n"); + fprintf (f, "#ifndef _FSM_%s_\n", fsm->name); + fprintf (f, "#define _FSM_%s_\n\n", fsm->name); + fprintf (f, "#include \n"); + fprintf (f, "#include \n\n"); + + /* Include all other fsm headers. */ + all_fsm = fsm_build_all_fsm; + while (all_fsm != NULL) + { + fprintf (f, "#include \"fsm_%s_gen.h\"\n",all_fsm->fsm->name); + all_fsm = all_fsm->next; + } + + /* Gen max active states define */ + fprintf (f, "#define fsm_%s_max_active_states %u\n", + fsm->name, + fsm->max_active_states); + + /* Gen state enum. */ + fprintf (f, "typedef enum\n{\n"); + sc = fsm->states; + while (sc != NULL) + { + fprintf (f, "\tFSM_STATE_%s_%s = %u,\n", fsm->name, sc->state.var_name, sc->state.code); + sc = sc->next; + } + fprintf (f, "\tFSM_STATE_%s_NB_ = %u\n", fsm->name, fsm->state_nb); + fprintf (f, "} fsm_%s_state_t;\n\n", fsm->name); + + /* Gen event enum. */ + fprintf (f, "typedef enum\n{\n"); + ec = fsm->events; + while (ec != NULL) + { + fprintf (f, "\tFSM_EVENT_%s_%s = %u,\n", fsm->name, ec->event.var_name, ec->event.code); + ec = ec->next; + } + fprintf (f, "\tFSM_EVENT_%s_NB_ = %u\n", fsm->name, fsm->event_nb); + fprintf (f, "} fsm_%s_event_t;\n\n", fsm->name); + + /* Gen state strings. */ + if (embedded_strings) + { + sc = fsm->states; + while (sc != NULL) + { + fprintf (f, "extern prog_char fsm_%s_state_str_%s[%u] PROGMEM;\n", + fsm->name, + sc->state.var_name, + strlen (sc->state.var_name) + 1); + sc = sc->next; + } + fprintf (f, "extern const char *fsm_%s_state_str[%u] PROGMEM;\n\n", + fsm->name, + fsm->state_nb); + + /* Gen event strings. */ + ec = fsm->events; + while (ec != NULL) + { + fprintf (f, "extern prog_char fsm_%s_event_str_%s[%u] PROGMEM;\n", + fsm->name, + ec->event.var_name, + strlen (ec->event.var_name) + 1); + ec = ec->next; + } + fprintf (f, "extern const char *fsm_%s_event_str[%u] PROGMEM;\n\n", + fsm->name, + fsm->event_nb); + + /* Create a RAM string able to store event or state string. */ + j = 0; + for (i = 0; i < fsm->event_nb; i++) + { + e = fsm_build_get_event_by_code (fsm, i); + if (strlen (e->var_name) > j) + j = strlen (e->var_name); + } + for (i = 0; i < fsm->state_nb; i++) + { + s = fsm_build_get_state_by_code (fsm, i); + if (strlen (s->var_name) > j) + j = strlen (s->var_name); + } + fprintf (f, "extern char fsm_%s_str_buff[%u];\n", fsm->name, j + 1); + + /* Convert an event enum in string. */ + fprintf (f, "char *\nfsm_%s_get_event_string_from_enum \ + (fsm_%s_event_t e);\n", fsm->name, fsm->name); + + /* Convert a event string in enum. */ + fprintf (f, "fsm_%s_event_t\nfsm_%s_get_event_enum_from_string \ + (char *str);\n", fsm->name, fsm->name); + + /* Convert an state enum in string. */ + fprintf (f, "char *\nfsm_%s_get_state_string_from_enum \ + (fsm_%s_state_t s);\n", fsm->name, fsm->name); + + /* Convert a state string in enum. */ + fprintf (f, "fsm_%s_state_t\nfsm_%s_get_state_enum_from_string \ + (char *str);\n", fsm->name, fsm->name); + } + + /* Gen transitions branches enum. */ + fprintf (f, "typedef enum\n{\n"); + tc = fsm->trans; + while (tc != NULL) + { + bc = tc->trans.output_branches; + while (bc != NULL) + { + if (bc->name != NULL) + fprintf (f, "\tFSM_BRANCH_%s_%s_%s_%s = %u,\n", + fsm->name, + tc->trans.state->var_name, + tc->trans.event->var_name, + bc->name, + bc->state->code); + else + fprintf (f, "\tFSM_BRANCH_%s_%s_%s_ = %u,\n", + fsm->name, + tc->trans.state->var_name, + tc->trans.event->var_name, + bc->state->code); + bc = bc->next; + } + tc = tc->next; + } + fprintf (f, "} fsm_%s_branch_t;\n\n", fsm->name); + + /* Gen function headers. */ + tc = fsm->trans; + while (tc != NULL) + { + fprintf (f, "fsm_%s_branch_t fsm_%s_trans_func_%s_%s (void);\n", + fsm->name, + fsm->name, + tc->trans.state->var_name, + tc->trans.event->var_name); + tc = tc->next; + } + fprintf (f, "\n"); + + /* Gen function table. */ + fprintf (f, "typedef fsm_%s_branch_t (*fsm_%s_func_t)(void);\n", fsm->name, + fsm->name); + fprintf (f, "extern const fsm_%s_func_t PROGMEM fsm_%s_trans_table[%u][%u];\n\n", + fsm->name, + fsm->name, + fsm->event_nb, + fsm->state_nb); + + /* Gen active states array. */ + fprintf (f, "extern fsm_%s_state_t fsm_%s_active_states[%u];\n\n", + fsm->name, + fsm->name, + fsm->max_active_states); + + /* Gen initialization function. */ + sc = fsm->starters; + i = 0; + fprintf (f, "void\nfsm_%s_init () __attribute__ ((constructor));\n\n", + fsm->name); + + /* Gen handle function. */ + fprintf (f, "int\nfsm_%s_handle (fsm_%s_event_t e);\n", + fsm->name, + fsm->name); + + /* Gen can handle function. */ + fprintf (f, "uint16_t\nfsm_%s_can_handle (fsm_%s_event_t e);\n\n", + fsm->name, + fsm->name); + + if (fsm->timeouts != NULL) + { + /* Gen handle timeout function. */ + fprintf (f, "int\nfsm_%s_handle_timeout ();\n", + fsm->name); + + /* Gen timeout values. */ + fprintf (f, "extern int32_t fsm_%s_timeout_values[FSM_STATE_%s_NB_];\n", + fsm->name, + fsm->name); + + /* Gen timeout corresponding events. */ + fprintf (f, "extern fsm_%s_event_t fsm_%s_timeout_events[FSM_STATE_%s_NB_];\n", + fsm->name, + fsm->name, + fsm->name); + + /* Gen timeout counters array. */ + fprintf (f, "extern int32_t fsm_%s_timeout_counters[%u];\n\n", + fsm->name, + fsm->max_active_states); + } + + /* Conclusion. */ + fprintf (f, "#endif /* #ifndef _FSM_%s_ */", fsm->name), + + /* Close file. */ + fclose (f); + + /* Free. */ + free (fn); +} + +void +fsm_build_gen_avr_c (fsm_build_t *fsm, uint embedded_strings) +{ + fsm_build_state_chain_t *sc; + fsm_build_event_chain_t *ec; + fsm_build_trans_chain_t *tc; + fsm_build_timeout_chain_t *toc; + fsm_build_state_t *s; + fsm_build_event_t *e; + uint i, j, found; + + /* Open file. */ + char *fn = (char*) malloc ((strlen (fsm->name) + + strlen ("fsm_gen_.c") + 1) * sizeof (char)); + sprintf (fn, "fsm_%s_gen.c", fsm->name); + FILE *f = fopen (fn, "w"); + + /* Introduction. */ + fprintf (f, "/* This file has been generated, do not edit. */\n\n"); + fprintf (f, "#include \"fsm_%s_gen.h\"\n\n", fsm->name); + + /* Gen state strings. */ + if (embedded_strings) + { + sc = fsm->states; + while (sc != NULL) + { + fprintf (f, "prog_char fsm_%s_state_str_%s[] PROGMEM = \"%s\";\n", + fsm->name, + sc->state.var_name, + sc->state.var_name); + sc = sc->next; + } + fprintf (f, "const char *fsm_%s_state_str[] PROGMEM =\n{\n", fsm->name); + for (i = 0; i < fsm->state_nb; i++) + { + s = fsm_build_get_state_by_code (fsm, i); + fprintf (f, "\tfsm_%s_state_str_%s", fsm->name, s->var_name); + if (i == fsm->state_nb - 1) + fprintf (f, "\n"); + else + fprintf (f, ",\n"); + } + fprintf (f, "};\n\n"); + + /* Gen event strings. */ + ec = fsm->events; + while (ec != NULL) + { + fprintf (f, "prog_char fsm_%s_event_str_%s[] PROGMEM = \"%s\";\n", + fsm->name, + ec->event.var_name, + ec->event.var_name); + ec = ec->next; + } + fprintf (f, "const char *fsm_%s_event_str[] PROGMEM =\n{\n", fsm->name); + for (i = 0; i < fsm->event_nb; i++) + { + e = fsm_build_get_event_by_code (fsm, i); + fprintf (f, "\tfsm_%s_event_str_%s", fsm->name, e->var_name); + if (i == fsm->event_nb - 1) + fprintf (f, "\n"); + else + fprintf (f, ",\n"); + } + fprintf (f, "};\n\n"); + + /* Create a RAM string able to store event or state string. */ + j = 0; + for (i = 0; i < fsm->event_nb; i++) + { + e = fsm_build_get_event_by_code (fsm, i); + if (strlen (e->var_name) > j) + j = strlen (e->var_name); + } + for (i = 0; i < fsm->state_nb; i++) + { + s = fsm_build_get_state_by_code (fsm, i); + if (strlen (s->var_name) > j) + j = strlen (s->var_name); + } + fprintf (f, "char fsm_%s_str_buff[%u];\n", fsm->name, j + 1); + + /* Convert an event enum in string. */ + fprintf (f, "char *\nfsm_%s_get_event_string_from_enum \ + (fsm_%s_event_t e)\n{\n", fsm->name, fsm->name); + fprintf (f, "\treturn strcpy_P (fsm_%s_str_buff, \ + (char *) pgm_read_word (&(fsm_%s_event_str[e])));\n", fsm->name, fsm->name); + fprintf (f, "}\n\n"); + + /* Convert a event string in enum. */ + fprintf (f, "fsm_%s_event_t\nfsm_%s_get_event_enum_from_string \ + (char *str)\n{\n", fsm->name, fsm->name); + fprintf (f, "\tuint16_t i;\n"); + fprintf (f, "\tfor (i = 0; i < FSM_EVENT_%s_NB_; i++)\n", fsm->name); + fprintf (f, "\t\tif (strcpy_P (str, \ + (char *) pgm_read_word (&(fsm_%s_event_str[i]))) == 0)\n", fsm->name); + fprintf (f, "\t\t\treturn i;\n"); + fprintf (f, "\treturn FSM_EVENT_%s_NB_;\n", fsm->name); + fprintf (f, "}\n\n"); + + /* Convert an state enum in string. */ + fprintf (f, "char *\nfsm_%s_get_state_string_from_enum \ + (fsm_%s_state_t s)\n{\n", fsm->name, fsm->name); + fprintf (f, "\treturn strcpy_P (fsm_%s_str_buff, \ + (char *) pgm_read_word (&(fsm_%s_state_str[s])));\n", fsm->name, fsm->name); + fprintf (f, "}\n\n"); + + /* Convert a state string in enum. */ + fprintf (f, "fsm_%s_state_t\nfsm_%s_get_state_enum_from_string \ + (char *str)\n{\n", fsm->name, fsm->name); + fprintf (f, "\tuint16_t i;\n"); + fprintf (f, "\tfor (i = 0; i < FSM_STATE_%s_NB_; i++)\n", fsm->name); + fprintf (f, "\t\tif (strcpy_P (str, \ + (char *) pgm_read_word (&(fsm_%s_state_str[i]))) == 0)\n", fsm->name); + fprintf (f, "\t\t\treturn i;\n"); + fprintf (f, "\treturn FSM_STATE_%s_NB_;\n", fsm->name); + fprintf (f, "}\n\n"); + } + + /* Gen function table. */ + fprintf (f, "const fsm_%s_func_t PROGMEM fsm_%s_trans_table[%u][%u] = \n{\n", + fsm->name, + fsm->name, + fsm->event_nb, + fsm->state_nb); + /* for each events and state, see if it exists an associated transition. */ + for (i = 0; i < fsm->event_nb; i++) + { + e = fsm_build_get_event_by_code (fsm, i); + fprintf (f, "\t{"); + for (j = 0; j < fsm->state_nb; j++) + { + s = fsm_build_get_state_by_code (fsm, j); + tc = fsm->trans; + found = 0; + while (tc != NULL) + { + if (tc->trans.state == s && tc->trans.event == e) + { + found = 1; + fprintf (f, "&fsm_%s_trans_func_%s_%s", + fsm->name, + tc->trans.state->var_name, + tc->trans.event->var_name); + tc = tc->next; + break; + } + tc = tc->next; + } + if (!found) + fprintf (f, "(fsm_%s_func_t) 0", fsm->name); + if (j == fsm->state_nb - 1) + fprintf (f, "}"); + else + fprintf (f, ", "); + } + if (i != fsm->event_nb - 1) + fprintf (f, ","); + fprintf (f, "\n"); + } + fprintf (f, "};\n\n"); + + /* Gen active states array. */ + fprintf (f, "fsm_%s_state_t fsm_%s_active_states[%u];\n\n", + fsm->name, + fsm->name, + fsm->max_active_states); + + /* Gen initialization function. */ + sc = fsm->starters; + i = 0; + fprintf (f, "void\nfsm_%s_init ()\n{\n", fsm->name); + while (sc != NULL) + { + fprintf (f, "\tfsm_%s_active_states[%u] = %u;\n", + fsm->name, + i, + sc->state.code); + if (fsm->timeouts != NULL) + { + toc = fsm->timeouts; + while (toc != NULL) + { + if (toc->timeout.trans->state->code == sc->state.code) + { + fprintf (f, "\tfsm_%s_timeout_counters[%u] = %u;\n", + fsm->name, + i, + toc->timeout.timeout); + } + toc = toc->next; + } + } + i++; + sc = sc->next; + } + fprintf (f, "}\n\n"); + + /* Gen handle function. */ + fprintf (f, "int\nfsm_%s_handle (fsm_%s_event_t e)\n{\n", + fsm->name, + fsm->name); + fprintf (f, "\tuint16_t i;\n"); + fprintf (f, "\tint handled = 0;\n"); + fprintf (f, "\tfor (i = 0; i < fsm_%s_max_active_states; i++)\n\t{\n", + fsm->name); + fprintf (f, "\t\tif (fsm_%s_trans_table[e][fsm_%s_active_states[i]])\n", + fsm->name, + fsm->name); + fprintf (f, "\t\t{\n"); + fprintf (f, "\t\t\tfsm_%s_active_states[i] = \ + fsm_%s_trans_table[e][fsm_%s_active_states[i]]();\n", + fsm->name, + fsm->name, + fsm->name); + fprintf (f, "\t\t\thandled = 1;\n"); + if (fsm->timeouts != NULL) + { + fprintf (f, "\t\t\tfsm_%s_timeout_counters[i] = fsm_%s_timeout_values[e];\n", + fsm->name, + fsm->name); + } + fprintf (f, "\t\t}\n"); + fprintf (f, "\t}\n"); + fprintf (f, "\treturn handled;\n"); + fprintf (f, "}\n\n"); + + /* Gen can handle function. */ + fprintf (f, "uint16_t\nfsm_%s_can_handle (fsm_%s_event_t e)\n{\n", + fsm->name, + fsm->name); + fprintf (f, "\tuint16_t i;\n"); + fprintf (f, "\tfor (i = 0; i < fsm_%s_max_active_states; i++)\n", + fsm->name); + fprintf (f, "\t\tif (fsm_%s_trans_table[e][fsm_%s_active_states[i]])\n", + fsm->name, + fsm->name); + fprintf (f, "\t\t\treturn 1;\n"); + fprintf (f, "\treturn 0;\n"); + fprintf (f, "}\n\n"); + + if (fsm->timeouts != NULL) + { + /* Gen timeout counters array. */ + fprintf (f, "int32_t fsm_%s_timeout_counters[%u];\n", + fsm->name, + fsm->max_active_states); + + /* Gen timeout values array. */ + fprintf (f, "int32_t fsm_%s_timeout_values[FSM_STATE_%s_NB_] =\n{\n", + fsm->name, + fsm->name); + int value; + for (i = 0; i < fsm->state_nb; i++) + { + value = -1; + s = fsm_build_get_state_by_code (fsm, i); + assert (s); + toc = fsm->timeouts; + while (toc != NULL) + { + if (s->code == toc->timeout.trans->state->code) + { + value = toc->timeout.timeout; + break; + } + toc = toc->next; + } + fprintf (f, "\t%i", value); + if (i != fsm->state_nb - 1) + fprintf (f, ","); + fprintf (f, "\n"); + } + fprintf (f, "};\n\n"); + + /* Gen timeout corresponding events array. */ + fprintf (f, "fsm_%s_event_t fsm_%s_timeout_events[FSM_STATE_%s_NB_] =\n{\n", + fsm->name, + fsm->name, + fsm->name); + for (i = 0; i < fsm->state_nb; i++) + { + value = -1; + s = fsm_build_get_state_by_code (fsm, i); + assert (s); + toc = fsm->timeouts; + while (toc != NULL) + { + if (s->code == toc->timeout.trans->state->code) + { + value = toc->timeout.trans->event->code; + break; + } + toc = toc->next; + } + if (value == -1) + fprintf (f, "\tFSM_STATE_%s_NB_", fsm->name); + else + fprintf (f, "\t%u", value); + + if (i != fsm->state_nb - 1) + fprintf (f, ","); + fprintf (f, "\n"); + } + fprintf (f, "};\n\n"); + + /* Gen handle timeout function. */ + fprintf (f, "int\nfsm_%s_handle_timeout ()\n{\n", + fsm->name); + fprintf (f, "\tuint16_t i;\n"); + fprintf (f, "\tint out = 0;\n"); + fprintf (f, "\tfor (i = 0; i < fsm_%s_max_active_states; i++)\n\t{\n", + fsm->name); + fprintf (f, "\t\tif (fsm_%s_timeout_counters[i] > 0)\n", + fsm->name); + fprintf (f, "\t\t\tfsm_%s_timeout_counters[i]--;\n", + fsm->name); + fprintf (f, "\t\tif (fsm_%s_timeout_counters[i] == 0)\n\t\t{\n", + fsm->name); + fprintf (f, "\t\t\tfsm_%s_handle (fsm_%s_timeout_events[fsm_%s_active_states[i]]);\n", + fsm->name, + fsm->name, + fsm->name); + fprintf (f, "\t\t\tout = 1;\n"); + fprintf (f, "\t\t}\n\n"); + fprintf (f, "\t}\n"); + fprintf (f, "\treturn out;\n"); + fprintf (f, "}\n\n"); + } + + /* Close file. */ + fclose (f); + + /* Free. */ + free (fn); +} + +void +fsm_build_gen (char *arch, uint embedded_strings) +{ + fsm_build_chain_t *curs = fsm_build_all_fsm; + while (curs != NULL) + { + if (strcmp (arch, "AVR") == 0) + { + fsm_build_gen_avr_h (curs->fsm, embedded_strings); + fsm_build_gen_avr_c (curs->fsm, embedded_strings); + } + curs = curs->next; + } +} + +void +fsm_build_free (fsm_build_t *fsm) +{ + fsm_build_state_chain_t *sc; + fsm_build_state_chain_t *sc_tmp; + fsm_build_event_chain_t *ec; + fsm_build_event_chain_t *ec_tmp; + fsm_build_trans_chain_t *tc; + fsm_build_trans_chain_t *tc_tmp; + fsm_build_branch_chain_t *bc; + fsm_build_branch_chain_t *bc_tmp; + fsm_trans_func_chain_t *fc; + fsm_trans_func_chain_t *fc_tmp; + fsm_build_timeout_chain_t *toc; + fsm_build_timeout_chain_t *toc_tmp; + uint i; + + /* Free states. */ + sc = fsm->states; + while (sc != NULL) + { + sc_tmp = sc; + free (sc->state.var_name); + sc = sc->next; + free (sc_tmp); + } + + /* Free events. */ + ec = fsm->events; + while (sc != NULL) + { + ec_tmp = ec; + free (ec->event.var_name); + ec = ec->next; + free (ec_tmp); + } + + /* Free trans */ + tc = fsm->trans; + while (tc != NULL) + { + tc_tmp = tc; + + /* Free each branches. */ + bc = tc->trans.output_branches; + while (bc != NULL) + { + bc_tmp = bc; + free (bc->name); + bc = bc->next; + free (bc_tmp); + } + + tc = tc->next; + free (tc_tmp); + } + + /* Free start chain. */ + sc = fsm->starters; + while (sc != NULL) + { + sc_tmp = sc; + sc = sc->next; + free (sc_tmp); + } + + /* Free timeout chain. */ + toc = fsm->timeouts; + while (toc != NULL) + { + toc_tmp = toc; + toc = toc->next; + free (toc_tmp); + } + + /* Free run data (trans_table). */ + for (i = 0; i < fsm->event_nb; i++) + free (fsm->run.trans_table[i]); + free (fsm->run.trans_table); + + /* Free run data (active states). */ + free (fsm->run.active_states); + + /* Free last-seen event array*/ + free (fsm->run.events_before_active_state); + + /* Free run data (function pool). */ + fc = fsm->run.func_pool; + while (fc != NULL) + { + fc_tmp = fc; + fc = fc->next; + free (fc_tmp); + } + + /*Free run data (timeout counters). */ + free (fsm->run.timeout_counters); +} + diff --git a/digital/io/src/fsm_templates/template_cb.h b/digital/io/src/fsm_templates/template_cb.h deleted file mode 100644 index 4f4deeec..00000000 --- a/digital/io/src/fsm_templates/template_cb.h +++ /dev/null @@ -1,10 +0,0 @@ -#ifndef %(prefix)s_cb_h -#define %(prefix)s_cb_h -/* - * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT! - * - * %(name)s callbacks declaration. - * -%(*comments)s */ - -%(states_template,template_cb_decl.h)s#endif /* %(prefix)s_cb_h */ diff --git a/digital/io/src/fsm_templates/template_cb_decl.h b/digital/io/src/fsm_templates/template_cb_decl.h deleted file mode 100644 index fd72d678..00000000 --- a/digital/io/src/fsm_templates/template_cb_decl.h +++ /dev/null @@ -1,6 +0,0 @@ -/* - * %(state)s =%(event)s=> -%(*branches_to)s */ -fsm_branch_t -%(prefix)s__%(state)s__%(event)s (void); - diff --git a/digital/io/src/fsm_templates/template_cb_impl.c b/digital/io/src/fsm_templates/template_cb_impl.c deleted file mode 100644 index 91ff4fbb..00000000 --- a/digital/io/src/fsm_templates/template_cb_impl.c +++ /dev/null @@ -1,9 +0,0 @@ -/* - * %(state)s =%(event)s=> -%(*branches_to)s */ -fsm_branch_t -%(prefix)s__%(state)s__%(event)s (void) -{ -%(returns)s -} - diff --git a/digital/io/src/fsm_templates/template_fsm.c b/digital/io/src/fsm_templates/template_fsm.c deleted file mode 100644 index 03ce5abf..00000000 --- a/digital/io/src/fsm_templates/template_fsm.c +++ /dev/null @@ -1,55 +0,0 @@ -/* - * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT! - * - * %(name)s - * -%(*comments)s */ -#include "common.h" -#include "fsm.h" -#include "%(prefix)s_cb.h" - -#define NULL ((void *)0L) - -/* %(name)s transition table. */ -static const fsm_transition_t PROGMEM -%(prefix)s_transition_table[%(PREFIX)s_STATE_NB][%(PREFIX)s_EVENT_NB] = { -%(transition_table)s}; - -/* %(name)s state timeout table. */ -static const uint16_t -%(prefix)s_state_timeout_table[%(PREFIX)s_STATE_NB] = { -%(states,%(@timeout|0xffff)s)s}; - -#ifdef HOST - -/* %(name)s state names. */ -static const char * -%(prefix)s_states_names[] = { -%(states_names)s}; - -/* %(name)s event names. */ -static const char * -%(prefix)s_events_names[] = { -%(events_names)s}; - -#endif /* HOST */ - -/* %(name)s context. */ -fsm_t %(prefix)s_fsm = { - &%(prefix)s_transition_table[0][0], - %(PREFIX)s_EVENT_NB, - { - %(initials)s}, - { - %(initials)s}, - %(initials_nb)s, - &%(prefix)s_state_timeout_table[0], - { 0 }, - %(PREFIX)s_EVENT_state_timeout, -#ifdef HOST - "%(name)s", - %(prefix)s_states_names, - %(prefix)s_events_names, -#endif -}; - diff --git a/digital/io/src/fsm_templates/template_fsm.h b/digital/io/src/fsm_templates/template_fsm.h deleted file mode 100644 index 9ac98895..00000000 --- a/digital/io/src/fsm_templates/template_fsm.h +++ /dev/null @@ -1,52 +0,0 @@ -#ifndef %(prefix)s_fsm_h -#define %(prefix)s_fsm_h -/* - * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT! - * - * %(name)s - * -%(*comments)s */ - -/* %(name)s states. */ -enum %(prefix)s_state_t -{ -%(states)s %(PREFIX)s_STATE_NB -}; - -/* %(name)s events. */ -enum %(prefix)s_event_t -{ -%(events)s %(PREFIX)s_EVENT_NB -}; - -/* This macro enables checks for branches used in the wrong state/event - * combination. */ -#ifdef HOST -# define _BRANCH(state, event, to) \ - ((%(PREFIX)s_STATE_ ## state) << 16 \ - | (%(PREFIX)s_EVENT_ ## event) << 8 \ - | (%(PREFIX)s_STATE_ ## to)) -#else -# define _BRANCH(state, event, to) \ - ((%(PREFIX)s_STATE_ ## to)) -#endif - -/* %(name)s branches. */ -enum %(prefix)s_branch_t -{ -%(branches)s}; - -#undef _BRANCH - -/* Value to return to follow the only branch. */ -#define %(prefix)s_next(state, event) \ - %(PREFIX)s_BRANCH__ ## state ## __ ## event ## __ - -/* Value to return to follow a given branch. */ -#define %(prefix)s_next_branch(state, event, branch) \ - %(PREFIX)s_BRANCH__ ## state ## __ ## event ## __ ## branch - -/* %(name)s context. */ -extern fsm_t %(prefix)s_fsm; - -#endif /* %(prefix)s_fsm_h */ diff --git a/digital/io/src/fsm_templates/template_hola_cb_skel.c b/digital/io/src/fsm_templates/template_hola_cb_skel.c deleted file mode 100644 index 980718da..00000000 --- a/digital/io/src/fsm_templates/template_hola_cb_skel.c +++ /dev/null @@ -1,11 +0,0 @@ -/* - * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT! - * - * Skeleton for %(name)s hola callbacks implementation. - * -%(*comments)s */ -#include "common.h" -#include "fsm.h" -#include "%(prefix)s_cb.h" - -%(states_template,template_cb_impl.c,origin=hola)s diff --git a/digital/io/src/fsm_templates/template_init_cb_skel.c b/digital/io/src/fsm_templates/template_init_cb_skel.c deleted file mode 100644 index fea9270e..00000000 --- a/digital/io/src/fsm_templates/template_init_cb_skel.c +++ /dev/null @@ -1,11 +0,0 @@ -/* - * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT! - * - * Skeleton for %(name)s init callbacks implementation. - * -%(*comments)s */ -#include "common.h" -#include "fsm.h" -#include "%(prefix)s_cb.h" - -%(states_template,template_cb_impl.c,origin=init)s diff --git a/digital/io/src/fsm_templates/template_loader_cb_skel.c b/digital/io/src/fsm_templates/template_loader_cb_skel.c deleted file mode 100644 index 06398952..00000000 --- a/digital/io/src/fsm_templates/template_loader_cb_skel.c +++ /dev/null @@ -1,11 +0,0 @@ -/* - * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT! - * - * Skeleton for %(name)s loader callbacks implementation. - * -%(*comments)s */ -#include "common.h" -#include "fsm.h" -#include "%(prefix)s_cb.h" - -%(states_template,template_cb_impl.c,origin=loader)s diff --git a/digital/io/src/fsm_templates/template_move_cb_skel.c b/digital/io/src/fsm_templates/template_move_cb_skel.c deleted file mode 100644 index 533ffbfd..00000000 --- a/digital/io/src/fsm_templates/template_move_cb_skel.c +++ /dev/null @@ -1,11 +0,0 @@ -/* - * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT! - * - * Skeleton for %(name)s move callbacks implementation. - * -%(*comments)s */ -#include "common.h" -#include "fsm.h" -#include "%(prefix)s_cb.h" - -%(states_template,template_cb_impl.c,origin=move)s diff --git a/digital/io/src/fsm_templates/template_top_cb_skel.c b/digital/io/src/fsm_templates/template_top_cb_skel.c deleted file mode 100644 index 8895dc64..00000000 --- a/digital/io/src/fsm_templates/template_top_cb_skel.c +++ /dev/null @@ -1,11 +0,0 @@ -/* - * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT! - * - * Skeleton for %(name)s top callbacks implementation. - * -%(*comments)s */ -#include "common.h" -#include "fsm.h" -#include "%(prefix)s_cb.h" - -%(states_template,template_cb_impl.c,origin=top)s diff --git a/digital/io/src/hola.c b/digital/io/src/hola.c index 1fd15f18..c7d07e21 100644 --- a/digital/io/src/hola.c +++ b/digital/io/src/hola.c @@ -23,3 +23,118 @@ * * }}} */ +#define FSM_NAME AI + +#include "common.h" +#include "fsm.h" +#include "asserv.h" +#include "mimot.h" +#include "bot.h" +#include "playground.h" + +#include "modules/math/fixed/fixed.h" + +uint32_t hola_angle; +int16_t hola_timeout; + +FSM_STATES ( + /* waiting start */ + HOLA_IDLE, + /* wait hola signal */ + HOLA_WAIT_HOLA, + /* wait until the jack is inserted for the first time */ + HOLA_WAIT_JACK_IN, + /* wait until the jack is removed to initialise robot */ + HOLA_WAIT_JACK_OUT, + /* moving clamps in rounds */ + HOLA_ROUNDS, + /* moving clamp up */ + HOLA_UP, + HOLA_CENTER, + HOLA_CLAP_CLOSE, + HOLA_CLAP_OPEN) + +FSM_EVENTS (hola_start) + +FSM_START_WITH (HOLA_IDLE) + +FSM_TRANS (HOLA_IDLE, start, HOLA_WAIT_HOLA) +{ + return FSM_NEXT (HOLA_IDLE, start); +} + +FSM_TRANS (HOLA_WAIT_HOLA, hola_start, HOLA_WAIT_JACK_IN) +{ + return FSM_NEXT (HOLA_WAIT_HOLA, hola_start); +} + +FSM_TRANS (HOLA_WAIT_JACK_IN, jack_inserted_into_bot, HOLA_WAIT_JACK_OUT) +{ + return FSM_NEXT (HOLA_WAIT_JACK_IN, jack_inserted_into_bot); +} + +FSM_TRANS (HOLA_WAIT_JACK_OUT, jack_removed_from_bot, HOLA_ROUNDS) +{ + return FSM_NEXT (HOLA_WAIT_JACK_OUT, jack_removed_from_bot); +} + +FSM_TRANS (HOLA_ROUNDS, asserv_last_cmd_ack, + /* move clamp rounds */ + no_timeout, HOLA_ROUNDS, + /* move up, rotate */ + timeout, HOLA_UP) +{ + if (++hola_timeout < 150) + { + int32_t cos = fixed_cos_f824 (hola_angle) + 0x1000000; + int32_t sin = fixed_sin_f824 (hola_angle) + 0x1000000; + int32_t dx = BOT_CLAMP_OPEN_STEP + + fixed_mul_f824 (BOT_CLAMP_STROKE_STEP / 3 / 2, cos); + int32_t dy = BOT_ELEVATOR_ZERO_SPEED + + fixed_mul_f824 (BOT_ELEVATOR_REST_STEP / 3 / 2, sin); + mimot_move_motor0_absolute (dx, BOT_CLAMP_SPEED); + mimot_move_motor1_absolute (dx, BOT_CLAMP_SPEED); + asserv_move_motor0_absolute (dy, BOT_ELEVATOR_SPEED); + hola_angle += 0x1000000 / (225 / 12); + return FSM_NEXT (HOLA_ROUNDS, asserv_last_cmd_ack, no_timeout); + } + else + { + asserv_set_speed (0x10, 0x1c, 0x10, 0x1c); + asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED / 3); + asserv_move_angularly (POSITION_A_DEG (174)); + return FSM_NEXT (HOLA_ROUNDS, asserv_last_cmd_ack, timeout); + } +} + +FSM_TRANS (HOLA_UP, bot_move_succeed, HOLA_CENTER) +{ + asserv_set_speed (0x10, 0x20, 0x10, 0x20); + asserv_move_angularly (POSITION_A_DEG (-90)); + asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP / 2, BOT_ELEVATOR_SPEED); + return FSM_NEXT (HOLA_UP, bot_move_succeed); +} + +FSM_TRANS (HOLA_CENTER, bot_move_succeed, HOLA_CLAP_CLOSE) +{ + mimot_motor0_clamp (BOT_CLAMP_ZERO_SPEED, 0); + mimot_motor1_clamp (BOT_CLAMP_ZERO_SPEED, 0); + return FSM_NEXT (HOLA_CENTER, bot_move_succeed); +} + +FSM_TRANS (HOLA_CLAP_CLOSE, clamp_succeed, HOLA_CLAP_OPEN) +{ + mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + return FSM_NEXT (HOLA_CLAP_CLOSE, clamp_succeed); +} + +/* + * close clamp + */ +FSM_TRANS (HOLA_CLAP_OPEN, clamp_succeed, HOLA_CLAP_CLOSE) +{ + mimot_motor0_clamp (BOT_CLAMP_SPEED, 0); + mimot_motor1_clamp (BOT_CLAMP_SPEED, 0); + return FSM_NEXT (HOLA_CLAP_OPEN, clamp_succeed); +} diff --git a/digital/io/src/hola.fsm b/digital/io/src/hola.fsm deleted file mode 100644 index ca219491..00000000 --- a/digital/io/src/hola.fsm +++ /dev/null @@ -1,59 +0,0 @@ -# Hola FSM -hola - entertain people - -States: - *HOLA_IDLE - waiting start - HOLA_WAIT_HOLA - wait hola signal - HOLA_WAIT_JACK_IN - wait until the jack is inserted for the first time - HOLA_WAIT_JACK_OUT - wait until the jack is removed to initialise robot - HOLA_ROUNDS - moving clamps in rounds - HOLA_UP - moving clamp up - HOLA_CENTER - HOLA_CLAP_CLOSE - HOLA_CLAP_OPEN - -Events: - hola_start - start hola - -HOLA_IDLE: - start -> HOLA_WAIT_HOLA - -HOLA_WAIT_HOLA: - hola_start -> HOLA_WAIT_JACK_IN - -HOLA_WAIT_JACK_IN: - jack_inserted_into_bot -> HOLA_WAIT_JACK_OUT - -HOLA_WAIT_JACK_OUT: - jack_removed_from_bot -> HOLA_ROUNDS - -HOLA_ROUNDS: - asserv_last_cmd_ack: no_timeout -> . - move clamp rounds - asserv_last_cmd_ack: timeout -> HOLA_UP - move up - rotate - -HOLA_UP: - bot_move_succeed -> HOLA_CENTER - rotate to center - -HOLA_CENTER: - bot_move_succeed -> HOLA_CLAP_CLOSE - close clamp - -HOLA_CLAP_CLOSE: - clamp_succeed -> HOLA_CLAP_OPEN - open clamp - -HOLA_CLAP_OPEN: - clamp_succeed -> HOLA_CLAP_CLOSE - close clamp diff --git a/digital/io/src/init.c b/digital/io/src/init.c index 1d0d8d82..4cb2bcd9 100644 --- a/digital/io/src/init.c +++ b/digital/io/src/init.c @@ -22,8 +22,212 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ -#include "common.h" +#define FSM_NAME AI +#include "common.h" +#include "fsm.h" +#include "asserv.h" #include "init.h" +#include "playground.h" +#include "main.h" +#include "bot.h" +#include "switch.h" +#include "chrono.h" +#include "playground.h" +#include "modules/trace/trace.h" +#include "modules/path/path.h" uint8_t init_match_is_started = 0; + +FSM_STATES ( + /* waiting for the beginning of the top FSM. */ + INIT_IDLE, + /* wait for the jack to be inserted into the bot for the first time. */ + INIT_WAIT_FIRST_JACK_IN, + /* wait the jack to be removed from the bot for the first time. */ + INIT_WAIT_FIRST_JACK_OUT, + /* wait for the jack to be inserted into the bot for the second time. */ + INIT_WAIT_SECOND_JACK_IN, + /* just wait for operator hands to be removed from the jack. */ + INIT_WAIT_FOR_HANDS_OUT, + /* go to the wall for the first time. */ + INIT_GO_TO_THE_WALL, + /* reset the Y position of the bot. + * reset the angular position of the bot. + * go backward from the wall for a specific distance. */ + INIT_GO_AWAY_FROM_THE_WALL, + /* make the bot face the other wall. */ + INIT_FACE_OTHER_WALL, + /* make sure rotation is really finished before continuing. */ + INIT_WAIT_AFTER_ROTATION, + /* go to the wall for the second time. */ + INIT_GO_TO_THE_WALL_AGAIN, + /* reset the X position of the bot. + * go backward from the wall for a specific distance. */ + INIT_GO_AWAY_FROM_THE_WALL_AGAIN, + /* go to the start position. */ + INIT_GO_TO_START_POSITION, + /* wait the jack to be removed from the bot for the second time. */ + INIT_WAIT_SECOND_JACK_OUT) + +FSM_EVENTS ( + /* the jack is inserted into the bot. */ + jack_inserted_into_bot, + /* the jack is removed from the bot. */ + jack_removed_from_bot) + +FSM_START_WITH (INIT_IDLE) + +/* + * do nothing. + */ +FSM_TRANS (INIT_IDLE, start, INIT_WAIT_FIRST_JACK_IN) +{ + return FSM_NEXT (INIT_IDLE, start); +} + +/* + * do nothing. + */ +FSM_TRANS (INIT_WAIT_FIRST_JACK_IN, + jack_inserted_into_bot, + INIT_WAIT_FIRST_JACK_OUT) +{ + return FSM_NEXT (INIT_WAIT_FIRST_JACK_IN, jack_inserted_into_bot); +} + +/* + * start trace module. + * get and store the color of the bot. + */ +FSM_TRANS (INIT_WAIT_FIRST_JACK_OUT, + jack_removed_from_bot, + INIT_WAIT_SECOND_JACK_IN) +{ + /* Initialize trace module (erase the flash). */ + trace_init (); + /* Get the color. */ + bot_color = switch_get_color (); + return FSM_NEXT (INIT_WAIT_FIRST_JACK_OUT, jack_removed_from_bot); +} + +/* + * do nothing. + */ +FSM_TRANS (INIT_WAIT_SECOND_JACK_IN, + jack_inserted_into_bot, + INIT_WAIT_FOR_HANDS_OUT) +{ + return FSM_NEXT (INIT_WAIT_SECOND_JACK_IN, jack_inserted_into_bot); +} + +/* + * fuck the wall in front. + */ +FSM_TRANS_TIMEOUT (INIT_WAIT_FOR_HANDS_OUT, 225, + INIT_GO_TO_THE_WALL) +{ + /* Go to the wall, no backward. */ + asserv_go_to_the_wall (0); + return FSM_NEXT_TIMEOUT (INIT_WAIT_FOR_HANDS_OUT); +} + +/* + * reset the Y position of the bot. + * reset the angular position of the bot. + * move away from the wall (linear move). + */ +FSM_TRANS (INIT_GO_TO_THE_WALL, + bot_move_succeed, + INIT_GO_AWAY_FROM_THE_WALL) +{ + asserv_set_y_position (PG_Y (PG_LENGTH - BOT_SIZE_FRONT)); + asserv_set_angle_position (PG_A_DEG (90)); + /* Move away from the border. */ + asserv_move_linearly (- INIT_DIST); + return FSM_NEXT (INIT_GO_TO_THE_WALL, bot_move_succeed); +} + +/* + * turn to face the other wall. + */ +FSM_TRANS (INIT_GO_AWAY_FROM_THE_WALL, + bot_move_succeed, + INIT_FACE_OTHER_WALL) +{ + /* Face the other wall. */ + asserv_goto_angle (PG_A_DEG (180)); + return FSM_NEXT (INIT_GO_AWAY_FROM_THE_WALL, bot_move_succeed); +} + +/* + * nothing to do. + */ +FSM_TRANS (INIT_FACE_OTHER_WALL, + bot_move_succeed, + INIT_WAIT_AFTER_ROTATION) +{ + return FSM_NEXT (INIT_FACE_OTHER_WALL, bot_move_succeed); +} + +/* + * fuck the wall in front. + */ +FSM_TRANS_TIMEOUT (INIT_WAIT_AFTER_ROTATION, 100, + INIT_GO_TO_THE_WALL_AGAIN) +{ + /* Go to the wall, no backward. */ + asserv_go_to_the_wall (0); + return FSM_NEXT_TIMEOUT (INIT_WAIT_AFTER_ROTATION); +} + +/* + * reset the X position of the bot. + * move away from the wall (linear move). + */ +FSM_TRANS (INIT_GO_TO_THE_WALL_AGAIN, + bot_move_succeed, + INIT_GO_AWAY_FROM_THE_WALL_AGAIN) +{ + asserv_set_x_position (PG_X (BOT_SIZE_FRONT)); + /* Move away from the border. */ + asserv_move_linearly (- INIT_DIST); + return FSM_NEXT (INIT_GO_TO_THE_WALL_AGAIN, bot_move_succeed); +} + +/* + * go to the start position with a go to movement. + */ +FSM_TRANS (INIT_GO_AWAY_FROM_THE_WALL_AGAIN, + bot_move_succeed, + INIT_GO_TO_START_POSITION) +{ + /* Move away from the border. */ + asserv_goto_xya (BOT_INIT_XYA, 0); + return FSM_NEXT (INIT_GO_AWAY_FROM_THE_WALL_AGAIN, bot_move_succeed); +} + +/* + * nothing to do, the bot is at the start position. + */ +FSM_TRANS (INIT_GO_TO_START_POSITION, + bot_move_succeed, + INIT_WAIT_SECOND_JACK_OUT) +{ + return FSM_NEXT (INIT_GO_TO_START_POSITION, bot_move_succeed); +} + +/* + * tell other FSM the match begins. + * start the chrono. + */ +FSM_TRANS (INIT_WAIT_SECOND_JACK_OUT, + jack_removed_from_bot, + INIT_IDLE) +{ + /* Set the flag to transmit to other FSM. */ + init_match_is_started = 1; + /* Start the chrono. */ + chrono_init (); + return FSM_NEXT (INIT_WAIT_SECOND_JACK_OUT, jack_removed_from_bot); +} diff --git a/digital/io/src/init.fsm b/digital/io/src/init.fsm deleted file mode 100644 index f780ba45..00000000 --- a/digital/io/src/init.fsm +++ /dev/null @@ -1,106 +0,0 @@ -# init FSM -# This FSM is used to place the bot at a "start position" for a match. It is -# more precise than a human hand and reset the asserv board to the start -# position. -# Some remarks: -# - for security reason, we need to insert the jack then removed it, then -# insert it back. Why? Because it can already be inserted. -# - other FSM can do a similar thing by using the first jack out to -# initialize. -init - place the bot in the start position for a match. - -States: - *INIT_IDLE - waiting for the beginning of the top FSM. - INIT_WAIT_FIRST_JACK_IN - wait for the jack to be inserted into the bot for the first time. - INIT_WAIT_FIRST_JACK_OUT - wait the jack to be removed from the bot for the first time. - INIT_WAIT_SECOND_JACK_IN - wait for the jack to be inserted into the bot for the second time. - INIT_WAIT_FOR_HANDS_OUT[timeout=225] - just wait for operator hands to be removed from the jack. - INIT_GO_TO_THE_WALL - go to the wall for the first time. - INIT_GO_AWAY_FROM_THE_WALL - reset the Y position of the bot. - reset the angular position of the bot. - go backward from the wall for a specific distance. - INIT_FACE_OTHER_WALL - make the bot face the other wall. - INIT_WAIT_AFTER_ROTATION [timeout=100] - make sure rotation is really finished before continuing. - INIT_GO_TO_THE_WALL_AGAIN - go to the wall for the second time. - INIT_GO_AWAY_FROM_THE_WALL_AGAIN - reset the X position of the bot. - go backward from the wall for a specific distance. - INIT_GO_TO_START_POSITION - go to the start position. - INIT_WAIT_SECOND_JACK_OUT - wait the jack to be removed from the bot for the second time. - -Events: - jack_inserted_into_bot - the jack is inserted into the bot. - jack_removed_from_bot - the jack is removed from the bot. - -INIT_IDLE: - start -> INIT_WAIT_FIRST_JACK_IN - do nothing. - -INIT_WAIT_FIRST_JACK_IN: - jack_inserted_into_bot -> INIT_WAIT_FIRST_JACK_OUT - do nothing. - -INIT_WAIT_FIRST_JACK_OUT: - jack_removed_from_bot -> INIT_WAIT_SECOND_JACK_IN - start trace module. - get and store the color of the bot. - -INIT_WAIT_SECOND_JACK_IN: - jack_inserted_into_bot -> INIT_WAIT_FOR_HANDS_OUT - do nothing. - -INIT_WAIT_FOR_HANDS_OUT: - state_timeout -> INIT_GO_TO_THE_WALL - fuck the wall in front. - -INIT_GO_TO_THE_WALL: - bot_move_succeed -> INIT_GO_AWAY_FROM_THE_WALL - reset the Y position of the bot. - reset the angular position of the bot. - move away from the wall (linear move). - -INIT_GO_AWAY_FROM_THE_WALL: - bot_move_succeed -> INIT_FACE_OTHER_WALL - turn to face the other wall. - -INIT_FACE_OTHER_WALL: - bot_move_succeed -> INIT_WAIT_AFTER_ROTATION - nothing to do. - -INIT_WAIT_AFTER_ROTATION: - state_timeout -> INIT_GO_TO_THE_WALL_AGAIN - fuck the wall in front. - -INIT_GO_TO_THE_WALL_AGAIN: - bot_move_succeed -> INIT_GO_AWAY_FROM_THE_WALL_AGAIN - reset the X position of the bot. - move away from the wall (linear move). - -INIT_GO_AWAY_FROM_THE_WALL_AGAIN: - bot_move_succeed -> INIT_GO_TO_START_POSITION - go to the start position with a go to movement. - -INIT_GO_TO_START_POSITION: - bot_move_succeed -> INIT_WAIT_SECOND_JACK_OUT - nothing to do, the bot is at the start position. - -INIT_WAIT_SECOND_JACK_OUT: - jack_removed_from_bot -> INIT_IDLE - tell other FSM the match begins. - start the chrono. - diff --git a/digital/io/src/loader.c b/digital/io/src/loader.c index 4157023e..8dd557da 100644 --- a/digital/io/src/loader.c +++ b/digital/io/src/loader.c @@ -22,10 +22,16 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ +#define FSM_NAME AI + #include "common.h" #include "loader.h" - #include "fsm.h" +#include "asserv.h" +#include "mimot.h" +#include "bot.h" +#include "main.h" +#include "food.h" uint8_t loader_elements; uint8_t loader_want_up; @@ -34,13 +40,614 @@ void loader_up (void) { loader_want_up = 1; - fsm_handle_event (&ai_fsm, AI_EVENT_loader_up); + FSM_HANDLE (AI, loader_up); } void loader_down (void) { loader_want_up = 0; - fsm_handle_event (&ai_fsm, AI_EVENT_loader_down); + FSM_HANDLE (AI, loader_down); +} + +FSM_STATES ( + /* wait start */ + LOADER_IDLE, + /* wait until the jack is inserted for the first time */ + LOADER_WAIT_JACK_IN, + /* wait until the jack is removed to initialise robot */ + LOADER_WAIT_JACK_OUT, + /* move the elevator down to find zero position */ + LOADER_INIT_ELEVATOR_ZERO, + /* test clamp closing */ + LOADER_INIT_CLAMP_CLOSE, + /* find clamp zero position */ + LOADER_INIT_CLAMP_ZERO, + /* find gate zero */ + LOADER_INIT_GATE_ZERO, + /* move the elevator up so that the robot can go to the wall */ + LOADER_INIT_ELEVATOR_UP, + /* wait with gate open */ + LOADER_INIT_GATE_WAIT, + /* close gate before start */ + LOADER_INIT_GATE_CLOSE, + /* up position, clamp is protected */ + LOADER_UP, + /* down position, ready to take an element */ + LOADER_DOWN, + /* moving to the up position */ + LOADER_UPING, + /* moving to the down position */ + LOADER_DOWNING, + /* error while moving up, stop in an unknown state */ + LOADER_ERROR, + /* error recovery moving down */ + LOADER_ERROR_DOWNING, + /* error recovery moving down, open clamp */ + LOADER_ERROR_DOWNING_OPEN, + /* error recovery moving up */ + LOADER_ERROR_UPING, + /* error recovery moving up, open clamp */ + LOADER_ERROR_UPING_OPEN, + /* close clamp */ + LOADER_LOAD_CLOSING, + /* move load up to the unload position */ + LOADER_LOAD_UPING, + /* open clamp to unload, wait until unload position */ + LOADER_LOAD_UNLOADING, + /* check clamp is open */ + LOADER_LOAD_UNLOADING_OPEN, + /* open clamp due to no elements to pick */ + LOADER_LOAD_EMPTY_OPEN) + +FSM_EVENTS ( + /* asserv success result */ + elevator_succeed, + /* asserv failure result */ + elevator_failed, + /* elevator above unloading position */ + elevator_unload_position, + /* asserv failure result */ + clamp_failed, + /* asserv success result */ + gate_succeed, + /* asserv failure result */ + gate_failed, + /* element sensed between clamp to be loaded */ + loader_element, + /* make the loader ready to load an element */ + loader_down, + /* protect the clamp by moving it up */ + loader_up, + /* posted when loader successfully moved down */ + loader_downed, + /* posted when loader successfully moved up */ + loader_uped, + /* posted when loader in an unknown state due to error while moving up */ + loader_errored, + /* loader blocked, due to a black corn */ + loader_black) + +FSM_START_WITH (LOADER_IDLE) + +FSM_TRANS (LOADER_IDLE, start, LOADER_WAIT_JACK_IN) +{ + return FSM_NEXT (LOADER_IDLE, start); +} + +FSM_TRANS (LOADER_WAIT_JACK_IN, + jack_inserted_into_bot, + LOADER_WAIT_JACK_OUT) +{ + return FSM_NEXT (LOADER_WAIT_JACK_IN, jack_inserted_into_bot); +} + +FSM_TRANS (LOADER_WAIT_JACK_IN, + hola_start, + LOADER_IDLE) +{ + return FSM_NEXT (LOADER_WAIT_JACK_IN, hola_start); +} + +/* + * find elevator zero + * close clamp + * find gate zero + */ +FSM_TRANS (LOADER_WAIT_JACK_OUT, + jack_removed_from_bot, + LOADER_INIT_ELEVATOR_ZERO) +{ + asserv_motor0_zero_position (-BOT_ELEVATOR_ZERO_SPEED); + mimot_motor0_clamp (BOT_CLAMP_ZERO_SPEED, 0); + mimot_motor1_clamp (BOT_CLAMP_ZERO_SPEED, 0); + asserv_motor1_zero_position (-BOT_GATE_SPEED); + return FSM_NEXT (LOADER_WAIT_JACK_OUT, jack_removed_from_bot); +} + +FSM_TRANS (LOADER_INIT_ELEVATOR_ZERO, + elevator_succeed, + LOADER_INIT_CLAMP_CLOSE) +{ + return FSM_NEXT (LOADER_INIT_ELEVATOR_ZERO, elevator_succeed); +} + +/* + * move elevator up + * find clamp zero + */ +FSM_TRANS (LOADER_INIT_CLAMP_CLOSE, + clamp_succeed, + LOADER_INIT_CLAMP_ZERO) +{ + asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, + BOT_ELEVATOR_ZERO_SPEED); + mimot_motor0_zero_position (-BOT_CLAMP_ZERO_SPEED); + mimot_motor1_zero_position (-BOT_CLAMP_ZERO_SPEED); + return FSM_NEXT (LOADER_INIT_CLAMP_CLOSE, clamp_succeed); +} + +FSM_TRANS (LOADER_INIT_CLAMP_ZERO, + clamp_succeed, + LOADER_INIT_GATE_ZERO) +{ + return FSM_NEXT (LOADER_INIT_CLAMP_ZERO, clamp_succeed); +} + +FSM_TRANS (LOADER_INIT_GATE_ZERO, + gate_succeed, + LOADER_INIT_ELEVATOR_UP) +{ + return FSM_NEXT (LOADER_INIT_GATE_ZERO, gate_succeed); +} + +FSM_TRANS (LOADER_INIT_ELEVATOR_UP, + elevator_succeed, + LOADER_INIT_GATE_WAIT) +{ + return FSM_NEXT (LOADER_INIT_ELEVATOR_UP, elevator_succeed); +} + +/* + * initialisation failure + */ +FSM_TRANS (LOADER_INIT_ELEVATOR_UP, + elevator_failed, + LOADER_IDLE) +{ + /* Move so that the operator notice it. */ + asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP / 3, + BOT_ELEVATOR_ZERO_SPEED); + return FSM_NEXT (LOADER_INIT_ELEVATOR_UP, elevator_failed); } +/* + * close gate + */ +FSM_TRANS_TIMEOUT (LOADER_INIT_GATE_WAIT, 225, + LOADER_INIT_GATE_CLOSE) +{ + asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED); + return FSM_NEXT_TIMEOUT (LOADER_INIT_GATE_WAIT); +} + +FSM_TRANS (LOADER_INIT_GATE_CLOSE, + gate_succeed, + LOADER_UP) +{ + return FSM_NEXT (LOADER_INIT_GATE_CLOSE, gate_succeed); +} + +/* + * move down + */ +FSM_TRANS (LOADER_UP, + loader_down, + LOADER_DOWNING) +{ + asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED); + return FSM_NEXT (LOADER_UP, loader_down); +} + +/* + * move up + */ +FSM_TRANS (LOADER_DOWN, + loader_up, + LOADER_UPING) +{ + asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED); + return FSM_NEXT (LOADER_DOWN, loader_up); +} + +/* + * clamp + */ +FSM_TRANS (LOADER_DOWN, + loader_element, + LOADER_LOAD_CLOSING) +{ + mimot_motor0_clamp (BOT_CLAMP_SPEED, BOT_CLAMP_PWM); + mimot_motor1_clamp (BOT_CLAMP_SPEED, BOT_CLAMP_PWM); + return FSM_NEXT (LOADER_DOWN, loader_element); +} + +/* + * post loader_uped event + */ +FSM_TRANS (LOADER_UPING, + elevator_succeed, + LOADER_UP) +{ + main_post_event (FSM_EVENT (AI,loader_uped)); + return FSM_NEXT (LOADER_UPING, elevator_succeed); +} + +/* + * post loader_errored event + */ +FSM_TRANS (LOADER_UPING, + elevator_failed, + LOADER_ERROR) +{ + main_post_event (FSM_EVENT (AI, loader_errored)); + return FSM_NEXT (LOADER_UPING, elevator_failed); +} + +/* + * move down + */ +FSM_TRANS (LOADER_UPING, + loader_down, + LOADER_DOWNING) +{ + asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED); + return FSM_NEXT (LOADER_UPING, loader_down); +} + +/* + * release elevator motor + * post loader_downed event + */ +FSM_TRANS (LOADER_DOWNING, + elevator_succeed, + LOADER_DOWN) +{ + asserv_motor0_free (); + main_post_event (FSM_EVENT (AI, loader_downed)); + return FSM_NEXT (LOADER_DOWNING, elevator_succeed); +} + +/* + * post loader_errored event + */ +FSM_TRANS (LOADER_DOWNING, + elevator_failed, + LOADER_ERROR) +{ + main_post_event (FSM_EVENT (AI, loader_errored)); + return FSM_NEXT (LOADER_DOWNING, elevator_failed); +} + +/* + * emergency stop + * post loader_errored event + */ +FSM_TRANS (LOADER_DOWNING, + loader_element, + LOADER_ERROR) +{ + asserv_motor0_free (); + main_post_event (FSM_EVENT (AI, loader_errored)); + return FSM_NEXT (LOADER_DOWNING, loader_element); +} + +/* + * move up + */ +FSM_TRANS (LOADER_DOWNING, + loader_up, + LOADER_UPING) +{ + asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED); + return FSM_NEXT (LOADER_DOWNING, loader_up); +} + +/* + * move down + */ +FSM_TRANS (LOADER_ERROR, + loader_down, + LOADER_ERROR_DOWNING) +{ + asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED); + return FSM_NEXT (LOADER_ERROR, loader_down); +} + +/* + * move up + */ +FSM_TRANS (LOADER_ERROR, + loader_up, + LOADER_ERROR_UPING) +{ + asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP, BOT_ELEVATOR_SPEED); + return FSM_NEXT (LOADER_ERROR, loader_up); +} + +/* + * release elevator motor + * open clamp + */ +FSM_TRANS (LOADER_ERROR_DOWNING, + elevator_succeed, + LOADER_ERROR_DOWNING_OPEN) +{ + asserv_motor0_free (); + mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + return FSM_NEXT (LOADER_ERROR_DOWNING, elevator_succeed); +} + +/* + * post loader_errored event + */ +FSM_TRANS (LOADER_ERROR_DOWNING, + elevator_failed, + LOADER_ERROR) +{ + main_post_event (FSM_EVENT (AI, loader_errored)); + return FSM_NEXT (LOADER_ERROR_DOWNING, elevator_failed); +} + +/* + * post loader_errored event + */ +FSM_TRANS_TIMEOUT (LOADER_ERROR_DOWNING, 225, + LOADER_ERROR) +{ + main_post_event (FSM_EVENT (AI, loader_errored)); + return FSM_NEXT_TIMEOUT (LOADER_ERROR_DOWNING); +} + +/* + * post loader_downed event + */ +FSM_TRANS (LOADER_ERROR_DOWNING_OPEN, + clamp_succeed, + LOADER_DOWN) +{ + main_post_event (FSM_EVENT (AI, loader_downed)); + return FSM_NEXT (LOADER_ERROR_DOWNING_OPEN, clamp_succeed); +} + +/* + * post loader_errored event + */ +FSM_TRANS (LOADER_ERROR_DOWNING_OPEN, + clamp_failed, + LOADER_ERROR) +{ + main_post_event (FSM_EVENT (AI, loader_errored)); + return FSM_NEXT (LOADER_ERROR_DOWNING_OPEN, clamp_failed); +} + +/* + * post loader_errored event + */ +FSM_TRANS_TIMEOUT (LOADER_ERROR_DOWNING_OPEN, 225, + LOADER_ERROR) +{ + main_post_event (FSM_EVENT (AI, loader_errored)); + return FSM_NEXT_TIMEOUT (LOADER_ERROR_DOWNING_OPEN); +} + +/* + * open clamp + */ +FSM_TRANS (LOADER_ERROR_UPING, + elevator_succeed, + LOADER_ERROR_UPING_OPEN) +{ + mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + return FSM_NEXT (LOADER_ERROR_UPING, elevator_succeed); +} + +/* + * post loader_errored event + */ +FSM_TRANS (LOADER_ERROR_UPING, + elevator_failed, + LOADER_ERROR) +{ + main_post_event (FSM_EVENT (AI, loader_errored)); + return FSM_NEXT (LOADER_ERROR_UPING, elevator_failed); +} + +/* + * post loader_errored event + */ +FSM_TRANS_TIMEOUT (LOADER_ERROR_UPING, 225, + LOADER_ERROR) +{ + main_post_event (FSM_EVENT (AI, loader_errored)); + return FSM_NEXT_TIMEOUT (LOADER_ERROR_UPING); +} + +/* + * post loader_uped event + */ +FSM_TRANS (LOADER_ERROR_UPING_OPEN, + clamp_succeed, + LOADER_UP) +{ + main_post_event (FSM_EVENT (AI, loader_uped)); + return FSM_NEXT (LOADER_ERROR_UPING_OPEN, clamp_succeed); +} + +/* + * post loader_errored event + */ +FSM_TRANS (LOADER_ERROR_UPING_OPEN, + clamp_failed, + LOADER_ERROR) +{ + main_post_event (FSM_EVENT (AI, loader_errored)); + return FSM_NEXT (LOADER_ERROR_UPING_OPEN, clamp_failed); +} + +/* + * post loader_errored event + */ +FSM_TRANS_TIMEOUT (LOADER_ERROR_UPING_OPEN, 225, + LOADER_ERROR) +{ + main_post_event (FSM_EVENT (AI, loader_errored)); + return FSM_NEXT_TIMEOUT (LOADER_ERROR_UPING_OPEN); +} + +FSM_TRANS (LOADER_LOAD_CLOSING, clamp_succeed, + /* move up + * count one element */ + full, LOADER_LOAD_UPING, + /* open clamp */ + empty, LOADER_LOAD_EMPTY_OPEN) +{ + /* Measure load using clamp position. */ + uint16_t tickness = BOT_CLAMP_WIDTH_STEP + - mimot_get_motor0_position () + - mimot_get_motor1_position (); + if (tickness > BOT_CLAMP_EMPTY_STEP) + { + asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP, + BOT_ELEVATOR_SPEED); + position_t robot_position; + asserv_get_position (&robot_position); + food_taken (robot_position); + loader_elements++; + return FSM_NEXT (LOADER_LOAD_CLOSING, clamp_succeed, full); + } + else + { + mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + return FSM_NEXT (LOADER_LOAD_CLOSING, clamp_succeed, empty); + } +} + +/* + * open clamp + */ +FSM_TRANS (LOADER_LOAD_UPING, + elevator_unload_position, + LOADER_LOAD_UNLOADING) +{ + mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + return FSM_NEXT (LOADER_LOAD_UPING, elevator_unload_position); +} + +/* + * post loader_errored or loader_black event + * remove one element + * open clamp + */ +FSM_TRANS (LOADER_LOAD_UPING, + elevator_failed, + LOADER_ERROR) +{ + if (asserv_get_motor0_position () < BOT_ELEVATOR_BLACK_THRESHOLD_STEP) + { + if (loader_elements) + loader_elements--; + main_post_event (FSM_EVENT (AI, loader_black)); + } + else + main_post_event (FSM_EVENT (AI, loader_errored)); + mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + return FSM_NEXT (LOADER_LOAD_UPING, elevator_failed); +} + +FSM_TRANS (LOADER_LOAD_UNLOADING, + elevator_succeed, + LOADER_LOAD_UNLOADING_OPEN) +{ + return FSM_NEXT (LOADER_LOAD_UNLOADING, elevator_succeed); +} + +/* + * post loader_errored event + * open clamp + */ +FSM_TRANS (LOADER_LOAD_UNLOADING, + elevator_failed, + LOADER_ERROR) +{ + main_post_event (FSM_EVENT (AI, loader_errored)); + mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + return FSM_NEXT (LOADER_LOAD_UNLOADING, elevator_failed); +} + +FSM_TRANS (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed, + /* move down */ + down, LOADER_DOWNING, + /* move up */ + up, LOADER_UPING) +{ + if (loader_want_up) + { + asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, + BOT_ELEVATOR_SPEED); + return FSM_NEXT (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed, up); + } + else + { + asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, + BOT_ELEVATOR_SPEED); + return FSM_NEXT (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed, down); + } +} + +/* + * post loader_errored event + */ +FSM_TRANS (LOADER_LOAD_UNLOADING_OPEN, + clamp_failed, + LOADER_ERROR) +{ + main_post_event (FSM_EVENT (AI, loader_errored)); + return FSM_NEXT (LOADER_LOAD_UNLOADING_OPEN, clamp_failed); +} + +FSM_TRANS (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, + /* post loader_downed event */ + down, LOADER_DOWN, + /* move up */ + up, LOADER_UPING) +{ + if (loader_want_up) + { + asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, + BOT_ELEVATOR_SPEED); + return FSM_NEXT (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, up); + } + else + { + main_post_event (FSM_EVENT (AI, loader_downed)); + return FSM_NEXT (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, down); + } +} + +/* + * post loader_errored event + */ +FSM_TRANS (LOADER_LOAD_EMPTY_OPEN, + clamp_failed, + LOADER_ERROR) +{ + main_post_event (FSM_EVENT (AI, loader_errored)); + return FSM_NEXT (LOADER_LOAD_EMPTY_OPEN, clamp_failed); +} diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm deleted file mode 100644 index a188194e..00000000 --- a/digital/io/src/loader.fsm +++ /dev/null @@ -1,227 +0,0 @@ -# loader FSM -loader - Handle Eurobot 2010 robot clamp, elevator and output door. - -States: - *LOADER_IDLE - wait start - LOADER_WAIT_JACK_IN - wait until the jack is inserted for the first time - LOADER_WAIT_JACK_OUT - wait until the jack is removed to initialise robot - LOADER_INIT_ELEVATOR_ZERO - move the elevator down to find zero position - LOADER_INIT_CLAMP_CLOSE - test clamp closing - LOADER_INIT_CLAMP_ZERO - find clamp zero position - LOADER_INIT_GATE_ZERO - find gate zero - LOADER_INIT_ELEVATOR_UP - move the elevator up so that the robot can go to the wall - LOADER_INIT_GATE_WAIT[timeout=225] - wait with gate open - LOADER_INIT_GATE_CLOSE - close gate before start - LOADER_UP - up position, clamp is protected - LOADER_DOWN - down position, ready to take an element - LOADER_UPING - moving to the up position - LOADER_DOWNING - moving to the down position - LOADER_ERROR - error while moving up, stop in an unknown state - LOADER_ERROR_DOWNING [timeout=225] - error recovery moving down - LOADER_ERROR_DOWNING_OPEN [timeout=225] - error recovery moving down, open clamp - LOADER_ERROR_UPING [timeout=225] - error recovery moving up - LOADER_ERROR_UPING_OPEN [timeout=225] - error recovery moving up, open clamp - LOADER_LOAD_CLOSING - close clamp - LOADER_LOAD_UPING - move load up to the unload position - LOADER_LOAD_UNLOADING - open clamp to unload, wait until unload position - LOADER_LOAD_UNLOADING_OPEN - check clamp is open - LOADER_LOAD_EMPTY_OPEN - open clamp due to no elements to pick - -Events: - elevator_succeed - asserv success result - elevator_failed - asserv failure result - elevator_unload_position - elevator above unloading position - clamp_failed - asserv failure result - gate_succeed - asserv success result - gate_failed - asserv failure result - loader_element - element sensed between clamp to be loaded - loader_down - make the loader ready to load an element - loader_up - protect the clamp by moving it up - loader_downed - posted when loader successfully moved down - loader_uped - posted when loader successfully moved up - loader_errored - posted when loader in an unknown state due to error while moving up - loader_black - loader blocked, due to a black corn - -LOADER_IDLE: - start -> LOADER_WAIT_JACK_IN - -LOADER_WAIT_JACK_IN: - jack_inserted_into_bot -> LOADER_WAIT_JACK_OUT - hola_start -> LOADER_IDLE - -LOADER_WAIT_JACK_OUT: - jack_removed_from_bot -> LOADER_INIT_ELEVATOR_ZERO - find elevator zero - close clamp - find gate zero - -LOADER_INIT_ELEVATOR_ZERO: - elevator_succeed -> LOADER_INIT_CLAMP_CLOSE - -LOADER_INIT_CLAMP_CLOSE: - clamp_succeed -> LOADER_INIT_CLAMP_ZERO - move elevator up - find clamp zero - -LOADER_INIT_CLAMP_ZERO: - clamp_succeed -> LOADER_INIT_GATE_ZERO - -LOADER_INIT_GATE_ZERO: - gate_succeed -> LOADER_INIT_ELEVATOR_UP - -LOADER_INIT_ELEVATOR_UP: - elevator_succeed -> LOADER_INIT_GATE_WAIT - elevator_failed -> LOADER_IDLE - initialisation failure - -LOADER_INIT_GATE_WAIT: - state_timeout -> LOADER_INIT_GATE_CLOSE - close gate - -LOADER_INIT_GATE_CLOSE: - gate_succeed -> LOADER_UP - -LOADER_UP: - loader_down -> LOADER_DOWNING - move down - -LOADER_DOWN: - loader_up -> LOADER_UPING - move up - loader_element -> LOADER_LOAD_CLOSING - clamp - -LOADER_ERROR: - loader_down -> LOADER_ERROR_DOWNING - move down - loader_up -> LOADER_ERROR_UPING - move up - -LOADER_ERROR_DOWNING: - elevator_succeed -> LOADER_ERROR_DOWNING_OPEN - release elevator motor - open clamp - elevator_failed -> LOADER_ERROR - post loader_errored event - state_timeout -> LOADER_ERROR - post loader_errored event - -LOADER_ERROR_DOWNING_OPEN: - clamp_succeed -> LOADER_DOWN - post loader_downed event - clamp_failed -> LOADER_ERROR - post loader_errored event - state_timeout -> LOADER_ERROR - post loader_errored event - -LOADER_ERROR_UPING: - elevator_succeed -> LOADER_ERROR_UPING_OPEN - open clamp - elevator_failed -> LOADER_ERROR - post loader_errored event - state_timeout -> LOADER_ERROR - post loader_errored event - -LOADER_ERROR_UPING_OPEN: - clamp_succeed -> LOADER_UP - post loader_uped event - clamp_failed -> LOADER_ERROR - post loader_errored event - state_timeout -> LOADER_ERROR - post loader_errored event - -LOADER_DOWNING: - elevator_succeed -> LOADER_DOWN - release elevator motor - post loader_downed event - elevator_failed -> LOADER_ERROR - post loader_errored event - loader_element -> LOADER_ERROR - emergency stop - post loader_errored event - loader_up -> LOADER_UPING - move up - -LOADER_UPING: - elevator_succeed -> LOADER_UP - post loader_uped event - elevator_failed -> LOADER_ERROR - post loader_errored event - loader_down -> LOADER_DOWNING - move down - -LOADER_LOAD_CLOSING: - clamp_succeed: full -> LOADER_LOAD_UPING - move up - count one element - clamp_succeed: empty -> LOADER_LOAD_EMPTY_OPEN - open clamp - -LOADER_LOAD_UPING: - elevator_unload_position -> LOADER_LOAD_UNLOADING - open clamp - elevator_failed -> LOADER_ERROR - post loader_errored or loader_black event - remove one element - open clamp - -LOADER_LOAD_UNLOADING: - elevator_succeed -> LOADER_LOAD_UNLOADING_OPEN - elevator_failed -> LOADER_ERROR - post loader_errored event - open clamp - -LOADER_LOAD_UNLOADING_OPEN: - clamp_succeed: down -> LOADER_DOWNING - move down - clamp_succeed: up -> LOADER_UPING - move up - clamp_failed -> LOADER_ERROR - post loader_errored event - -LOADER_LOAD_EMPTY_OPEN: - clamp_succeed: down -> LOADER_DOWN - post loader_downed event - clamp_succeed: up -> LOADER_UPING - move up - clamp_failed -> LOADER_ERROR - post loader_errored event - diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 35ee83c1..dc98d12d 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -35,6 +35,8 @@ /* AVR include, non HOST */ #ifndef HOST # include "switch.h" /* Manage switches (jack, color selector) */ +#else +#include #endif /* HOST */ #include "main_timer.h" @@ -44,7 +46,8 @@ #include "mimot.h" #include "twi_master.h" #include "eeprom.h" /* Parameters loaded/stored in the EEPROM */ -#include "fsm.h" /* fsm_* */ +#define FSM_NAME AI +#include "fsm.h" #include "bot.h" #include "servo_pos.h" #include "usdist.h" @@ -144,16 +147,18 @@ main_event_to_fsm (void) { /* If an event is handled, stop generating any other event, because a * transition may have invalidated the current robot state. */ -#define FSM_HANDLE_EVENT(fsm, event) \ - do { if (fsm_handle_event ((fsm), (event))) return; } while (0) -#define FSM_HANDLE_TIMEOUT(fsm) \ - do { if (fsm_handle_timeout (fsm)) return; } while (0) +#define FSM_HANDLE_E(fsm, event) \ + do { if (FSM_HANDLE (fsm, event)) return; } while (0) +#define FSM_HANDLE_VAR_E(fsm, event) \ + do { if (FSM_HANDLE_VAR (fsm, event)) return; } while (0) +#define FSM_HANDLE_TIMEOUT_E(fsm) \ + do { if (FSM_HANDLE_TIMEOUT (fsm)) return; } while (0) /* Update FSM timeouts. */ - FSM_HANDLE_TIMEOUT (&ai_fsm); + FSM_HANDLE_TIMEOUT_E (AI); /* If we have entering this function, last command of the asserv board has * been aquited. */ - FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_asserv_last_cmd_ack); + FSM_HANDLE_E (AI, asserv_last_cmd_ack); asserv_status_e move_status = none, @@ -171,50 +176,50 @@ main_event_to_fsm (void) /* Check commands move status. */ if (move_status == success) - FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_bot_move_succeed); + FSM_HANDLE_E (AI, bot_move_succeed); else if (move_status == failure) - FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_bot_move_failed); + FSM_HANDLE_E (AI, bot_move_failed); if (motor0_status == success) - FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_elevator_succeed); + FSM_HANDLE_E (AI, elevator_succeed); else if (motor0_status == failure) - FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_elevator_failed); + FSM_HANDLE_E (AI, elevator_failed); if (motor1_status == success) - FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_gate_succeed); + FSM_HANDLE_E (AI, gate_succeed); else if (motor1_status == failure) - FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_gate_failed); + FSM_HANDLE_E (AI, gate_failed); if (motorm0_status == success && motorm1_status == success) - FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_clamp_succeed); + FSM_HANDLE_E (AI, clamp_succeed); else if (motorm0_status == failure || motorm1_status == failure) - FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_clamp_failed); + FSM_HANDLE_E (AI, clamp_failed); /* Check positions. */ if (asserv_get_motor0_position () > BOT_ELEVATOR_UNLOAD_STEP) - FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_elevator_unload_position); + FSM_HANDLE_E (AI, elevator_unload_position); /* Contacts. */ if (asserv_get_motor0_position () < BOT_ELEVATOR_REST_STEP && (!IO_GET (CONTACT_BUMPER0) || !IO_GET (CONTACT_BUMPER1))) - FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_loader_element); + FSM_HANDLE_E (AI, loader_element); if (!IO_GET (CONTACT_STRATEGY)) { if (switch_get_color ()) loader_down (); else - FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_hola_start); + FSM_HANDLE_E (AI, hola_start); } /* Jack */ if (switch_get_jack ()) - FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_jack_removed_from_bot); + FSM_HANDLE_E (AI, jack_removed_from_bot); else - FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_jack_inserted_into_bot); + FSM_HANDLE_E (AI, jack_inserted_into_bot); if (init_match_is_started) { - FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_init_match_is_started); + FSM_HANDLE_E (AI, init_match_is_started); /* This must be done in the last part of this block. */ init_match_is_started = 0; @@ -229,7 +234,7 @@ main_event_to_fsm (void) /* We need to save the event before reseting it */ uint8_t save_event = main_pop_event (); /* Post the event */ - FSM_HANDLE_EVENT (&ai_fsm, save_event); + FSM_HANDLE_VAR_E (AI, save_event); } /* Check obstacles. */ @@ -240,7 +245,7 @@ main_event_to_fsm (void) position_t robot_pos; asserv_get_position (&robot_pos); if (robot_pos.v.y < PG_FIELD_Y_MAX) - FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_in_field); + FSM_HANDLE_E (AI, in_field); /* TODO: Check other sensors */ } @@ -276,10 +281,8 @@ main_init (void) usdist_init (); /* Top. */ top_init (); - /* Init FSM. */ - fsm_init (&ai_fsm); /* Start FSM. */ - fsm_handle_event (&ai_fsm, AI_EVENT_start); + FSM_HANDLE (AI, start); /* PWM module */ pwm_init (); /* Servo pos init. */ @@ -305,12 +308,12 @@ main_loop (void) uint8_t timer_count = main_timer_wait (); if (main_stats_timer_) proto_send1b('M', timer_count); - if (timer_count == 1) - { - /* Main timer has reached overflow earlier! - We are late and this is really bad. */ - TRACE (TRACE_MAIN_TIMER__LATE); - } + if (timer_count == 1) + { + /* Main timer has reached overflow earlier! + We are late and this is really bad. */ + TRACE (TRACE_MAIN_TIMER__LATE); + } } /* Update chrono. */ @@ -706,6 +709,18 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) int main (int argc, char **argv) { + /* produce AVR's FSM headers. */ +#ifdef HOST + int i; + if (argc > 1) + for (i = 1; i < argc; i++) + if (strcmp (argv[i], "--gen") == 0) + { + FSM_GENERATE (AVR, 0); + return 0; + } +#endif + avr_init (argc, argv); /* Initialize the main and its subsystems */ diff --git a/digital/io/src/move.c b/digital/io/src/move.c index e4f59c39..fa65418d 100644 --- a/digital/io/src/move.c +++ b/digital/io/src/move.c @@ -22,6 +22,9 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ + +#define FSM_NAME AI + #include "common.h" #include "move.h" #include "fsm.h" @@ -29,9 +32,25 @@ #include "radar.h" #include "asserv.h" #include "main.h" - +#include "events.h" +#include "playground.h" +#include "loader.h" #include "modules/path/path.h" #include "modules/utils/utils.h" +#include "main.h" /* main_post_event_for_top_fsm */ +#include "modules/math/fixed/fixed.h" /* fixed_* */ +#include "modules/trace/trace.h" +#include "debug.host.h" +#include + +#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 (¤t_pos); + /* Give the current position of the bot to the path module */ + path_endpoints (current_pos.v, move_data.final.v); + /* Update the path module */ + move_data.slow = 0; + path_update (); + found = path_get_next (&dst); + /* If not found, try to escape. */ + if (!found) + { + move_data.slow = 1; + path_escape (8); + path_update (); + found = path_get_next (&dst); + } + /* If not on the grid, slow down. */ + int16_t mx = current_pos.v.x % MOVE_GRID_X; + int16_t my = current_pos.v.y % MOVE_GRID_Y; + if (mx < MOVE_GRID_X_ORIGIN - MOVE_GRID_MARGIN + || mx > MOVE_GRID_X_ORIGIN + MOVE_GRID_MARGIN + || my < MOVE_GRID_Y_ORIGIN - MOVE_GRID_MARGIN + || my > MOVE_GRID_Y_ORIGIN + MOVE_GRID_MARGIN + ) + move_data.slow = 1; + /* If found, go. */ + if (found) + { + return move_go_to_next (dst); + } + else + { + /* Error, not final move. */ + move_data.final_move = 0; + return 0; + } +} + +/** Go to next position in path. Returns 1 for linear move, 2 for angular + * move. */ +static uint8_t +move_path_next (void) +{ + vect_t dst; + path_get_next (&dst); + return move_go_to_next (dst); +} + +FSM_TRANS (MOVE_IDLE, move_start, + /* rotate towards next position. */ + path_found_rotate, MOVE_ROTATING, + /* move to next position. */ + path_found, MOVE_MOVING, + /* post failure event. */ + no_path_found, MOVE_IDLE) +{ + uint8_t next = move_path_init (); + if (next) + { + if (next == 2) + return FSM_NEXT (MOVE_IDLE, move_start, path_found_rotate); + else + return FSM_NEXT (MOVE_IDLE, move_start, path_found); + } + else + { + main_post_event (FSM_EVENT (AI, move_fsm_failed)); + return FSM_NEXT (MOVE_IDLE, move_start, no_path_found); + } +} + +/* + * move to next position. + */ +FSM_TRANS (MOVE_ROTATING, + bot_move_succeed, + MOVE_MOVING) +{ + loader_down (); + move_go (); + return FSM_NEXT (MOVE_ROTATING, bot_move_succeed); +} + +/* + * move to next position. + */ +FSM_TRANS (MOVE_ROTATING, + bot_move_failed, + MOVE_MOVING) +{ + loader_down (); + move_go (); + return FSM_NEXT (MOVE_ROTATING, bot_move_failed); +} + +/* + * move to next position. + */ +FSM_TRANS_TIMEOUT (MOVE_ROTATING, 1125, + MOVE_MOVING) +{ + loader_down (); + move_go (); + return FSM_NEXT_TIMEOUT (MOVE_ROTATING); +} + +/* + * move backward + * loader up + */ +FSM_TRANS (MOVE_ROTATING, + loader_errored, + MOVE_LOADER_UNBLOCKING_UPING) +{ + asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE); + loader_up (); + return FSM_NEXT (MOVE_ROTATING, loader_errored); +} + +FSM_TRANS (MOVE_MOVING, bot_move_succeed, + /* post success event. */ + done, MOVE_IDLE, + /* rotate towards next position. */ + path_found_rotate, MOVE_ROTATING, + /* move to next position. */ + path_found, MOVE_MOVING, + /* post failure event */ + no_path_found, MOVE_IDLE) +{ + if (move_data.final_move) + { + main_post_event (FSM_EVENT (AI, move_fsm_succeed)); + return FSM_NEXT (MOVE_MOVING, bot_move_succeed, done); + } + else + { + uint8_t next = move_path_next (); + if (next == 2) + return FSM_NEXT (MOVE_MOVING, bot_move_succeed, path_found_rotate); + else + return FSM_NEXT (MOVE_MOVING, bot_move_succeed, path_found); + } + //return FSM_NEXT (MOVE_MOVING, bot_move_succeed, no_path_found); +} + + +/* + * reset final_move. + * move backward to turn freely. + */ +void +move_MOVE_MOVING_bot_move_failed_MOVE_MOVING_BACKWARD_TO_TURN_FREELY () +{ + move_data.final_move = 0; + /* Assume there is an obstacle in front of the robot. */ + position_t robot_pos; + asserv_get_position (&robot_pos); + vect_t obstacle_pos; + int16_t dist = asserv_get_last_moving_direction () == 1 + ? BOT_SIZE_FRONT + MOVE_REAL_OBSTACLE_RADIUS + : -(BOT_SIZE_BACK + MOVE_REAL_OBSTACLE_RADIUS); + vect_from_polar_uf016 (&obstacle_pos, dist, robot_pos.a); + vect_translate (&obstacle_pos, &robot_pos.v); + path_obstacle (0, obstacle_pos, MOVE_OBSTACLE_RADIUS, 0, + MOVE_OBSTACLE_VALIDITY); + /* Move backward to turn freely. */ + asserv_move_linearly (asserv_get_last_moving_direction () == 1 ? + - 300 : 300); +} + +FSM_TRANS (MOVE_MOVING, + bot_move_failed, + MOVE_MOVING_BACKWARD_TO_TURN_FREELY) +{ + move_MOVE_MOVING_bot_move_failed_MOVE_MOVING_BACKWARD_TO_TURN_FREELY (); + return FSM_NEXT (MOVE_MOVING, bot_move_failed); +} + +/* + * reset final_move. + * move backward to turn freely. + */ +FSM_TRANS_TIMEOUT (MOVE_MOVING, 2250, + MOVE_MOVING_BACKWARD_TO_TURN_FREELY) +{ + move_MOVE_MOVING_bot_move_failed_MOVE_MOVING_BACKWARD_TO_TURN_FREELY (); + return FSM_NEXT_TIMEOUT (MOVE_MOVING); +} + +FSM_TRANS (MOVE_MOVING, obstacle_in_front, + /* reset final_move. + * stop the bot. */ + tryagain, MOVE_WAIT_FOR_CLEAR_PATH, + /* stop the bot. + * post failure event. */ + tryout, MOVE_IDLE) +{ + move_data.final_move = 0; + asserv_stop_motor (); + if (--move_data.try_again_counter == 0) + { + main_post_event (FSM_EVENT (AI, move_fsm_failed)); + return FSM_NEXT (MOVE_MOVING, obstacle_in_front, tryout); + } + else + return FSM_NEXT (MOVE_MOVING, obstacle_in_front, tryagain); +} + +/* + * move backward + * loader up + */ +FSM_TRANS (MOVE_MOVING, + loader_errored, + MOVE_LOADER_UNBLOCKING_UPING) +{ + asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE); + loader_up (); + return FSM_NEXT (MOVE_MOVING, loader_errored); +} + +FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, + /* post failure event. */ + tryout, MOVE_IDLE, + /* rotate towards next position. */ + path_found_rotate, MOVE_ROTATING, + /* move to next position. */ + path_found, MOVE_MOVING, + /* post failure event. */ + no_path_found, MOVE_IDLE) +{ + if (--move_data.try_again_counter == 0) + { + main_post_event (FSM_EVENT (AI, move_fsm_failed)); + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, tryout); + } + else + { + uint8_t next = move_path_init (); + if (next) + { + if (next == 2) + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, path_found_rotate); + else + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, path_found); + } + else + { + main_post_event (FSM_EVENT (AI, move_fsm_failed)); + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, no_path_found); + } + } +} + +FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, + /* post failure event. */ + tryout, MOVE_IDLE, + /* rotate towards next position. */ + path_found_rotate, MOVE_ROTATING, + /* move to next position. */ + path_found, MOVE_MOVING, + /* nothing to do. */ + no_path_found_tryagain, MOVE_WAIT_FOR_CLEAR_PATH, + /* post failure event. */ + no_path_found_tryout, MOVE_IDLE) +{ + if (--move_data.try_again_counter == 0) + { + main_post_event (FSM_EVENT (AI, move_fsm_failed)); + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, tryout); + } + else + { + uint8_t next = move_path_init (); + if (next) + { + if (next == 2) + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, path_found_rotate); + else + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, path_found); + } + else + { + if (--move_data.try_again_counter == 0) + { + main_post_event (FSM_EVENT (AI, move_fsm_failed)); + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_path_found_tryout); + } + else + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_path_found_tryagain); + } + } +} + +FSM_TRANS_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, 255, + /* rotate towards next position. */ + path_found_rotate, MOVE_ROTATING, + /* move to next position. */ + path_found, MOVE_MOVING, + /* decrement counter. */ + no_path_found_tryagain, MOVE_WAIT_FOR_CLEAR_PATH, + /* post failure. */ + no_path_found_tryout, MOVE_IDLE) +{ + /* Try to move. */ + uint8_t next = move_path_init (); + if (next) + { + if (next == 2) + return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, path_found_rotate); + else + return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, path_found); + } + else + { + /* Error, no new position, should we try again? */ + if (--move_data.try_again_counter == 0) + { + main_post_event (FSM_EVENT (AI, move_fsm_failed)); + return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, no_path_found_tryout); + } + else + return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, no_path_found_tryagain); + } +} + +/* + * loader down + * reset unblocking retry counter + */ +FSM_TRANS (MOVE_LOADER_UNBLOCKING_UPING, + bot_move_succeed, + MOVE_LOADER_UNBLOCKING_DOWNING) +{ + loader_down (); + move_data.loader_unblocking_retry = 2; + return FSM_NEXT (MOVE_LOADER_UNBLOCKING_UPING, bot_move_succeed); +} + +/* + * loader down + * reset unblocking retry counter + */ +FSM_TRANS (MOVE_LOADER_UNBLOCKING_UPING, + bot_move_failed, + MOVE_LOADER_UNBLOCKING_DOWNING) +{ + loader_down (); + move_data.loader_unblocking_retry = 2; + return FSM_NEXT (MOVE_LOADER_UNBLOCKING_UPING, bot_move_failed); +} + +FSM_TRANS (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, + /* rotate towards next position. */ + path_found_rotate, MOVE_ROTATING, + /* move to next position. */ + path_found, MOVE_MOVING, + /* post failure. */ + no_path_found, MOVE_IDLE) +{ + /* Try to move. */ + uint8_t next = move_path_init (); + if (next) + { + if (next == 2) + return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, path_found_rotate); + else + return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, path_found); + } + else + { + main_post_event (FSM_EVENT (AI, move_fsm_failed)); + return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, no_path_found); + } +} + +FSM_TRANS (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, + /* move backward. + * loader up. */ + tryagain, MOVE_LOADER_UNBLOCKING_UPING, + /* rotate towards next position. */ + tryout_path_found_rotate, MOVE_ROTATING, + /* move to next position. */ + tryout_path_found, MOVE_ROTATING, + /* post failure. */ + tryout_no_path_found, MOVE_IDLE) +{ + if (--move_data.loader_unblocking_retry) + { + asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE); + loader_up (); + return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryagain); + } + else + { + /* Try to move. */ + uint8_t next = move_path_init (); + if (next) + { + if (next == 2) + return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_path_found_rotate); + else + return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_path_found); + } + else + { + main_post_event (FSM_EVENT (AI, move_fsm_failed)); + return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_no_path_found); + } + } +} + +FSM_TRANS_TIMEOUT (MOVE_LOADER_UNBLOCKING_DOWNING, 450, + /* move backward. + * loader up. */ + tryagain, MOVE_LOADER_UNBLOCKING_UPING, + /* rotate towards next position. */ + tryout_path_found_rotate, MOVE_ROTATING, + /* move to next position. */ + tryout_path_found, MOVE_ROTATING, + /* post failure. */ + tryout_no_path_found, MOVE_IDLE) +{ + if (--move_data.loader_unblocking_retry) + { + asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE); + loader_up (); + return FSM_NEXT_TIMEOUT (MOVE_LOADER_UNBLOCKING_DOWNING, tryagain); + } + else + { + /* Try to move. */ + uint8_t next = move_path_init (); + if (next) + { + if (next == 2) + return FSM_NEXT_TIMEOUT (MOVE_LOADER_UNBLOCKING_DOWNING, tryout_path_found_rotate); + else + return FSM_NEXT_TIMEOUT (MOVE_LOADER_UNBLOCKING_DOWNING, tryout_path_found); + } + else + { + main_post_event (FSM_EVENT (AI, move_fsm_failed)); + return FSM_NEXT_TIMEOUT (MOVE_LOADER_UNBLOCKING_DOWNING, tryout_no_path_found); + } + } +} + diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm deleted file mode 100644 index add29b05..00000000 --- a/digital/io/src/move.fsm +++ /dev/null @@ -1,137 +0,0 @@ -# Move FSM -# This FSM is responsible to move the bot from the current position to the -# desired one. It will try to avoid obstacle and manage the case when the -# asserv is blocked. -move - move with avoid obstacle. - -States: - *MOVE_IDLE - waiting for the beginning of the move FSM. - MOVE_ROTATING [timeout=1125] - rotating towards next point. - MOVE_MOVING [timeout=2250] - moving to a position (intermediate or final). - MOVE_MOVING_BACKWARD_TO_TURN_FREELY - moving backward to go away from what is blocking the bot. - MOVE_WAIT_FOR_CLEAR_PATH [timeout=255] - waiting for obstacle to disappear. - MOVE_LOADER_UNBLOCKING_UPING - moving loader up and move backward to unblock loader. - MOVE_LOADER_UNBLOCKING_DOWNING [timeout=450] - moving loader down. - -Events: - move_start - initialize the FSM and start the movement directly. - obstacle_in_front - the bot has seen something (front is the same when going backward). - -MOVE_IDLE: - move_start: path_found_rotate -> MOVE_ROTATING - rotate towards next position. - move_start: path_found -> MOVE_MOVING - move to next position. - move_start: no_path_found -> MOVE_IDLE - post failure event. - -MOVE_ROTATING: - bot_move_succeed -> MOVE_MOVING - move to next position. - bot_move_failed -> MOVE_MOVING - move to next position. - state_timeout -> MOVE_MOVING - move to next position. - loader_errored -> MOVE_LOADER_UNBLOCKING_UPING - move backward - loader up - -MOVE_MOVING: - bot_move_succeed: done -> MOVE_IDLE - post success event. - bot_move_succeed: path_found_rotate -> MOVE_ROTATING - rotate towards next position. - bot_move_succeed: path_found -> MOVE_MOVING - move to next position. - bot_move_succeed: no_path_found -> MOVE_IDLE - post failure event. - bot_move_failed -> MOVE_MOVING_BACKWARD_TO_TURN_FREELY - reset final_move. - move backward to turn freely. - state_timeout -> MOVE_MOVING_BACKWARD_TO_TURN_FREELY - reset final_move. - move backward to turn freely. - obstacle_in_front: tryagain -> MOVE_WAIT_FOR_CLEAR_PATH - reset final_move. - stop the bot. - obstacle_in_front: tryout -> MOVE_IDLE - stop the bot. - post failure event. - loader_errored -> MOVE_LOADER_UNBLOCKING_UPING - move backward - loader up - -MOVE_MOVING_BACKWARD_TO_TURN_FREELY: - bot_move_succeed: tryout -> MOVE_IDLE - post failure event. - bot_move_succeed: path_found_rotate -> MOVE_ROTATING - rotate towards next position. - bot_move_succeed: path_found -> MOVE_MOVING - move to next position. - bot_move_succeed: no_path_found -> MOVE_IDLE - post failure event. - bot_move_failed: tryout -> MOVE_IDLE - post failure event. - bot_move_failed: path_found_rotate -> MOVE_ROTATING - rotate towards next position. - bot_move_failed: path_found -> MOVE_MOVING - move to next position. - bot_move_failed: no_path_found_tryagain -> MOVE_WAIT_FOR_CLEAR_PATH - nothing to do. - bot_move_failed: no_path_found_tryout -> MOVE_IDLE - post failure event. - -MOVE_WAIT_FOR_CLEAR_PATH: - state_timeout: path_found_rotate -> MOVE_ROTATING - rotate towards next position. - state_timeout: path_found -> MOVE_MOVING - move to next position. - state_timeout: no_path_found_tryagain -> . - decrement counter. - state_timeout: no_path_found_tryout -> MOVE_IDLE - post failure. - -MOVE_LOADER_UNBLOCKING_UPING: - bot_move_succeed -> MOVE_LOADER_UNBLOCKING_DOWNING - loader down - reset unblocking retry counter - bot_move_failed -> MOVE_LOADER_UNBLOCKING_DOWNING - loader down - reset unblocking retry counter - -MOVE_LOADER_UNBLOCKING_DOWNING: - loader_downed: path_found_rotate -> MOVE_ROTATING - rotate towards next position. - loader_downed: path_found -> MOVE_MOVING - move to next position. - loader_downed: no_path_found -> MOVE_IDLE - post failure. - loader_errored: tryagain -> MOVE_LOADER_UNBLOCKING_UPING - move backward - loader up - loader_errored: tryout_path_found_rotate -> MOVE_ROTATING - rotate towards next position. - loader_errored: tryout_path_found -> MOVE_ROTATING - move to next position. - loader_errored: tryout_no_path_found -> MOVE_IDLE - post failure. - state_timeout: tryagain -> MOVE_LOADER_UNBLOCKING_UPING - move backward - loader up - state_timeout: tryout_path_found_rotate -> MOVE_ROTATING - rotate towards next position. - state_timeout: tryout_path_found -> MOVE_ROTATING - move to next position. - state_timeout: tryout_no_path_found -> MOVE_IDLE - post failure. - diff --git a/digital/io/src/top.c b/digital/io/src/top.c index 5bfeaa4d..bc6eb20e 100644 --- a/digital/io/src/top.c +++ b/digital/io/src/top.c @@ -22,6 +22,10 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ + +#define FSM_NAME AI + +#include "fsm.h" #include "common.h" #include "defs.h" #include "top.h" @@ -88,3 +92,358 @@ top_collect (uint8_t force) } } +FSM_INIT + +FSM_STATES (IDLE, + WAIT_INIT_TO_FINISH, + FIRST_GO_BEGIN_OF_LINE_FAST, + FIRST_GO_END_OF_LINE_FAST, + FIRST_GO_END_OF_LINE_SLOW, + UNLOAD, + UNLOAD_LOADER_UP, + UNLOAD_FACE_BIN, + UNLOAD_FACE_BIN_UNBLOCK, + UNLOAD_BACK_BIN, + UNLOAD_UNLOAD, + COLLECT, + COLLECT_SLOW_MOTION, + COLLECT_BLACK) + +FSM_EVENTS (start, + move_fsm_succeed, + move_fsm_failed, + bot_move_succeed, + bot_move_failed, + init_match_is_started, + in_field, + asserv_last_cmd_ack, + clamp_succeed) + +FSM_START_WITH (IDLE) + +/* Nothing to do. */ +FSM_TRANS (IDLE, start, WAIT_INIT_TO_FINISH) +{ + return FSM_NEXT (IDLE, start); +} + +/* + * the match start + * set fast speed + * go to first sequence begin of line + */ +FSM_TRANS (WAIT_INIT_TO_FINISH, + init_match_is_started, + FIRST_GO_BEGIN_OF_LINE_FAST) +{ + asserv_set_speed (BOT_MOVE_FAST); + move_start_noangle (PG_VECT (375, 1503), 0, 0); + return FSM_NEXT (WAIT_INIT_TO_FINISH, init_match_is_started); +} + +/* + * go to end of line + * loader down + */ +FSM_TRANS (FIRST_GO_BEGIN_OF_LINE_FAST, + move_fsm_succeed, + FIRST_GO_END_OF_LINE_FAST) +{ + move_start_noangle (PG_VECT (2625, 253), 0, 0); + loader_down (); + return FSM_NEXT (FIRST_GO_BEGIN_OF_LINE_FAST, move_fsm_succeed); +} + +/* + * set slow speed + * retry + */ +FSM_TRANS (FIRST_GO_BEGIN_OF_LINE_FAST, + move_fsm_failed, + FIRST_GO_BEGIN_OF_LINE_FAST) +{ + asserv_set_speed (BOT_MOVE_SLOW); + move_start_noangle (PG_VECT (375, 1503), 0, 0); + return FSM_NEXT (FIRST_GO_BEGIN_OF_LINE_FAST, move_fsm_failed); +} + +/* + * set slow speed + */ +FSM_TRANS (FIRST_GO_END_OF_LINE_FAST, + in_field, + FIRST_GO_END_OF_LINE_SLOW) +{ + asserv_set_speed (BOT_MOVE_SLOW); + return FSM_NEXT (FIRST_GO_END_OF_LINE_FAST, in_field); +} + +/* + * set slow speed + * move loader up + */ +FSM_TRANS (FIRST_GO_END_OF_LINE_FAST, + move_fsm_succeed, + UNLOAD_LOADER_UP) +{ + asserv_set_speed (BOT_MOVE_SLOW); + loader_up (); + return FSM_NEXT (FIRST_GO_END_OF_LINE_FAST, move_fsm_succeed); +} + +/* + * set slow speed + * retry + */ +FSM_TRANS (FIRST_GO_END_OF_LINE_FAST, + move_fsm_failed, + FIRST_GO_END_OF_LINE_SLOW) +{ + asserv_set_speed (BOT_MOVE_SLOW); + move_start_noangle (PG_VECT (2625, 253), 0, 0); + return FSM_NEXT (FIRST_GO_END_OF_LINE_FAST, move_fsm_failed); +} + +/* + * move loader up + */ +FSM_TRANS (FIRST_GO_END_OF_LINE_SLOW, + move_fsm_succeed, + UNLOAD_LOADER_UP) +{ + loader_up (); + return FSM_NEXT (FIRST_GO_END_OF_LINE_SLOW, move_fsm_succeed); +} + +/* + * retry + */ +FSM_TRANS (FIRST_GO_END_OF_LINE_SLOW, + move_fsm_failed, + FIRST_GO_END_OF_LINE_SLOW) +{ + move_start_noangle (PG_VECT (2625, 253), 0, 0); + return FSM_NEXT (FIRST_GO_END_OF_LINE_SLOW, move_fsm_failed); +} + +/* + * move loader up + */ +FSM_TRANS (UNLOAD, move_fsm_succeed, UNLOAD_LOADER_UP) +{ + loader_up (); + return FSM_NEXT (UNLOAD, move_fsm_succeed); +} + +/* + * retry + */ +FSM_TRANS (UNLOAD, move_fsm_failed, UNLOAD) +{ + move_start_noangle (PG_VECT (2625, 253), 0, 0); + return FSM_NEXT (UNLOAD, move_fsm_failed); +} + +/* + * turn toward bin + */ +FSM_TRANS (UNLOAD_LOADER_UP, loader_uped, UNLOAD_FACE_BIN) +{ + asserv_goto_angle (PG_A_DEG (90)); + return FSM_NEXT (UNLOAD_LOADER_UP, loader_uped); +} + +/* + * turn toward bin + */ +FSM_TRANS (UNLOAD_LOADER_UP, loader_errored, UNLOAD_FACE_BIN) +{ + asserv_goto_angle (PG_A_DEG (90)); + return FSM_NEXT (UNLOAD_LOADER_UP, loader_errored); +} + +/* + * go backward to bin + */ +FSM_TRANS (UNLOAD_FACE_BIN, bot_move_succeed, UNLOAD_BACK_BIN) +{ + asserv_move_linearly (-(128 + 250 / 2 - BOT_SIZE_BACK - 50)); + return FSM_NEXT (UNLOAD_FACE_BIN, bot_move_succeed); +} + +/* + * move backward + */ +FSM_TRANS (UNLOAD_FACE_BIN, bot_move_failed, UNLOAD_FACE_BIN_UNBLOCK) +{ + asserv_move_linearly (-40); + return FSM_NEXT (UNLOAD_FACE_BIN, bot_move_failed); +} + +/* + * turn toward bin + */ +FSM_TRANS (UNLOAD_FACE_BIN_UNBLOCK, bot_move_succeed, UNLOAD_FACE_BIN) +{ + asserv_goto_angle (PG_A_DEG (90)); + return FSM_NEXT (UNLOAD_FACE_BIN_UNBLOCK, bot_move_succeed); +} + +/* + * turn toward bin + */ +FSM_TRANS (UNLOAD_FACE_BIN_UNBLOCK, bot_move_failed, UNLOAD_FACE_BIN) +{ + asserv_goto_angle (PG_A_DEG (90)); + return FSM_NEXT (UNLOAD_FACE_BIN_UNBLOCK, bot_move_failed); +} + +/* + * unload + */ +FSM_TRANS (UNLOAD_BACK_BIN, bot_move_succeed, UNLOAD_UNLOAD) +{ + asserv_move_motor1_absolute (0, BOT_GATE_SPEED); + return FSM_NEXT (UNLOAD_BACK_BIN, bot_move_succeed); +} + +/* + * unload + */ +FSM_TRANS (UNLOAD_BACK_BIN, bot_move_failed, UNLOAD_UNLOAD) +{ + asserv_move_motor1_absolute (0, BOT_GATE_SPEED); + return FSM_NEXT (UNLOAD_BACK_BIN, bot_move_failed); +} + +/* + * close gate + * loader down + * choose best food to collect + */ +FSM_TRANS_TIMEOUT (UNLOAD_UNLOAD, 255, COLLECT) +{ + loader_elements = 0; + asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED); + loader_down (); + top_collect (1); + return FSM_NEXT_TIMEOUT (UNLOAD_UNLOAD); +} + +/* + * post loader_element event + */ +FSM_TRANS (COLLECT, move_fsm_succeed, + unload, UNLOAD, + collect, COLLECT, + slow_motion, COLLECT_SLOW_MOTION) +{ + int16_t slow_motion = food_slow_motion (top_food); + if (slow_motion) + { + FSM_HANDLE (AI, loader_element); + return FSM_NEXT (COLLECT, move_fsm_succeed, slow_motion); + } + else if (top_collect (0)) + return FSM_NEXT (COLLECT, move_fsm_succeed, collect); + else + return FSM_NEXT (COLLECT, move_fsm_succeed, unload); +} + +/* + * cancel this element + */ +FSM_TRANS (COLLECT, move_fsm_failed, + unload, UNLOAD, + collect, COLLECT) +{ + if (top_collect (0)) + { + food_cancel (top_food); + return FSM_NEXT (COLLECT, move_fsm_failed, collect); + } + else + return FSM_NEXT (COLLECT, move_fsm_failed, unload); +} + +/* + * move backward + * mark as black + */ +FSM_TRANS (COLLECT, loader_black, COLLECT_BLACK) +{ + asserv_move_linearly (-90); + food_black (top_food); + return FSM_NEXT (COLLECT, loader_black); +} + +/* + * collect + */ +FSM_TRANS (COLLECT_SLOW_MOTION, loader_downed, + unload, UNLOAD, + collect, COLLECT) +{ + if (top_collect (0)) + return FSM_NEXT (COLLECT_SLOW_MOTION, loader_downed, collect); + else + return FSM_NEXT (COLLECT_SLOW_MOTION, loader_downed, unload); +} + +/* + * same as above + */ +FSM_TRANS (COLLECT_SLOW_MOTION, loader_errored, + unload, UNLOAD, + collect, COLLECT) +{ + if (top_collect (0)) + return FSM_NEXT (COLLECT_SLOW_MOTION, loader_errored, collect); + else + return FSM_NEXT (COLLECT_SLOW_MOTION, loader_errored, unload); +} + +/* + * same as above + */ +FSM_TRANS_TIMEOUT (COLLECT_SLOW_MOTION, 450, + unload, UNLOAD, + collect, COLLECT) +{ + if (top_collect (0)) + return FSM_NEXT_TIMEOUT (COLLECT_SLOW_MOTION, collect); + else + return FSM_NEXT_TIMEOUT (COLLECT_SLOW_MOTION, unload); +} + +/* + * move backward + * mark as black + */ +FSM_TRANS (COLLECT_SLOW_MOTION, loader_black, COLLECT_BLACK) +{ + asserv_move_linearly (-90); + food_black (top_food); + return FSM_NEXT (COLLECT_SLOW_MOTION, loader_black); +} + +FSM_TRANS (COLLECT_BLACK, bot_move_succeed, + unload, UNLOAD, + collect, COLLECT) +{ + if (top_collect (0)) + return FSM_NEXT (COLLECT_BLACK, bot_move_succeed, collect); + else + return FSM_NEXT (COLLECT_BLACK, bot_move_succeed, unload); +} + +FSM_TRANS (COLLECT_BLACK, bot_move_failed, + unload, UNLOAD, + collect, COLLECT) +{ + if (top_collect (0)) + return FSM_NEXT (COLLECT_BLACK, bot_move_failed, collect); + else + return FSM_NEXT (COLLECT_BLACK, bot_move_failed, unload); +} + diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm deleted file mode 100644 index 277f0a5a..00000000 --- a/digital/io/src/top.fsm +++ /dev/null @@ -1,162 +0,0 @@ -# Top FSM. -top - Main FSM calling other FSM. - -States: - *IDLE - waiting for the beginning of the top FSM. - WAIT_INIT_TO_FINISH - waiting for the end of init. - FIRST_GO_BEGIN_OF_LINE_FAST - first sequence, go to start of diagonal line - FIRST_GO_END_OF_LINE_FAST - first sequence, go to end of line - FIRST_GO_END_OF_LINE_SLOW - slow down to take elements - UNLOAD - go to unloading position - UNLOAD_LOADER_UP - move loader to up position - UNLOAD_FACE_BIN - orient robot to unload angle - UNLOAD_FACE_BIN_UNBLOCK - move backward to unblock - UNLOAD_BACK_BIN - move back to unload bin - UNLOAD_UNLOAD[timeout=225] - unloading, wait - COLLECT - collecting elements - COLLECT_SLOW_MOTION[timeout=450] - picking element before bumper contact - COLLECT_BLACK - moving backward after trying to collect a black corn - -Events: - start - initialize the FSM. - move_fsm_succeed - when the move FSM returns with success. - move_fsm_failed - when the move FSM returns with an error. - bot_move_succeed - when the bot has succeed its asserv movement. - bot_move_failed - when the bot has failed its asserv movement. - init_match_is_started - when the init FSM has finished and the match start. - in_field - the robot is in the zone containing interesting elements. - state_timeout - timeout in cycle count. - asserv_last_cmd_ack - last command sent to the asserv board has been acquitted. - clamp_succeed - asserv success result - -import init.fsm -import hola.fsm -import loader.fsm -import move.fsm - -IDLE: - start -> WAIT_INIT_TO_FINISH - nothing to do. - -WAIT_INIT_TO_FINISH: - init_match_is_started -> FIRST_GO_BEGIN_OF_LINE_FAST - the match start - set fast speed - go to first sequence begin of line - -FIRST_GO_BEGIN_OF_LINE_FAST: - move_fsm_succeed -> FIRST_GO_END_OF_LINE_FAST - go to end of line - loader down - move_fsm_failed -> . - set slow speed - retry - -FIRST_GO_END_OF_LINE_FAST: - in_field -> FIRST_GO_END_OF_LINE_SLOW - set slow speed - move_fsm_succeed -> UNLOAD_LOADER_UP - set slow speed - move loader up - move_fsm_failed -> FIRST_GO_END_OF_LINE_SLOW - set slow speed - retry - -FIRST_GO_END_OF_LINE_SLOW: - move_fsm_succeed -> UNLOAD_LOADER_UP - move loader up - move_fsm_failed -> FIRST_GO_END_OF_LINE_SLOW - retry - -UNLOAD: - move_fsm_succeed -> UNLOAD_LOADER_UP - move loader up - move_fsm_failed -> . - retry - -UNLOAD_LOADER_UP: - loader_uped -> UNLOAD_FACE_BIN - turn toward bin - loader_errored -> UNLOAD_FACE_BIN - turn toward bin - -UNLOAD_FACE_BIN: - bot_move_succeed -> UNLOAD_BACK_BIN - go backward to bin - bot_move_failed -> UNLOAD_FACE_BIN_UNBLOCK - move backward - -UNLOAD_FACE_BIN_UNBLOCK: - bot_move_succeed -> UNLOAD_FACE_BIN - turn toward bin - bot_move_failed -> UNLOAD_FACE_BIN - turn toward bin - -UNLOAD_BACK_BIN: - bot_move_succeed -> UNLOAD_UNLOAD - unload - bot_move_failed -> UNLOAD_UNLOAD - unload - -UNLOAD_UNLOAD: - state_timeout -> COLLECT - close gate - loader down - choose best food to collect - -COLLECT: - move_fsm_succeed: unload -> UNLOAD - move_fsm_succeed: collect -> COLLECT - move_fsm_succeed: slow_motion -> COLLECT_SLOW_MOTION - post loader_element event - move_fsm_failed: unload -> UNLOAD - move_fsm_failed: collect -> COLLECT - cancel this element - loader_black -> COLLECT_BLACK - move backward - mark as black - -COLLECT_SLOW_MOTION: - loader_downed: unload -> UNLOAD - loader_downed: collect -> COLLECT - collect - loader_errored: unload -> UNLOAD - loader_errored: collect -> COLLECT - same as above - state_timeout: unload -> UNLOAD - state_timeout: collect -> COLLECT - same as above - loader_black -> COLLECT_BLACK - move backward - mark as black - -COLLECT_BLACK: - bot_move_succeed: unload -> UNLOAD - bot_move_succeed: collect -> COLLECT - bot_move_failed: unload -> UNLOAD - bot_move_failed: collect -> COLLECT -- cgit v1.2.3