summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2012-05-17 06:10:38 +0200
committerNicolas Schodet2012-05-17 06:10:38 +0200
commit20d29c23e7b6fc011636d5f979f4b7261f213359 (patch)
tree5e9c2048b6a7f99963406cb42a36da4597459f52
parent3ca89203c92272ca9f1de6e24cbcd894b4fefbc3 (diff)
digital/io-hub/src/guybrush: release goldbar when blocking
-rw-r--r--digital/io-hub/src/guybrush/bottom_clamp.c9
-rw-r--r--digital/io-hub/src/guybrush/top.c21
2 files changed, 24 insertions, 6 deletions
diff --git a/digital/io-hub/src/guybrush/bottom_clamp.c b/digital/io-hub/src/guybrush/bottom_clamp.c
index ca44c98b..dd821bae 100644
--- a/digital/io-hub/src/guybrush/bottom_clamp.c
+++ b/digital/io-hub/src/guybrush/bottom_clamp.c
@@ -593,6 +593,15 @@ FSM_TRANS(CLAMP_CLOSE_ALL_CLAMPS, robot_is_back,CLAMP_REARRANGE_CD)
}
+FSM_TRANS (CLAMP_CLOSE_ALL_CLAMPS, stop_tree_approach, CLAMP_READY_TO_EMPTY_TREE)
+{
+ IO_CLR (OUTPUT_LOWER_CLAMP_1_CLOSE);
+ IO_CLR (OUTPUT_LOWER_CLAMP_2_CLOSE);
+ IO_SET (OUTPUT_UPPER_CLAMP_OPEN);
+ fsm_queue_post_event (FSM_EVENT (AI, clamps_ready));
+ return FSM_NEXT (CLAMP_CLOSE_ALL_CLAMPS, stop_tree_approach);
+}
+
FSM_TRANS_TIMEOUT (CLAMP_REARRANGE_CD, TIMEOUT_RECENTRAGE, CLAMP_BOTTOM_CLAMP_HIDE_POS2)
{
/*Hidding the clamp inside the robot.*/
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);