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