summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/guybrush/top.c
diff options
context:
space:
mode:
authorNicolas Schodet2012-05-19 06:41:23 +0200
committerNicolas Schodet2012-05-19 06:41:23 +0200
commit8cdb95cae92096856b3ce83ff039db0a4e33a927 (patch)
tree8d9c7abe55fe9b8bedea040fba6fd7c77e31a9c7 /digital/io-hub/src/guybrush/top.c
parent3d499f7f209ca7b69e9cc5c893358ff76c3c2215 (diff)
digital/io-hub/src/guybrush: go back in captain room
Diffstat (limited to 'digital/io-hub/src/guybrush/top.c')
-rw-r--r--digital/io-hub/src/guybrush/top.c36
1 files changed, 33 insertions, 3 deletions
diff --git a/digital/io-hub/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c
index 8bda0462..6655afd9 100644
--- a/digital/io-hub/src/guybrush/top.c
+++ b/digital/io-hub/src/guybrush/top.c
@@ -108,7 +108,9 @@ FSM_STATES (
TOP_BOTTLE_GOING_BACK,
/* Going to an unload position. */
- TOP_UNLOAD_GOING)
+ TOP_UNLOAD_GOING,
+ /* Going back in unload zone. */
+ TOP_UNLOAD_GOING_BACK)
FSM_START_WITH (TOP_START)
@@ -647,13 +649,41 @@ FSM_TRANS (TOP_BOTTLE_APPROACHING, robot_move_failure,
/** UNLOAD */
-FSM_TRANS (TOP_UNLOAD_GOING, move_success, TOP_DECISION)
+static void
+top_do_unload (void)
{
IO_SET (OUTPUT_DOOR_OPEN);
IO_CLR (OUTPUT_DOOR_CLOSE);
strat_success ();
top.close_door = 1;
- return FSM_NEXT (TOP_UNLOAD_GOING, move_success);
+}
+
+FSM_TRANS (TOP_UNLOAD_GOING, move_success,
+ hold, TOP_DECISION,
+ captain, TOP_UNLOAD_GOING_BACK)
+{
+ if (top.decision_pos.y < PG_LENGTH - PG_CAPTAIN_ROOM_LENGTH_MM)
+ {
+ top_do_unload ();
+ return FSM_NEXT (TOP_UNLOAD_GOING, move_success, hold);
+ }
+ else
+ {
+ asserv_move_linearly (-50);
+ return FSM_NEXT (TOP_UNLOAD_GOING, move_success, captain);
+ }
+}
+
+FSM_TRANS (TOP_UNLOAD_GOING_BACK, robot_move_success, TOP_DECISION)
+{
+ top_do_unload ();
+ return FSM_NEXT (TOP_UNLOAD_GOING_BACK, robot_move_success);
+}
+
+FSM_TRANS (TOP_UNLOAD_GOING_BACK, robot_move_failure, TOP_DECISION)
+{
+ top_do_unload ();
+ return FSM_NEXT (TOP_UNLOAD_GOING_BACK, robot_move_failure);
}
/** UNLOAD failures. */