summaryrefslogtreecommitdiffhomepage
path: root/digital/io-hub/src/guybrush/top.c
diff options
context:
space:
mode:
authorNicolas Schodet2012-05-06 16:01:02 +0200
committerNicolas Schodet2012-05-07 16:31:46 +0200
commitc82623b6b12153e697e2226a56ce44773875a3d0 (patch)
tree4ab73c155bfe2e33415ae7da6c5421eb37472306 /digital/io-hub/src/guybrush/top.c
parentf78d43b5767db1ef7e39c871202851fc5d5360c9 (diff)
digital/io-hub/src/guybrush: push bottle
Diffstat (limited to 'digital/io-hub/src/guybrush/top.c')
-rw-r--r--digital/io-hub/src/guybrush/top.c57
1 files changed, 57 insertions, 0 deletions
diff --git a/digital/io-hub/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c
index 439ac1a6..5de7dc85 100644
--- a/digital/io-hub/src/guybrush/top.c
+++ b/digital/io-hub/src/guybrush/top.c
@@ -60,6 +60,15 @@ FSM_STATES (
/* Going back after totem has been emptied. */
TOP_TOTEM_GOING_BACK,
+ /* Going to push a bottle. */
+ TOP_BOTTLE_GOING,
+ /* Approaching the button. */
+ TOP_BOTTLE_APPROACHING,
+ /* Push the button. */
+ TOP_BOTTLE_PUSHING,
+ /* Going back after a bottle has been pushed. */
+ TOP_BOTTLE_GOING_BACK,
+
/* Going to an unload position. */
TOP_UNLOAD_GOING,
/* Unloading, waiting for elements to fall. */
@@ -88,6 +97,16 @@ top_go_totem (void)
move_start (pos, 0);
}
+/** Go push a bottle button. */
+static void
+top_go_bottle (void)
+{
+ position_t pos;
+ pos.v = top.decision_pos;
+ pos.a = POSITION_A_DEG (90);
+ move_start (pos, 0);
+}
+
/** Go unload. */
static void
top_go_unload (void)
@@ -109,6 +128,9 @@ top_decision (void)
case STRAT_DECISION_TOTEM:
top_go_totem ();
break;
+ case STRAT_DECISION_BOTTLE:
+ top_go_bottle ();
+ break;
case STRAT_DECISION_UNLOAD:
top_go_unload ();
break;
@@ -124,6 +146,8 @@ top_decision (void)
default: assert (0); \
case STRAT_DECISION_TOTEM: \
return FSM_NEXT (state, event, totem); \
+ case STRAT_DECISION_BOTTLE: \
+ return FSM_NEXT (state, event, bottle); \
case STRAT_DECISION_UNLOAD: \
return FSM_NEXT (state, event, unload); \
} \
@@ -131,6 +155,7 @@ top_decision (void)
FSM_TRANS (TOP_START, init_start_round,
totem, TOP_TOTEM_GOING,
+ bottle, TOP_BOTTLE_GOING,
unload, TOP_UNLOAD_GOING)
{
strat_init ();
@@ -161,11 +186,42 @@ FSM_TRANS (TOP_TOTEM_PUSHING, robot_move_success, TOP_TOTEM_GOING_BACK)
FSM_TRANS (TOP_TOTEM_GOING_BACK, move_success,
totem, TOP_TOTEM_GOING,
+ bottle, TOP_BOTTLE_GOING,
unload, TOP_UNLOAD_GOING)
{
RETURN_TOP_DECISION_SWITCH (TOP_TOTEM_GOING_BACK, move_success);
}
+/** BOTTLE */
+
+FSM_TRANS (TOP_BOTTLE_GOING, move_success, TOP_BOTTLE_APPROACHING)
+{
+ asserv_move_linearly (-(BOT_SIZE_RADIUS + 70 - BOT_SIZE_BACK - 22 - 30));
+ return FSM_NEXT (TOP_BOTTLE_GOING, move_success);
+}
+
+FSM_TRANS (TOP_BOTTLE_APPROACHING, robot_move_success, TOP_BOTTLE_PUSHING)
+{
+ asserv_push_the_wall (ASSERV_BACKWARD, -1, -1, -1);
+ return FSM_NEXT (TOP_BOTTLE_APPROACHING, robot_move_success);
+}
+
+FSM_TRANS (TOP_BOTTLE_PUSHING, robot_move_success, TOP_BOTTLE_GOING_BACK)
+{
+ asserv_stop_motor ();
+ strat_success ();
+ move_start_noangle (top.decision_pos, 0, 0);
+ return FSM_NEXT (TOP_BOTTLE_PUSHING, robot_move_success);
+}
+
+FSM_TRANS (TOP_BOTTLE_GOING_BACK, move_success,
+ totem, TOP_TOTEM_GOING,
+ bottle, TOP_BOTTLE_GOING,
+ unload, TOP_UNLOAD_GOING)
+{
+ RETURN_TOP_DECISION_SWITCH (TOP_BOTTLE_GOING_BACK, move_success);
+}
+
/** UNLOAD */
FSM_TRANS (TOP_UNLOAD_GOING, move_success, TOP_UNLOADING)
@@ -177,6 +233,7 @@ FSM_TRANS (TOP_UNLOAD_GOING, move_success, TOP_UNLOADING)
FSM_TRANS_TIMEOUT (TOP_UNLOADING, 250,
totem, TOP_TOTEM_GOING,
+ bottle, TOP_BOTTLE_GOING,
unload, TOP_UNLOAD_GOING)
{
strat_success ();