summaryrefslogtreecommitdiff
path: root/digital/io
diff options
context:
space:
mode:
authorNicolas Schodet2011-05-04 19:58:33 +0200
committerNicolas Schodet2011-05-04 19:58:33 +0200
commit4a6a90cb512f1a43e1229ef0d13ef3e5abd25b9c (patch)
tree506bc6e9b0983fcc8c88047ea20b85a12e4deb09 /digital/io
parent42163ae3d62642753899c389a7a1a42ae9bd68a9 (diff)
digital/{ai,io}: move fsm queue to ai
Diffstat (limited to 'digital/io')
-rw-r--r--digital/io/src/Makefile4
-rw-r--r--digital/io/src/loader.c44
-rw-r--r--digital/io/src/main.c40
-rw-r--r--digital/io/src/main.h4
-rw-r--r--digital/io/src/move.c24
5 files changed, 39 insertions, 77 deletions
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);
}
}