From 4a6a90cb512f1a43e1229ef0d13ef3e5abd25b9c Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 4 May 2011 19:58:33 +0200 Subject: digital/{ai,io}: move fsm queue to ai --- digital/ai/src/fsm/fsm_queue.c | 71 ++++++++++++++++++++++++++++++++++++++++++ digital/ai/src/fsm/fsm_queue.h | 41 ++++++++++++++++++++++++ digital/io/src/Makefile | 4 +-- digital/io/src/loader.c | 44 +++++++++++++------------- digital/io/src/main.c | 40 ++---------------------- digital/io/src/main.h | 4 --- digital/io/src/move.c | 24 +++++++------- 7 files changed, 151 insertions(+), 77 deletions(-) create mode 100644 digital/ai/src/fsm/fsm_queue.c create mode 100644 digital/ai/src/fsm/fsm_queue.h (limited to 'digital') diff --git a/digital/ai/src/fsm/fsm_queue.c b/digital/ai/src/fsm/fsm_queue.c new file mode 100644 index 00000000..357fc39e --- /dev/null +++ b/digital/ai/src/fsm/fsm_queue.c @@ -0,0 +1,71 @@ +/* fsm_queue.c */ +/* ai - Robot Artificial Intelligence. {{{ + * + * Copyright (C) 2011 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_queue.h" + +/** Maximum number of events in post queue. */ +#define FSM_QUEUE_SIZE 8 + +/** Module context. */ +struct fsm_queue_t +{ + /** Events to post to the FSM in next iteration. */ + uint8_t events[FSM_QUEUE_SIZE]; + /** First event in the queue. */ + uint8_t head; + /** Number of events in the queue. */ + uint8_t nb; +}; + +/** Global context. */ +static struct fsm_queue_t fsm_queue_global; +# define ctx fsm_queue_global + +void +fsm_queue_post_event (uint8_t event) +{ + assert (ctx.nb < FSM_QUEUE_SIZE); + uint8_t tail = (ctx.head + ctx.nb) % FSM_QUEUE_SIZE; + ctx.events[tail] = event; + ctx.nb++; +} + +uint8_t +fsm_queue_poll (void) +{ + return ctx.nb; +} + +uint8_t +fsm_queue_pop_event (void) +{ + uint8_t e; + assert (ctx.nb > 0); + e = ctx.events[ctx.head]; + ctx.nb--; + ctx.head = (ctx.head + 1) % FSM_QUEUE_SIZE; + return e; +} + diff --git a/digital/ai/src/fsm/fsm_queue.h b/digital/ai/src/fsm/fsm_queue.h new file mode 100644 index 00000000..ce230bec --- /dev/null +++ b/digital/ai/src/fsm/fsm_queue.h @@ -0,0 +1,41 @@ +#ifndef fsm_queue_h +#define fsm_queue_h +/* fsm_queue.h */ +/* ai - Robot Artificial Intelligence. {{{ + * + * Copyright (C) 2011 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. + * + * }}} */ + +/** Post an event to be processed in the next main loop. */ +void +fsm_queue_post_event (uint8_t event); + +/** Poll for event in the event queue (return non zero if there is an event + * pending). */ +uint8_t +fsm_queue_poll (void); + +/** Pop one event from the event queue. */ +uint8_t +fsm_queue_pop_event (void); + +#endif /* fsm_queue_h */ diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile index a21b0c1b..5cbea0ba 100644 --- a/digital/io/src/Makefile +++ b/digital/io/src/Makefile @@ -3,7 +3,7 @@ BASE = ../../avr # Name of the program to build. PROGS = io # Sources to compile. -io_SOURCES = main.c servo.avr.c eeprom.avr.c pwm.c \ +io_SOURCES = main.c fsm_queue.c servo.avr.c eeprom.avr.c pwm.c \ switch.avr.c chrono.c timer.avr.c servo_pos.c \ twi_master.c asserv.c mimot.c \ simu.host.c contact.c usdist.c radar.c \ @@ -12,7 +12,7 @@ io_SOURCES = main.c servo.avr.c eeprom.avr.c pwm.c \ # Modules needed for IO. MODULES = proto uart twi utils adc math/fixed math/geometry path/astar \ trace flash spi -AI_MODULES = twi_master common utils +AI_MODULES = twi_master common utils fsm # Configuration file. CONFIGFILE = avrconfig.h # IO board use an ATMega128. diff --git a/digital/io/src/loader.c b/digital/io/src/loader.c index 8dd557da..e8f8c139 100644 --- a/digital/io/src/loader.c +++ b/digital/io/src/loader.c @@ -30,7 +30,7 @@ #include "asserv.h" #include "mimot.h" #include "bot.h" -#include "main.h" +#include "fsm_queue.h" #include "food.h" uint8_t loader_elements; @@ -279,7 +279,7 @@ FSM_TRANS (LOADER_UPING, elevator_succeed, LOADER_UP) { - main_post_event (FSM_EVENT (AI,loader_uped)); + fsm_queue_post_event (FSM_EVENT (AI,loader_uped)); return FSM_NEXT (LOADER_UPING, elevator_succeed); } @@ -290,7 +290,7 @@ FSM_TRANS (LOADER_UPING, elevator_failed, LOADER_ERROR) { - main_post_event (FSM_EVENT (AI, loader_errored)); + fsm_queue_post_event (FSM_EVENT (AI, loader_errored)); return FSM_NEXT (LOADER_UPING, elevator_failed); } @@ -314,7 +314,7 @@ FSM_TRANS (LOADER_DOWNING, LOADER_DOWN) { asserv_motor0_free (); - main_post_event (FSM_EVENT (AI, loader_downed)); + fsm_queue_post_event (FSM_EVENT (AI, loader_downed)); return FSM_NEXT (LOADER_DOWNING, elevator_succeed); } @@ -325,7 +325,7 @@ FSM_TRANS (LOADER_DOWNING, elevator_failed, LOADER_ERROR) { - main_post_event (FSM_EVENT (AI, loader_errored)); + fsm_queue_post_event (FSM_EVENT (AI, loader_errored)); return FSM_NEXT (LOADER_DOWNING, elevator_failed); } @@ -338,7 +338,7 @@ FSM_TRANS (LOADER_DOWNING, LOADER_ERROR) { asserv_motor0_free (); - main_post_event (FSM_EVENT (AI, loader_errored)); + fsm_queue_post_event (FSM_EVENT (AI, loader_errored)); return FSM_NEXT (LOADER_DOWNING, loader_element); } @@ -396,7 +396,7 @@ FSM_TRANS (LOADER_ERROR_DOWNING, elevator_failed, LOADER_ERROR) { - main_post_event (FSM_EVENT (AI, loader_errored)); + fsm_queue_post_event (FSM_EVENT (AI, loader_errored)); return FSM_NEXT (LOADER_ERROR_DOWNING, elevator_failed); } @@ -406,7 +406,7 @@ FSM_TRANS (LOADER_ERROR_DOWNING, FSM_TRANS_TIMEOUT (LOADER_ERROR_DOWNING, 225, LOADER_ERROR) { - main_post_event (FSM_EVENT (AI, loader_errored)); + fsm_queue_post_event (FSM_EVENT (AI, loader_errored)); return FSM_NEXT_TIMEOUT (LOADER_ERROR_DOWNING); } @@ -417,7 +417,7 @@ FSM_TRANS (LOADER_ERROR_DOWNING_OPEN, clamp_succeed, LOADER_DOWN) { - main_post_event (FSM_EVENT (AI, loader_downed)); + fsm_queue_post_event (FSM_EVENT (AI, loader_downed)); return FSM_NEXT (LOADER_ERROR_DOWNING_OPEN, clamp_succeed); } @@ -428,7 +428,7 @@ FSM_TRANS (LOADER_ERROR_DOWNING_OPEN, clamp_failed, LOADER_ERROR) { - main_post_event (FSM_EVENT (AI, loader_errored)); + fsm_queue_post_event (FSM_EVENT (AI, loader_errored)); return FSM_NEXT (LOADER_ERROR_DOWNING_OPEN, clamp_failed); } @@ -438,7 +438,7 @@ FSM_TRANS (LOADER_ERROR_DOWNING_OPEN, FSM_TRANS_TIMEOUT (LOADER_ERROR_DOWNING_OPEN, 225, LOADER_ERROR) { - main_post_event (FSM_EVENT (AI, loader_errored)); + fsm_queue_post_event (FSM_EVENT (AI, loader_errored)); return FSM_NEXT_TIMEOUT (LOADER_ERROR_DOWNING_OPEN); } @@ -461,7 +461,7 @@ FSM_TRANS (LOADER_ERROR_UPING, elevator_failed, LOADER_ERROR) { - main_post_event (FSM_EVENT (AI, loader_errored)); + fsm_queue_post_event (FSM_EVENT (AI, loader_errored)); return FSM_NEXT (LOADER_ERROR_UPING, elevator_failed); } @@ -471,7 +471,7 @@ FSM_TRANS (LOADER_ERROR_UPING, FSM_TRANS_TIMEOUT (LOADER_ERROR_UPING, 225, LOADER_ERROR) { - main_post_event (FSM_EVENT (AI, loader_errored)); + fsm_queue_post_event (FSM_EVENT (AI, loader_errored)); return FSM_NEXT_TIMEOUT (LOADER_ERROR_UPING); } @@ -482,7 +482,7 @@ FSM_TRANS (LOADER_ERROR_UPING_OPEN, clamp_succeed, LOADER_UP) { - main_post_event (FSM_EVENT (AI, loader_uped)); + fsm_queue_post_event (FSM_EVENT (AI, loader_uped)); return FSM_NEXT (LOADER_ERROR_UPING_OPEN, clamp_succeed); } @@ -493,7 +493,7 @@ FSM_TRANS (LOADER_ERROR_UPING_OPEN, clamp_failed, LOADER_ERROR) { - main_post_event (FSM_EVENT (AI, loader_errored)); + fsm_queue_post_event (FSM_EVENT (AI, loader_errored)); return FSM_NEXT (LOADER_ERROR_UPING_OPEN, clamp_failed); } @@ -503,7 +503,7 @@ FSM_TRANS (LOADER_ERROR_UPING_OPEN, FSM_TRANS_TIMEOUT (LOADER_ERROR_UPING_OPEN, 225, LOADER_ERROR) { - main_post_event (FSM_EVENT (AI, loader_errored)); + fsm_queue_post_event (FSM_EVENT (AI, loader_errored)); return FSM_NEXT_TIMEOUT (LOADER_ERROR_UPING_OPEN); } @@ -561,10 +561,10 @@ FSM_TRANS (LOADER_LOAD_UPING, { if (loader_elements) loader_elements--; - main_post_event (FSM_EVENT (AI, loader_black)); + fsm_queue_post_event (FSM_EVENT (AI, loader_black)); } else - main_post_event (FSM_EVENT (AI, loader_errored)); + fsm_queue_post_event (FSM_EVENT (AI, loader_errored)); mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); return FSM_NEXT (LOADER_LOAD_UPING, elevator_failed); @@ -585,7 +585,7 @@ FSM_TRANS (LOADER_LOAD_UNLOADING, elevator_failed, LOADER_ERROR) { - main_post_event (FSM_EVENT (AI, loader_errored)); + fsm_queue_post_event (FSM_EVENT (AI, loader_errored)); mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); return FSM_NEXT (LOADER_LOAD_UNLOADING, elevator_failed); @@ -618,7 +618,7 @@ FSM_TRANS (LOADER_LOAD_UNLOADING_OPEN, clamp_failed, LOADER_ERROR) { - main_post_event (FSM_EVENT (AI, loader_errored)); + fsm_queue_post_event (FSM_EVENT (AI, loader_errored)); return FSM_NEXT (LOADER_LOAD_UNLOADING_OPEN, clamp_failed); } @@ -636,7 +636,7 @@ FSM_TRANS (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, } else { - main_post_event (FSM_EVENT (AI, loader_downed)); + fsm_queue_post_event (FSM_EVENT (AI, loader_downed)); return FSM_NEXT (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, down); } } @@ -648,6 +648,6 @@ FSM_TRANS (LOADER_LOAD_EMPTY_OPEN, clamp_failed, LOADER_ERROR) { - main_post_event (FSM_EVENT (AI, loader_errored)); + fsm_queue_post_event (FSM_EVENT (AI, loader_errored)); return FSM_NEXT (LOADER_LOAD_EMPTY_OPEN, clamp_failed); } diff --git a/digital/io/src/main.c b/digital/io/src/main.c index be04366f..22964ddd 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -48,6 +48,7 @@ #include "eeprom.h" /* Parameters loaded/stored in the EEPROM */ #define FSM_NAME AI #include "fsm.h" +#include "fsm_queue.h" #include "bot.h" #include "servo_pos.h" #include "usdist.h" @@ -78,18 +79,6 @@ static void main_loop (void); */ enum team_color_e team_color; -/** Maximum number of events in post queue. */ -#define MAIN_POST_EVENT_QUEUE_SIZE 8 - -/** Events to post to the FSM in next iteration. */ -uint8_t main_post_event_queue[MAIN_POST_EVENT_QUEUE_SIZE]; - -/** Number of events in the post queue. */ -uint8_t main_post_event_queue_nb; - -/** First event in the post queue. */ -uint8_t main_post_event_queue_head; - /** Obstacles positions, updated using radar module. */ vect_t main_obstacles_pos[2]; @@ -114,29 +103,6 @@ static uint8_t main_stats_asserv_, main_stats_asserv_cpt_; */ static uint8_t main_stats_timer_; -void -main_post_event (uint8_t event) -{ - assert (main_post_event_queue_nb < MAIN_POST_EVENT_QUEUE_SIZE); - uint8_t tail = (main_post_event_queue_head + main_post_event_queue_nb) - % MAIN_POST_EVENT_QUEUE_SIZE; - main_post_event_queue[tail] = event; - main_post_event_queue_nb++; -} - -/** Pop one event from the event queue. */ -static uint8_t -main_pop_event (void) -{ - uint8_t e; - assert (main_post_event_queue_nb > 0); - e = main_post_event_queue[main_post_event_queue_head]; - main_post_event_queue_nb--; - main_post_event_queue_head = (main_post_event_queue_head + 1) - % MAIN_POST_EVENT_QUEUE_SIZE; - return e; -} - /** * Main events management. * This function is responsible to get all events and send them to the @@ -226,13 +192,13 @@ main_event_to_fsm (void) } /* Event generated at the end of the sub FSM to post to the top FSM */ - if (main_post_event_queue_nb) + if (fsm_queue_poll ()) { /* We must post the event at the end of this block because it * will issue a continue and every instruction after will * never be executed. */ /* We need to save the event before reseting it */ - uint8_t save_event = main_pop_event (); + uint8_t save_event = fsm_queue_pop_event (); /* Post the event */ FSM_HANDLE_VAR_E (AI, save_event); } diff --git a/digital/io/src/main.h b/digital/io/src/main.h index 9096b532..2185b90c 100644 --- a/digital/io/src/main.h +++ b/digital/io/src/main.h @@ -29,8 +29,4 @@ extern vect_t main_obstacles_pos[2]; extern uint8_t main_obstacles_nb; -/** Post an event to be processed in the next main loop. */ -void -main_post_event (uint8_t event); - #endif /* main_h */ diff --git a/digital/io/src/move.c b/digital/io/src/move.c index fa65418d..60db0272 100644 --- a/digital/io/src/move.c +++ b/digital/io/src/move.c @@ -37,7 +37,7 @@ #include "loader.h" #include "modules/path/path.h" #include "modules/utils/utils.h" -#include "main.h" /* main_post_event_for_top_fsm */ +#include "fsm_queue.h" #include "modules/math/fixed/fixed.h" /* fixed_* */ #include "modules/trace/trace.h" #include "debug.host.h" @@ -305,7 +305,7 @@ FSM_TRANS (MOVE_IDLE, move_start, } else { - main_post_event (FSM_EVENT (AI, move_fsm_failed)); + fsm_queue_post_event (FSM_EVENT (AI, move_fsm_failed)); return FSM_NEXT (MOVE_IDLE, move_start, no_path_found); } } @@ -370,7 +370,7 @@ FSM_TRANS (MOVE_MOVING, bot_move_succeed, { if (move_data.final_move) { - main_post_event (FSM_EVENT (AI, move_fsm_succeed)); + fsm_queue_post_event (FSM_EVENT (AI, move_fsm_succeed)); return FSM_NEXT (MOVE_MOVING, bot_move_succeed, done); } else @@ -440,7 +440,7 @@ FSM_TRANS (MOVE_MOVING, obstacle_in_front, asserv_stop_motor (); if (--move_data.try_again_counter == 0) { - main_post_event (FSM_EVENT (AI, move_fsm_failed)); + fsm_queue_post_event (FSM_EVENT (AI, move_fsm_failed)); return FSM_NEXT (MOVE_MOVING, obstacle_in_front, tryout); } else @@ -472,7 +472,7 @@ FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, { if (--move_data.try_again_counter == 0) { - main_post_event (FSM_EVENT (AI, move_fsm_failed)); + fsm_queue_post_event (FSM_EVENT (AI, move_fsm_failed)); return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, tryout); } else @@ -487,7 +487,7 @@ FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, } else { - main_post_event (FSM_EVENT (AI, move_fsm_failed)); + fsm_queue_post_event (FSM_EVENT (AI, move_fsm_failed)); return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, no_path_found); } } @@ -507,7 +507,7 @@ FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, { if (--move_data.try_again_counter == 0) { - main_post_event (FSM_EVENT (AI, move_fsm_failed)); + fsm_queue_post_event (FSM_EVENT (AI, move_fsm_failed)); return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, tryout); } else @@ -524,7 +524,7 @@ FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, { if (--move_data.try_again_counter == 0) { - main_post_event (FSM_EVENT (AI, move_fsm_failed)); + fsm_queue_post_event (FSM_EVENT (AI, move_fsm_failed)); return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_path_found_tryout); } else @@ -557,7 +557,7 @@ FSM_TRANS_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, 255, /* Error, no new position, should we try again? */ if (--move_data.try_again_counter == 0) { - main_post_event (FSM_EVENT (AI, move_fsm_failed)); + fsm_queue_post_event (FSM_EVENT (AI, move_fsm_failed)); return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, no_path_found_tryout); } else @@ -610,7 +610,7 @@ FSM_TRANS (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, } else { - main_post_event (FSM_EVENT (AI, move_fsm_failed)); + fsm_queue_post_event (FSM_EVENT (AI, move_fsm_failed)); return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, no_path_found); } } @@ -645,7 +645,7 @@ FSM_TRANS (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, } else { - main_post_event (FSM_EVENT (AI, move_fsm_failed)); + fsm_queue_post_event (FSM_EVENT (AI, move_fsm_failed)); return FSM_NEXT (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_no_path_found); } } @@ -681,7 +681,7 @@ FSM_TRANS_TIMEOUT (MOVE_LOADER_UNBLOCKING_DOWNING, 450, } else { - main_post_event (FSM_EVENT (AI, move_fsm_failed)); + fsm_queue_post_event (FSM_EVENT (AI, move_fsm_failed)); return FSM_NEXT_TIMEOUT (MOVE_LOADER_UNBLOCKING_DOWNING, tryout_no_path_found); } } -- cgit v1.2.3