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/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 ++++++++++++------------ 5 files changed, 39 insertions(+), 77 deletions(-) (limited to 'digital/io') 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