summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/guybrush/top.c
diff options
context:
space:
mode:
authorNicolas Schodet2012-05-17 06:10:38 +0200
committerNicolas Schodet2012-05-17 06:10:38 +0200
commit20d29c23e7b6fc011636d5f979f4b7261f213359 (patch)
tree5e9c2048b6a7f99963406cb42a36da4597459f52 /digital/io-hub/src/guybrush/top.c
parent3ca89203c92272ca9f1de6e24cbcd894b4fefbc3 (diff)
digital/io-hub/src/guybrush: release goldbar when blocking
Diffstat (limited to 'digital/io-hub/src/guybrush/top.c')
-rw-r--r--digital/io-hub/src/guybrush/top.c21
1 files changed, 15 insertions, 6 deletions
diff --git a/digital/io-hub/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c
index 85015adb..8f80e174 100644
--- a/digital/io-hub/src/guybrush/top.c
+++ b/digital/io-hub/src/guybrush/top.c
@@ -84,6 +84,8 @@ FSM_STATES (
TOP_TOTEM_GOING_BACK,
/* Put clamps up. */
TOP_TOTEM_CLAMP_UPPING,
+ /* Release a goldbar blocking the robot. */
+ TOP_TOTEM_ERROR_RELEASE,
/* Going back after an error. */
TOP_TOTEM_ERROR_GOING_BACK,
@@ -333,12 +335,6 @@ FSM_TRANS (TOP_TOTEM_GOING_BACK, move_success, TOP_TOTEM_CLAMP_UPPING)
return FSM_NEXT (TOP_TOTEM_GOING_BACK, move_success);
}
-FSM_TRANS (TOP_TOTEM_GOING_BACK, move_failure, TOP_TOTEM_CLAMP_UPPING)
-{
- FSM_HANDLE (AI, robot_is_back);
- return FSM_NEXT (TOP_TOTEM_GOING_BACK, move_failure);
-}
-
FSM_TRANS (TOP_TOTEM_CLAMP_UPPING, clamps_ready, TOP_DECISION)
{
return FSM_NEXT (TOP_TOTEM_CLAMP_UPPING, clamps_ready);
@@ -368,6 +364,19 @@ FSM_TRANS (TOP_TOTEM_PUSHING, robot_move_failure,
return FSM_NEXT (TOP_TOTEM_PUSHING, robot_move_failure);
}
+FSM_TRANS (TOP_TOTEM_GOING_BACK, move_failure, TOP_TOTEM_ERROR_RELEASE)
+{
+ strat_failure ();
+ FSM_HANDLE (AI, stop_tree_approach);
+ return FSM_NEXT (TOP_TOTEM_GOING_BACK, move_failure);
+}
+
+FSM_TRANS (TOP_TOTEM_ERROR_RELEASE, clamps_ready, TOP_TOTEM_ERROR_GOING_BACK)
+{
+ move_start_noangle (top.decision_pos, ASSERV_BACKWARD, 0);
+ return FSM_NEXT (TOP_TOTEM_ERROR_RELEASE, clamps_ready);
+}
+
FSM_TRANS (TOP_TOTEM_ERROR_GOING_BACK, move_success, TOP_TOTEM_CLAMP_UPPING)
{
FSM_HANDLE (AI, stop_tree_approach);