summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--digital/io-hub/src/guybrush/top.c13
1 files changed, 10 insertions, 3 deletions
diff --git a/digital/io-hub/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c
index 2bc6c8e1..10f65c7f 100644
--- a/digital/io-hub/src/guybrush/top.c
+++ b/digital/io-hub/src/guybrush/top.c
@@ -111,7 +111,9 @@ FSM_STATES (
/* Going to an unload position. */
TOP_UNLOAD_GOING,
/* Going back in unload zone. */
- TOP_UNLOAD_GOING_BACK)
+ TOP_UNLOAD_GOING_BACK,
+ /* Unloading, waiting for elements to fall. */
+ TOP_UNLOADING)
FSM_START_WITH (TOP_START)
@@ -678,18 +680,23 @@ FSM_TRANS (TOP_UNLOAD_GOING, move_success,
}
}
-FSM_TRANS (TOP_UNLOAD_GOING_BACK, robot_move_success, TOP_DECISION)
+FSM_TRANS (TOP_UNLOAD_GOING_BACK, robot_move_success, TOP_UNLOADING)
{
top_do_unload ();
return FSM_NEXT (TOP_UNLOAD_GOING_BACK, robot_move_success);
}
-FSM_TRANS (TOP_UNLOAD_GOING_BACK, robot_move_failure, TOP_DECISION)
+FSM_TRANS (TOP_UNLOAD_GOING_BACK, robot_move_failure, TOP_UNLOADING)
{
top_do_unload ();
return FSM_NEXT (TOP_UNLOAD_GOING_BACK, robot_move_failure);
}
+FSM_TRANS_TIMEOUT (TOP_UNLOADING, 250, TOP_DECISION)
+{
+ return FSM_NEXT_TIMEOUT (TOP_UNLOADING);
+}
+
/** UNLOAD failures. */
FSM_TRANS (TOP_UNLOAD_GOING, move_failure, TOP_DECISION)