summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--digital/io-hub/src/guybrush/strat.c23
-rw-r--r--digital/io-hub/src/guybrush/strat.h2
-rw-r--r--digital/io-hub/src/guybrush/top.c69
3 files changed, 85 insertions, 9 deletions
diff --git a/digital/io-hub/src/guybrush/strat.c b/digital/io-hub/src/guybrush/strat.c
index fa1908e5..6ed38d01 100644
--- a/digital/io-hub/src/guybrush/strat.c
+++ b/digital/io-hub/src/guybrush/strat.c
@@ -79,6 +79,8 @@ struct strat_t
uint8_t last_decision;
/** Place of last decision. */
uint8_t last_place;
+ /** Robot content estimation. */
+ uint8_t load;
/** Places information. */
struct strat_place_dyn_t place[STRAT_PLACE_NB];
};
@@ -97,6 +99,13 @@ uint8_t
strat_decision (vect_t *pos)
{
uint8_t i;
+ if (strat.load > 1)
+ {
+ strat.last_decision = STRAT_DECISION_UNLOAD;
+ pos->x = PG_X (BOT_SIZE_RADIUS + 30);
+ pos->y = PG_Y (PG_LENGTH / 2);
+ return strat.last_decision;
+ }
for (i = 0; i < STRAT_PLACE_NB; i++)
{
if (strat.place[i].valid)
@@ -114,7 +123,17 @@ strat_decision (vect_t *pos)
void
strat_success (void)
{
- assert (strat.last_decision != (uint8_t) -1);
- strat.place[strat.last_place].valid = 0;
+ switch (strat.last_decision)
+ {
+ case STRAT_DECISION_TOTEM:
+ strat.place[strat.last_place].valid = 0;
+ strat.load++;
+ break;
+ case STRAT_DECISION_UNLOAD:
+ strat.load = 0;
+ break;
+ default:
+ assert (0);
+ }
}
diff --git a/digital/io-hub/src/guybrush/strat.h b/digital/io-hub/src/guybrush/strat.h
index 33839a75..8ecf14b4 100644
--- a/digital/io-hub/src/guybrush/strat.h
+++ b/digital/io-hub/src/guybrush/strat.h
@@ -30,6 +30,8 @@ enum
{
/** Go collect items on a totem side. */
STRAT_DECISION_TOTEM,
+ /** Go unload everything in our boat. */
+ STRAT_DECISION_UNLOAD,
};
/** Initialise strategy once color is known. */
diff --git a/digital/io-hub/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c
index fd979e85..439ac1a6 100644
--- a/digital/io-hub/src/guybrush/top.c
+++ b/digital/io-hub/src/guybrush/top.c
@@ -38,6 +38,8 @@
#include "strat.h"
#include "path.h"
+#include "output_defs.h"
+
/*
* Here is the top FSM. This FSM is suppose to give life to the robot with an
* impression of intelligence... Well...
@@ -48,6 +50,7 @@ FSM_INIT
FSM_STATES (
/* Initial state. */
TOP_START,
+
/* Going to a collect position above or below a totem. */
TOP_TOTEM_GOING,
/* Approaching a totem. */
@@ -55,7 +58,12 @@ FSM_STATES (
/* Pushing until full contact. */
TOP_TOTEM_PUSHING,
/* Going back after totem has been emptied. */
- TOP_TOTEM_GOING_BACK)
+ TOP_TOTEM_GOING_BACK,
+
+ /* Going to an unload position. */
+ TOP_UNLOAD_GOING,
+ /* Unloading, waiting for elements to fall. */
+ TOP_UNLOADING)
FSM_START_WITH (TOP_START)
@@ -80,6 +88,16 @@ top_go_totem (void)
move_start (pos, 0);
}
+/** Go unload. */
+static void
+top_go_unload (void)
+{
+ position_t pos;
+ pos.v = top.decision_pos;
+ pos.a = PG_A_DEG (70);
+ move_start (pos, 0);
+}
+
/** Call strat to make a decision, apply it and return the decision to go to
* the next state. */
static uint8_t
@@ -91,19 +109,36 @@ top_decision (void)
case STRAT_DECISION_TOTEM:
top_go_totem ();
break;
+ case STRAT_DECISION_UNLOAD:
+ top_go_unload ();
+ break;
default:
assert (0);
}
return decision;
}
-FSM_TRANS (TOP_START, init_start_round, TOP_TOTEM_GOING)
+#define RETURN_TOP_DECISION_SWITCH(state, event) do { \
+ switch (top_decision ()) \
+ { \
+ default: assert (0); \
+ case STRAT_DECISION_TOTEM: \
+ return FSM_NEXT (state, event, totem); \
+ case STRAT_DECISION_UNLOAD: \
+ return FSM_NEXT (state, event, unload); \
+ } \
+} while (0)
+
+FSM_TRANS (TOP_START, init_start_round,
+ totem, TOP_TOTEM_GOING,
+ unload, TOP_UNLOAD_GOING)
{
strat_init ();
- top_decision ();
- return FSM_NEXT (TOP_START, init_start_round);
+ RETURN_TOP_DECISION_SWITCH (TOP_START, init_start_round);
}
+/** TOTEM */
+
FSM_TRANS (TOP_TOTEM_GOING, move_success, TOP_TOTEM_APPROACHING)
{
asserv_move_linearly (PATH_GRID_CLEARANCE_MM - BOT_SIZE_FRONT - 30);
@@ -124,9 +159,29 @@ FSM_TRANS (TOP_TOTEM_PUSHING, robot_move_success, TOP_TOTEM_GOING_BACK)
return FSM_NEXT (TOP_TOTEM_PUSHING, robot_move_success);
}
-FSM_TRANS (TOP_TOTEM_GOING_BACK, move_success, TOP_TOTEM_GOING)
+FSM_TRANS (TOP_TOTEM_GOING_BACK, move_success,
+ totem, TOP_TOTEM_GOING,
+ unload, TOP_UNLOAD_GOING)
+{
+ RETURN_TOP_DECISION_SWITCH (TOP_TOTEM_GOING_BACK, move_success);
+}
+
+/** UNLOAD */
+
+FSM_TRANS (TOP_UNLOAD_GOING, move_success, TOP_UNLOADING)
{
- top_decision ();
- return FSM_NEXT (TOP_TOTEM_GOING_BACK, move_success);
+ IO_SET (OUTPUT_DOOR_OPEN);
+ IO_CLR (OUTPUT_DOOR_CLOSE);
+ return FSM_NEXT (TOP_UNLOAD_GOING, move_success);
+}
+
+FSM_TRANS_TIMEOUT (TOP_UNLOADING, 250,
+ totem, TOP_TOTEM_GOING,
+ unload, TOP_UNLOAD_GOING)
+{
+ strat_success ();
+ IO_CLR (OUTPUT_DOOR_OPEN);
+ IO_SET (OUTPUT_DOOR_CLOSE);
+ RETURN_TOP_DECISION_SWITCH (TOP_UNLOADING, TOP_UNLOADING_TIMEOUT);
}