summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
Diffstat (limited to 'digital')
-rw-r--r--digital/ai/src/fsm/fsm_queue.c71
-rw-r--r--digital/ai/src/fsm/fsm_queue.h41
-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
7 files changed, 151 insertions, 77 deletions
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);
}
}