From 645b7d11df890ed99e5b00c8272d4e794060f703 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 13 May 2010 01:28:31 +0200 Subject: digital/io/src: add event queue --- digital/io/src/ai_loader_cb.c | 6 +++--- digital/io/src/ai_move_cb.c | 10 +++++----- digital/io/src/main.c | 44 +++++++++++++++++++++++++++++++++++-------- digital/io/src/main.h | 12 ++++-------- 4 files changed, 48 insertions(+), 24 deletions(-) diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c index 3a2782ab..7de8a652 100644 --- a/digital/io/src/ai_loader_cb.c +++ b/digital/io/src/ai_loader_cb.c @@ -216,7 +216,7 @@ ai__LOADER_DOWN__loader_element (void) fsm_branch_t ai__LOADER_UPING__elevator_succeed (void) { - main_post_event_for_top_fsm = AI_EVENT_loader_uped; + main_post_event (AI_EVENT_loader_uped); return ai_next (LOADER_UPING, elevator_succeed); } @@ -228,7 +228,7 @@ ai__LOADER_UPING__elevator_succeed (void) fsm_branch_t ai__LOADER_UPING__elevator_failed (void) { - main_post_event_for_top_fsm = AI_EVENT_loader_errored; + main_post_event (AI_EVENT_loader_errored); return ai_next (LOADER_UPING, elevator_failed); } @@ -242,7 +242,7 @@ fsm_branch_t ai__LOADER_DOWNING__elevator_succeed (void) { asserv_motor0_free (); - main_post_event_for_top_fsm = AI_EVENT_loader_downed; + main_post_event (AI_EVENT_loader_downed); return ai_next (LOADER_DOWNING, elevator_succeed); } diff --git a/digital/io/src/ai_move_cb.c b/digital/io/src/ai_move_cb.c index eaa0aead..320bd01f 100644 --- a/digital/io/src/ai_move_cb.c +++ b/digital/io/src/ai_move_cb.c @@ -126,7 +126,7 @@ ai__MOVE_IDLE__move_start (void) return ai_next_branch (MOVE_IDLE, move_start, path_found); else { - main_post_event_for_top_fsm = AI_EVENT_move_fsm_failed; + main_post_event (AI_EVENT_move_fsm_failed); return ai_next_branch (MOVE_IDLE, move_start, no_path_found); } } @@ -145,7 +145,7 @@ ai__MOVE_MOVING__bot_move_succeed (void) { if (move_data.final_move) { - main_post_event_for_top_fsm = AI_EVENT_move_fsm_succeed; + main_post_event (AI_EVENT_move_fsm_succeed); return ai_next_branch (MOVE_MOVING, bot_move_succeed, done); } else if (move_get_next_position ()) @@ -154,7 +154,7 @@ ai__MOVE_MOVING__bot_move_succeed (void) } else { - main_post_event_for_top_fsm = AI_EVENT_move_fsm_failed; + main_post_event (AI_EVENT_move_fsm_failed); return ai_next_branch (MOVE_MOVING, bot_move_succeed, no_path_found); } } @@ -216,7 +216,7 @@ ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_succeed (void) } else { - main_post_event_for_top_fsm = AI_EVENT_move_fsm_failed; + main_post_event (AI_EVENT_move_fsm_failed); return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, no_path_found); } } @@ -260,7 +260,7 @@ ai__MOVE_WAIT_FOR_CLEAR_PATH__state_timeout (void) /* Error, no new position, should we try again? */ if (--move_data.try_again_counter == 0) { - main_post_event_for_top_fsm = AI_EVENT_move_fsm_failed; + main_post_event (AI_EVENT_move_fsm_failed); return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, no_path_found_and_no_try_again); } diff --git a/digital/io/src/main.c b/digital/io/src/main.c index f05653eb..6571e9a4 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -75,10 +75,17 @@ static void main_loop (void); */ enum team_color_e bot_color; -/** - * Post a event to the top FSM in the next iteration of main loop. - */ -uint8_t main_post_event_for_top_fsm = 0xFF; +/** 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]; @@ -104,6 +111,29 @@ 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 @@ -183,15 +213,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_for_top_fsm != 0xFF) + if (main_post_event_queue_nb) { /* 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_post_event_for_top_fsm; - /* Reset */ - main_post_event_for_top_fsm = 0xFF; + uint8_t save_event = main_pop_event (); /* Post the event */ FSM_HANDLE_EVENT (&ai_fsm, save_event); } diff --git a/digital/io/src/main.h b/digital/io/src/main.h index 0c2083ba..9096b532 100644 --- a/digital/io/src/main.h +++ b/digital/io/src/main.h @@ -25,16 +25,12 @@ * * }}} */ -/** - * 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. - * It will be posted and cleared in the next main loop iteration. - */ -extern uint8_t main_post_event_for_top_fsm; - 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 */ -- cgit v1.2.3