summaryrefslogtreecommitdiffhomepage
path: root/digital/io-hub/src
diff options
context:
space:
mode:
authorNicolas Schodet2011-05-14 01:36:57 +0200
committerNicolas Schodet2011-05-14 01:38:19 +0200
commit366eeaccdca0ce784415a7b1bbc7e75f7c889af7 (patch)
treec18f78fa54c488d50ca41478f0e8981ab24dda05 /digital/io-hub/src
parentc8d4bd0e0f79dc8e49ad0074a1e67317e903c41f (diff)
digital/io-hub: add first element handling in clamp
Diffstat (limited to 'digital/io-hub/src')
-rw-r--r--digital/io-hub/src/robospierre/Makefile4
-rw-r--r--digital/io-hub/src/robospierre/clamp.c146
-rw-r--r--digital/io-hub/src/robospierre/clamp.h5
-rw-r--r--digital/io-hub/src/robospierre/logistic.c212
-rw-r--r--digital/io-hub/src/robospierre/logistic.h68
-rw-r--r--digital/io-hub/src/robospierre/main.c23
6 files changed, 453 insertions, 5 deletions
diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile
index 21411631..20e9dee0 100644
--- a/digital/io-hub/src/robospierre/Makefile
+++ b/digital/io-hub/src/robospierre/Makefile
@@ -4,8 +4,8 @@ BASE = ../../../avr
PROGS = io_hub
# Sources to compile.
io_hub_SOURCES = main.c \
- clamp.c \
- fsm.host.c fsm_AI_gen.avr.c \
+ clamp.c logistic.c \
+ fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \
pwm.avr.c pwm.host.c \
contact.avr.c contact.host.c \
twi_master.c asserv.c mimot.c \
diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c
index 5d9533fa..1ee020e9 100644
--- a/digital/io-hub/src/robospierre/clamp.c
+++ b/digital/io-hub/src/robospierre/clamp.c
@@ -25,16 +25,42 @@
#include "common.h"
#include "clamp.h"
-#define FSM_NAME AI
-#include "fsm.h"
-
#include "mimot.h"
#include "pwm.h"
+#include "contact.h"
#include "bot.h"
+#include "element.h"
+
+#define FSM_NAME AI
+#include "fsm.h"
+#include "fsm_queue.h"
+
+#include "logistic.h"
+
+/*
+ * There is two FSM in this file.
+ *
+ * The clamp FSM handles high level clamp behaviour, new elements, drop, and
+ * gives orders to the clamp move FSM.
+ *
+ * The clamp move FSM only handle moving the clamp without load or moving an
+ * element from a slot to another one.
+ */
FSM_INIT
FSM_STATES (
+ /* Initial state, to be complete with full initialisation. */
+ CLAMP_START,
+ /* Returning to idle position. */
+ CLAMP_GOING_IDLE,
+ /* Waiting external events, clamp at middle level. */
+ CLAMP_IDLE,
+ /* Taking an element at bottom slots. */
+ CLAMP_TAKING_DOOR_CLOSING,
+ /* Moving elements around. */
+ CLAMP_MOVING_ELEMENT,
+
/* Waiting movement order. */
CLAMP_MOVE_IDLE,
/* Moving to a final or intermediary position. */
@@ -53,8 +79,14 @@ FSM_STATES (
CLAMP_MOVE_DST_CLAMP_OPENING)
FSM_EVENTS (
+ /* Here for the moment, to be moved later. */
+ start,
+ /* New element inside bottom slot. */
+ clamp_new_element,
/* Order to move the clamp. */
clamp_move,
+ /* Clamp movement success. */
+ clamp_move_success,
/* Elevation and elevation motor success. */
clamp_elevation_rotation_success,
/* Elevation motor failure. */
@@ -62,6 +94,7 @@ FSM_EVENTS (
/* Rotation motor failure. */
clamp_rotation_failure)
+FSM_START_WITH (CLAMP_START)
FSM_START_WITH (CLAMP_MOVE_IDLE)
/** Clamp context. */
@@ -73,6 +106,10 @@ struct clamp_t
uint8_t pos_request;
/** Element moving destination. */
uint8_t moving_to;
+ /** Position of a new element. */
+ uint8_t pos_new;
+ /** New element kind. */
+ uint8_t new_element;
};
/** Global context. */
@@ -123,16 +160,51 @@ clamp_move (uint8_t pos)
ctx.moving_to = CLAMP_POS_NB;
FSM_HANDLE (AI, clamp_move);
}
+ else
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_move_success));
}
void
clamp_move_element (uint8_t from, uint8_t to)
{
+ assert (from != to);
ctx.pos_request = from;
ctx.moving_to = to;
FSM_HANDLE (AI, clamp_move);
}
+void
+clamp_new_element (uint8_t pos, uint8_t element)
+{
+ assert (pos == CLAMP_SLOT_FRONT_BOTTOM || pos == CLAMP_SLOT_BACK_BOTTOM);
+ ctx.pos_new = pos;
+ ctx.new_element = element;
+ FSM_HANDLE (AI, clamp_new_element);
+}
+
+uint8_t
+clamp_handle_event (void)
+{
+ if (FSM_CAN_HANDLE (AI, clamp_new_element))
+ {
+ /* XXX: temporary hack. */
+ uint8_t element = contact_get_color () ? ELEMENT_PAWN : ELEMENT_KING;
+ if (!IO_GET (CONTACT_FRONT_BOTTOM)
+ && !logistic_global.slots[CLAMP_SLOT_FRONT_BOTTOM])
+ {
+ clamp_new_element (CLAMP_SLOT_FRONT_BOTTOM, element);
+ return 1;
+ }
+ if (!IO_GET (CONTACT_BACK_BOTTOM)
+ && !logistic_global.slots[CLAMP_SLOT_BACK_BOTTOM])
+ {
+ clamp_new_element (CLAMP_SLOT_BACK_BOTTOM, element);
+ return 1;
+ }
+ }
+ return 0;
+}
+
/** Find next position and start motors. */
static void
clamp_route (void)
@@ -192,6 +264,72 @@ clamp_route (void)
ctx.pos_current = pos_new;
}
+/* CLAMP FSM */
+
+FSM_TRANS (CLAMP_START, start, CLAMP_GOING_IDLE)
+{
+ clamp_move (CLAMP_SLOT_FRONT_MIDDLE);
+ return FSM_NEXT (CLAMP_START, start);
+}
+
+FSM_TRANS (CLAMP_GOING_IDLE, clamp_move_success, CLAMP_IDLE)
+{
+ return FSM_NEXT (CLAMP_GOING_IDLE, clamp_move_success);
+}
+
+FSM_TRANS (CLAMP_IDLE, clamp_new_element, CLAMP_TAKING_DOOR_CLOSING)
+{
+ pwm_set_timed (clamp_slot_door[ctx.pos_new], BOT_PWM_DOOR_CLOSE);
+ return FSM_NEXT (CLAMP_IDLE, clamp_new_element);
+}
+
+FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME,
+ move_element, CLAMP_MOVING_ELEMENT,
+ move_to_idle, CLAMP_GOING_IDLE,
+ done, CLAMP_IDLE)
+{
+ logistic_element_new (ctx.pos_new, ctx.new_element);
+ if (logistic_global.moving_from != CLAMP_SLOT_NB)
+ {
+ clamp_move_element (logistic_global.moving_from,
+ logistic_global.moving_to);
+ return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, move_element);
+ }
+ else if (logistic_global.clamp_pos_idle != ctx.pos_current)
+ {
+ clamp_move (logistic_global.clamp_pos_idle);
+ return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, move_to_idle);
+ }
+ else
+ return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, done);
+}
+
+FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_success,
+ move_element, CLAMP_MOVING_ELEMENT,
+ move_to_idle, CLAMP_GOING_IDLE,
+ done, CLAMP_IDLE)
+{
+ logistic_element_move_done ();
+ if (logistic_global.moving_from != CLAMP_SLOT_NB)
+ {
+ clamp_move_element (logistic_global.moving_from,
+ logistic_global.moving_to);
+ return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success,
+ move_element);
+ }
+ else if (logistic_global.clamp_pos_idle != ctx.pos_current)
+ {
+ clamp_move (logistic_global.clamp_pos_idle);
+ return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success,
+ move_to_idle);
+ }
+ else
+ return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success,
+ done);
+}
+
+/* CLAMP_MOVE FSM */
+
FSM_TRANS (CLAMP_MOVE_IDLE, clamp_move,
move, CLAMP_MOVE_ROUTING,
move_element, CLAMP_MOVE_SRC_ROUTING,
@@ -223,6 +361,7 @@ FSM_TRANS (CLAMP_MOVE_ROUTING, clamp_elevation_rotation_success,
{
if (ctx.pos_current == ctx.pos_request)
{
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_move_success));
return FSM_NEXT (CLAMP_MOVE_ROUTING, clamp_elevation_rotation_success,
done);
}
@@ -318,6 +457,7 @@ FSM_TRANS_TIMEOUT (CLAMP_MOVE_DST_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME,
FSM_TRANS_TIMEOUT (CLAMP_MOVE_DST_CLAMP_OPENING, BOT_PWM_CLAMP_OPEN_TIME,
CLAMP_MOVE_IDLE)
{
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_move_success));
return FSM_NEXT_TIMEOUT (CLAMP_MOVE_DST_CLAMP_OPENING);
}
diff --git a/digital/io-hub/src/robospierre/clamp.h b/digital/io-hub/src/robospierre/clamp.h
index 7b488f92..8c5f5c4c 100644
--- a/digital/io-hub/src/robospierre/clamp.h
+++ b/digital/io-hub/src/robospierre/clamp.h
@@ -62,4 +62,9 @@ clamp_move (uint8_t pos);
void
clamp_move_element (uint8_t from, uint8_t to);
+/** Examine sensors to generate new events, return non zero if an event was
+ * generated. */
+uint8_t
+clamp_handle_event (void);
+
#endif /* clamp_h */
diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c
new file mode 100644
index 00000000..534f1c46
--- /dev/null
+++ b/digital/io-hub/src/robospierre/logistic.c
@@ -0,0 +1,212 @@
+/* logistic.c */
+/* robospierre - Eurobot 2011 AI. {{{
+ *
+ * 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 "logistic.h"
+
+#include "clamp.h"
+#include "defs.h"
+
+#include "debug.host.h"
+
+/** Handle elements stored inside the robot. */
+
+/** Global context. */
+struct logistic_t logistic_global;
+#define ctx logistic_global
+
+inline void
+logistic_debug_dump (void)
+{
+#ifdef HOST
+ uint8_t i;
+ static const char *names[][CLAMP_SLOT_NB] = {
+ { "f1", "f2", "f3", "b1", "b2", "b3", "s1" },
+ { "F1", "F2", "F3", "B1", "B2", "B3", "S1" }
+ };
+ static const char *names_dir[] = { "--", "<-", "->" };
+ DPRINTF ("%s", names_dir[ctx.collect_direction]);
+ for (i = 0; i < CLAMP_SLOT_NB; i++)
+ {
+ DPRINTF (" %s", ctx.slots[i]
+ ? names[ELEMENT_IS_HEAD (ctx.slots[i]) ? 1 : 0][i]
+ : "__");
+ }
+ if (ctx.moving_from != CLAMP_SLOT_NB)
+ {
+ DPRINTF (" %s => %s", names[0][ctx.moving_from], names[0][ctx.moving_to]);
+ }
+ DPRINTF ("\n");
+#endif
+}
+
+/** Examine current state and take a decision. */
+static void
+logistic_decision (void)
+{
+ uint8_t i;
+ /* If currently moving, do not take decision. */
+ if (ctx.moving_from != CLAMP_SLOT_NB)
+ return;
+ /* Determine collect_direction. */
+ uint8_t front_head = 0, back_head = 0,
+ front_element = 0, back_element = 0;
+ uint8_t collect_direction;
+ for (i = CLAMP_SLOT_FRONT_BOTTOM; i <= CLAMP_SLOT_FRONT_TOP; i++)
+ {
+ if (ctx.slots[i])
+ {
+ front_element++;
+ if (ELEMENT_IS_HEAD (ctx.slots[i]))
+ front_head++;
+ }
+ }
+ for (i = CLAMP_SLOT_BACK_BOTTOM; i <= CLAMP_SLOT_BACK_TOP; i++)
+ {
+ if (ctx.slots[i])
+ {
+ back_element++;
+ if (ELEMENT_IS_HEAD (ctx.slots[i]))
+ back_head++;
+ }
+ }
+ if (front_head < back_head)
+ collect_direction = DIRECTION_FORWARD;
+ else if (front_head > back_head)
+ collect_direction = DIRECTION_BACKWARD;
+ else if (front_head)
+ {
+ if (front_element < back_element)
+ collect_direction = DIRECTION_FORWARD;
+ else if (front_element > back_element)
+ collect_direction = DIRECTION_BACKWARD;
+ else
+ collect_direction = ctx.collect_direction;
+ }
+ else
+ collect_direction = ctx.collect_direction;
+ ctx.collect_direction = collect_direction;
+ /* Now use this direction. */
+ uint8_t collect_bay, storage_bay;
+ uint8_t collect_bay_head, storage_bay_head;
+ if (collect_direction == DIRECTION_FORWARD)
+ {
+ collect_bay = CLAMP_SLOT_FRONT_BOTTOM;
+ storage_bay = CLAMP_SLOT_BACK_BOTTOM;
+ collect_bay_head = front_head;
+ storage_bay_head = back_head;
+ ctx.clamp_pos_idle = CLAMP_SLOT_FRONT_MIDDLE;
+ }
+ else
+ {
+ collect_bay = CLAMP_SLOT_BACK_BOTTOM;
+ storage_bay = CLAMP_SLOT_FRONT_BOTTOM;
+ collect_bay_head = back_head;
+ storage_bay_head = front_head;
+ ctx.clamp_pos_idle = CLAMP_SLOT_BACK_MIDDLE;
+ }
+ /* Find a destination for an element move. */
+ uint8_t moving_to = CLAMP_SLOT_NB;
+ uint8_t moving_from = CLAMP_SLOT_NB;
+ if (!ctx.slots[collect_bay + 1])
+ {
+ /* Movements in collect bay possible. */
+ if (ELEMENT_IS_HEAD (ctx.slots[collect_bay + 0]))
+ {
+ moving_to = collect_bay + 2;
+ moving_from = collect_bay + 0;
+ }
+ }
+ if (moving_to == CLAMP_SLOT_NB && !ctx.slots[storage_bay + 1])
+ {
+ /* No movement yet and movements in storage bay possible. */
+ if (ELEMENT_IS_HEAD (ctx.slots[storage_bay + 0]))
+ {
+ moving_to = storage_bay + 2;
+ moving_from = storage_bay + 0;
+ }
+ else if (storage_bay_head)
+ {
+ if (!ctx.slots[storage_bay + 0])
+ moving_to = storage_bay + 0;
+ else if (!ctx.slots[storage_bay + 1])
+ moving_to = storage_bay + 1;
+ }
+ }
+ if (moving_to == CLAMP_SLOT_NB && !ctx.slots[CLAMP_SLOT_SIDE])
+ {
+ /* No movement yet, store in side slot. */
+ moving_to = CLAMP_SLOT_SIDE;
+ }
+ /* Find a source if available. */
+ if (moving_to != CLAMP_SLOT_NB && moving_from == CLAMP_SLOT_NB)
+ {
+ if (ctx.slots[collect_bay + 0])
+ moving_from = collect_bay + 0;
+ else if (ctx.slots[CLAMP_SLOT_SIDE])
+ moving_from = CLAMP_SLOT_SIDE;
+ }
+ /* Ask for movement. */
+ if (moving_from != CLAMP_SLOT_NB)
+ {
+ ctx.moving_from = moving_from;
+ ctx.moving_to = moving_to;
+ }
+ logistic_debug_dump ();
+}
+
+void
+logistic_init (void)
+{
+ uint8_t i;
+ for (i = 0; i < CLAMP_SLOT_NB; i++)
+ ctx.slots[i] = 0;
+ ctx.moving_from = ctx.moving_to = CLAMP_SLOT_NB;
+ ctx.collect_direction = DIRECTION_FORWARD;
+}
+
+void
+logistic_update (void)
+{
+}
+
+void
+logistic_element_new (uint8_t pos, uint8_t element)
+{
+ assert (pos < CLAMP_SLOT_NB);
+ assert (!ctx.slots[pos]);
+ ctx.slots[pos] = element;
+ logistic_decision ();
+}
+
+void
+logistic_element_move_done (void)
+{
+ assert (!ctx.slots[ctx.moving_to]);
+ ctx.slots[ctx.moving_to] = ctx.slots[ctx.moving_from];
+ ctx.slots[ctx.moving_from] = 0;
+ ctx.moving_from = ctx.moving_to = CLAMP_SLOT_NB;
+ logistic_decision ();
+}
+
diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h
new file mode 100644
index 00000000..75b49d53
--- /dev/null
+++ b/digital/io-hub/src/robospierre/logistic.h
@@ -0,0 +1,68 @@
+#ifndef logistic_h
+#define logistic_h
+/* logistic.h */
+/* robospierre - Eurobot 2011 AI. {{{
+ *
+ * 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 "element.h"
+#include "clamp.h"
+
+/** Logistic context. */
+struct logistic_t
+{
+ /** Current robot content.
+ *
+ * Elements are fully specified (pawn, queen or king). An exception can
+ * occurs when a codebar is read but does not correspond to a valid word.
+ *
+ * When a movement is outgoing, the element is kept in the source slot. */
+ uint8_t slots[CLAMP_SLOT_NB];
+ /** Current element movement source and destination, CLAMP_SLOT_NB if no
+ * current movement. */
+ uint8_t moving_from, moving_to;
+ /** Best collect direction. */
+ uint8_t collect_direction;
+ /** Idle clamp position, depend on collect direction. */
+ uint8_t clamp_pos_idle;
+};
+
+/** Global context. */
+extern struct logistic_t logistic_global;
+
+/** Initialise module. */
+void
+logistic_init (void);
+
+/** To be called at regular interval to check for bad robot state. */
+void
+logistic_update (void);
+
+/** To be called when a new element is entering the robot. */
+void
+logistic_element_new (uint8_t pos, uint8_t element);
+
+/** To be called when a element movement is done. */
+void
+logistic_element_move_done (void);
+
+#endif /* logistic_h */
diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c
index 13bad26c..ffb05a99 100644
--- a/digital/io-hub/src/robospierre/main.c
+++ b/digital/io-hub/src/robospierre/main.c
@@ -44,8 +44,10 @@
#ifdef HOST
# include <string.h>
#endif
+#include "fsm_queue.h"
#include "clamp.h"
+#include "logistic.h"
#include "bot.h"
@@ -78,6 +80,8 @@ main_init (void)
/* IO modules. */
pwm_init ();
contact_init ();
+ /* AI modules. */
+ logistic_init ();
/* Initialization done. */
proto_send0 ('z');
}
@@ -107,6 +111,23 @@ main_event_to_fsm (void)
FSM_HANDLE_E (AI, clamp_elevation_failure);
else if (mimot_motor1_status == failure)
FSM_HANDLE_E (AI, clamp_rotation_failure);
+ /* Clamp specific events. */
+ if (clamp_handle_event ())
+ return;
+ /* Jack, XXX to be changed! */
+ if (!contact_get_jack ())
+ FSM_HANDLE_E (AI, start);
+ /* Events from the event queue. */
+ if (fsm_queue_poll ())
+ {
+ /* We must post the event at the end of this block because if it is
+ * handled, the function will return and every instruction after will
+ * never be executed. */
+ uint8_t save_event = fsm_queue_pop_event ();
+ /* Post the event */
+ FSM_HANDLE_VAR_E (AI, save_event);
+ }
+
}
/** Main (and infinite) loop. */
@@ -132,6 +153,8 @@ main_loop (void)
/* Update IO modules. */
pwm_update ();
contact_update ();
+ /* Update AI modules. */
+ logistic_update ();
/* Only manage events if slaves are synchronised. */
if (twi_master_sync ())
main_event_to_fsm ();