From f416af1eeed92e9508744544e39983af82479965 Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Mon, 18 May 2009 22:29:25 +0200 Subject: * digital/io/src: - fix event sent to the wrong FSM, - start top FSM, - rewrite event generated by move FSM when its finished, - init FSM generates an event when the match start, - init FSM starts the chrono when the match start, - top FSM now uses the event generated by the init FSM to know when the match start and not the jack states. --- digital/io/src/init.c | 4 +++ digital/io/src/init.fsm | 9 ++++- digital/io/src/init.h | 6 ++++ digital/io/src/init_cb.c | 18 +++++++++- digital/io/src/main.c | 16 ++++++--- digital/io/src/move_cb.c | 4 +-- digital/io/src/top.fsm | 61 ++++++++++++++++----------------- digital/io/src/top_cb.c | 88 +++++++++++++++++++++++++++++++++--------------- 8 files changed, 139 insertions(+), 67 deletions(-) (limited to 'digital/io/src') diff --git a/digital/io/src/init.c b/digital/io/src/init.c index 7b0e584c..1d0d8d82 100644 --- a/digital/io/src/init.c +++ b/digital/io/src/init.c @@ -22,4 +22,8 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ +#include "common.h" + #include "init.h" + +uint8_t init_match_is_started = 0; diff --git a/digital/io/src/init.fsm b/digital/io/src/init.fsm index 72db9a6f..89ec26e6 100644 --- a/digital/io/src/init.fsm +++ b/digital/io/src/init.fsm @@ -41,6 +41,8 @@ States: go backward from the wall for a specific distance. GO_TO_START_POSITION go to the start position. + WAIT_SECOND_JACK_OUT + wait the jack to be removed from the bot for the second time. Events: start @@ -114,6 +116,11 @@ GO_AWAY_FROM_THE_WALL_AGAIN: go to the start position with a go to movement. GO_TO_START_POSITION: - bot_move_succeed -> IDLE + bot_move_succeed -> WAIT_SECOND_JACK_OUT nothing to do, the bot is at the start position. +WAIT_SECOND_JACK_OUT: + jack_removed_from_bot -> IDLE + tell other FSM the match begins. + start the chrono. + diff --git a/digital/io/src/init.h b/digital/io/src/init.h index 99de38f4..ef3074fb 100644 --- a/digital/io/src/init.h +++ b/digital/io/src/init.h @@ -30,4 +30,10 @@ */ #define INIT_DIST (PG_START_ZONE_LENGTH - BOT_LENGTH - 50) +/** + * The match begin! + * This variable is set to 1 when the match start. + */ +extern uint8_t init_match_is_started; + #endif /* init_h */ diff --git a/digital/io/src/init_cb.c b/digital/io/src/init_cb.c index f763aaed..063314a3 100644 --- a/digital/io/src/init_cb.c +++ b/digital/io/src/init_cb.c @@ -31,6 +31,7 @@ #include "main.h" #include "aquajim.h" #include "switch.h" +#include "chrono.h" #include "modules/trace/trace.h" /* @@ -216,7 +217,7 @@ init__GO_AWAY_FROM_THE_WALL_AGAIN__bot_move_succeed (void) /* * GO_TO_START_POSITION =bot_move_succeed=> - * => IDLE + * => WAIT_SECOND_JACK_OUT * nothing to do, the bot is at the start position. */ fsm_branch_t @@ -225,4 +226,19 @@ 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 6b8643eb..e4455844 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -54,6 +54,7 @@ #include "elevator.h" #include "filterbridge.h" #include "cylinder.h" +#include "init.h" #include "io.h" @@ -172,7 +173,7 @@ main_event_to_fsm (void) /* 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 (&init_fsm, TOP_EVENT_asserv_last_cmd_ack); + FSM_HANDLE_EVENT (&top_fsm, TOP_EVENT_asserv_last_cmd_ack); asserv_status_e move_status = none, @@ -241,8 +242,6 @@ main_event_to_fsm (void) /* Jack */ if(switch_get_jack()) { - FSM_HANDLE_EVENT (&top_fsm, - TOP_EVENT_jack_removed_from_bot); FSM_HANDLE_EVENT (&init_fsm, INIT_EVENT_jack_removed_from_bot); } @@ -250,8 +249,6 @@ main_event_to_fsm (void) { FSM_HANDLE_EVENT (&init_fsm, INIT_EVENT_jack_inserted_into_bot); - FSM_HANDLE_EVENT (&top_fsm, - TOP_EVENT_jack_inserted_into_bot); FSM_HANDLE_EVENT (&elevator_fsm, ELEVATOR_EVENT_jack_inserted_into_bot); FSM_HANDLE_EVENT (&cylinder_fsm, @@ -260,6 +257,14 @@ main_event_to_fsm (void) CYLINDER_EVENT_jack_inserted_into_bot); } + if (init_match_is_started) + { + FSM_HANDLE_EVENT (&top_fsm, TOP_EVENT_init_match_is_started); + + /* This must be done in the last part of this block. */ + init_match_is_started = 0; + } + /* Event generated at the end of the sub FSM to post to the top FSM */ if (main_post_event_for_top_fsm != 0xFF) { @@ -348,6 +353,7 @@ main_init (void) /* 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); fsm_handle_event (&filterbridge_fsm, FILTERBRIDGE_EVENT_start); fsm_handle_event (&elevator_fsm, ELEVATOR_EVENT_start); fsm_handle_event (&cylinder_fsm, CYLINDER_EVENT_start); diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c index abf22c74..a5622369 100644 --- a/digital/io/src/move_cb.c +++ b/digital/io/src/move_cb.c @@ -342,7 +342,7 @@ move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_succeed (void) (move_data.final.y == move_data.intermediate.y)) { /* Post an event for the top FSM to tell we have finished */ - main_post_event_for_top_fsm = TOP_EVENT_move_fsm_finished; + main_post_event_for_top_fsm = TOP_EVENT_move_fsm_succeed; return move_next_branch (MOVING_TO_INTERMEDIATE_POSITION, bot_move_succeed, final_position); } else @@ -513,7 +513,7 @@ fsm_branch_t move__MOVING_TO_FINAL_POSITION__bot_move_succeed (void) { /* Post an event for the top FSM to tell we have finished */ - main_post_event_for_top_fsm = TOP_EVENT_move_fsm_finished; + main_post_event_for_top_fsm = TOP_EVENT_move_fsm_succeed; return move_next (MOVING_TO_FINAL_POSITION, bot_move_succeed); } diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm index 3c3eaa7e..04e0d982 100644 --- a/digital/io/src/top.fsm +++ b/digital/io/src/top.fsm @@ -5,50 +5,51 @@ top States: IDLE waiting for the beginning of the top FSM. - WAIT_FIRST_JACK_IN - waiting for the jack to be inserted into the bot for the first time. - WAIT_FIRST_JACK_OUT - waiting for the jack to be removed from the bot for the first time. - WAIT_SECOND_JACK_IN - waiting for the jack to be inserted into the bot for the second time. - WAIT_SECOND_JACK_OUT - waiting for the jack to be removed into the bot for the second time. - GET_PUCK_FROM_THE_GROUND - get some pucks from the ground. + WAIT_INIT_TO_FINISH + waiting for the end of init. + GET_PUCK_FROM_THE_GROUND [timeout=1590] + get some pucks from the ground (with a timeout set to 7 seconds). GET_PUCK_FROM_DISTRIBUTOR get some pucks from the distributor. - + STOP_TO_GO_TO_UNLOAD_AREA + stop the move FSM before starting a new one. + STOP_TO_GET_PUCK_FROM_DISTRIBUTOR + stop the move FSM before starting a new one. Events: start initialize the FSM. - move_fsm_finished - when the move FSM returns. - jack_inserted_into_bot - the jack is inserted into the bot. - jack_removed_from_bot - the jack is removed from the bot. + move_fsm_succeed + when the move FSM returns with success. + move_fsm_failed + when the move FSM returns with an error. + init_match_is_started + when the init FSM has finished and the match start. state_timeout timeout in cycle count. asserv_last_cmd_ack last command sent to the asserv board has been acquitted. + bot_is_full_of_puck + the bot has four pucks inside. IDLE: - start -> WAIT_FIRST_JACK_IN + start -> WAIT_INIT_TO_FINISH nothing to do. -WAIT_FIRST_JACK_IN: - jack_inserted_into_bot -> WAIT_FIRST_JACK_OUT - nothing to do. +WAIT_INIT_TO_FINISH: + init_match_is_started -> GET_PUCK_FROM_THE_GROUND + the match start, try to get some puck from the ground. -WAIT_FIRST_JACK_OUT: - jack_removed_from_bot -> WAIT_SECOND_JACK_IN - nothing to do. +GET_PUCK_FROM_THE_GROUND: + move_fsm_succeed: already_six_pucks_or_no_next_position -> GET_PUCK_FROM_DISTRIBUTOR + get the next distributor position and launch move FSM to go there. + move_fsm_succeed: next_position_exists -> . + go to the next position using move FSM. + move_fsm_failed -> GET_PUCK_FROM_DISTRIBUTOR + we have failed to do a move, go the distributor. + bot_is_full_of_puck -> STOP_TO_GO_TO_UNLOAD_AREA + stop move FSM. + state_timeout -> STOP_TO_GET_PUCK_FROM_DISTRIBUTOR + too much time lost to get puck from the ground, stop move FSM. -WAIT_SECOND_JACK_IN: - jack_removed_from_bot -> WAIT_SECOND_JACK_OUT - nothing to do. -WAIT_SECOND_JACK_OUT: - jack_removed_from_bot -> GET_PUCK_FROM_THE_GROUND - the match start, try to get some puck from the ground. diff --git a/digital/io/src/top_cb.c b/digital/io/src/top_cb.c index a10939d5..19127922 100644 --- a/digital/io/src/top_cb.c +++ b/digital/io/src/top_cb.c @@ -1,21 +1,41 @@ -/* - * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT! +/* top_cb.c */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2009 Dufour Jérémy + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. * - * Skeleton for top callbacks implementation. + * 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. * - * Top FSM with the Marcel's strategy + * }}} + * Main FSM calling other FSM. */ + + #include "common.h" #include "fsm.h" #include "top_cb.h" -#include "chrono.h" #include "asserv.h" #include "playground.h" #include "move.h" /* move FSM */ /* * IDLE =start=> - * => WAIT_FIRST_JACK_IN + * => WAIT_INIT_TO_FINISH * nothing to do. */ fsm_branch_t @@ -25,49 +45,61 @@ top__IDLE__start (void) } /* - * WAIT_FIRST_JACK_IN =jack_inserted_into_bot=> - * => WAIT_FIRST_JACK_OUT - * nothing to do. + * WAIT_INIT_TO_FINISH =init_match_is_started=> + * => GET_PUCK_FROM_THE_GROUND + * the match start, try to get some puck from the ground. */ fsm_branch_t -top__WAIT_FIRST_JACK_IN__jack_inserted_into_bot (void) +top__WAIT_INIT_TO_FINISH__init_match_is_started (void) { - return top_next (WAIT_FIRST_JACK_IN, jack_inserted_into_bot); + return top_next (WAIT_INIT_TO_FINISH, init_match_is_started); } /* - * WAIT_FIRST_JACK_OUT =jack_removed_from_bot=> - * => WAIT_SECOND_JACK_IN - * nothing to do. + * GET_PUCK_FROM_THE_GROUND =move_fsm_succeed=> + * already_six_pucks_or_no_next_position => GET_PUCK_FROM_DISTRIBUTOR + * get the next distributor position and launch move FSM to go there. + * next_position_exists => GET_PUCK_FROM_THE_GROUND + * go to the next position using move FSM. */ fsm_branch_t -top__WAIT_FIRST_JACK_OUT__jack_removed_from_bot (void) +top__GET_PUCK_FROM_THE_GROUND__move_fsm_succeed (void) { - return top_next (WAIT_FIRST_JACK_OUT, jack_removed_from_bot); + return top_next_branch (GET_PUCK_FROM_THE_GROUND, move_fsm_succeed, already_six_pucks_or_no_next_position); + return top_next_branch (GET_PUCK_FROM_THE_GROUND, move_fsm_succeed, next_position_exists); } /* - * WAIT_SECOND_JACK_IN =jack_removed_from_bot=> - * => WAIT_SECOND_JACK_OUT - * nothing to do. + * GET_PUCK_FROM_THE_GROUND =move_fsm_failed=> + * => GET_PUCK_FROM_DISTRIBUTOR + * we have failed to do a move, go the distributor. */ fsm_branch_t -top__WAIT_SECOND_JACK_IN__jack_removed_from_bot (void) +top__GET_PUCK_FROM_THE_GROUND__move_fsm_failed (void) { - return top_next (WAIT_SECOND_JACK_IN, jack_removed_from_bot); + return top_next (GET_PUCK_FROM_THE_GROUND, move_fsm_failed); } /* - * WAIT_SECOND_JACK_OUT =jack_removed_from_bot=> - * => GET_PUCK_FROM_THE_GROUND - * the match start, try to get some puck from the ground. + * GET_PUCK_FROM_THE_GROUND =bot_is_full_of_puck=> + * => STOP_TO_GO_TO_UNLOAD_AREA + * stop move FSM. + */ +fsm_branch_t +top__GET_PUCK_FROM_THE_GROUND__bot_is_full_of_puck (void) +{ + return top_next (GET_PUCK_FROM_THE_GROUND, bot_is_full_of_puck); +} + +/* + * GET_PUCK_FROM_THE_GROUND =state_timeout=> + * => STOP_TO_GET_PUCK_FROM_DISTRIBUTOR + * too much time lost to get puck from the ground, stop move FSM. */ fsm_branch_t -top__WAIT_SECOND_JACK_OUT__jack_removed_from_bot (void) +top__GET_PUCK_FROM_THE_GROUND__state_timeout (void) { - /* Start the chronometer */ - chrono_init (); - return top_next (WAIT_SECOND_JACK_OUT, jack_removed_from_bot); + return top_next (GET_PUCK_FROM_THE_GROUND, state_timeout); } -- cgit v1.2.3