summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--digital/io-hub/src/guybrush/top.c109
1 files changed, 109 insertions, 0 deletions
diff --git a/digital/io-hub/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c
index 1b5731eb..e6d156f6 100644
--- a/digital/io-hub/src/guybrush/top.c
+++ b/digital/io-hub/src/guybrush/top.c
@@ -67,6 +67,8 @@ FSM_STATES (
TOP_TOTEM_GOING_BACK,
/* Put clamps up. */
TOP_TOTEM_CLAMP_UPPING,
+ /* Going back after an error. */
+ TOP_TOTEM_ERROR_GOING_BACK,
/* Going to push a bottle. */
TOP_BOTTLE_GOING,
@@ -225,6 +227,12 @@ 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,
totem, TOP_TOTEM_GOING,
bottle, TOP_BOTTLE_GOING,
@@ -233,6 +241,45 @@ FSM_TRANS (TOP_TOTEM_CLAMP_UPPING, clamps_ready,
RETURN_TOP_DECISION_SWITCH (TOP_TOTEM_CLAMP_UPPING, clamps_ready);
}
+/** TOTEM failures. */
+
+FSM_TRANS (TOP_TOTEM_GOING, move_failure,
+ totem, TOP_TOTEM_GOING,
+ bottle, TOP_BOTTLE_GOING,
+ unload, TOP_UNLOAD_GOING)
+{
+ strat_failure ();
+ RETURN_TOP_DECISION_SWITCH (TOP_TOTEM_GOING, move_failure);
+}
+
+FSM_TRANS (TOP_TOTEM_APPROACHING, robot_move_failure,
+ TOP_TOTEM_ERROR_GOING_BACK)
+{
+ strat_failure ();
+ move_start_noangle (top.decision_pos, ASSERV_BACKWARD, 0);
+ return FSM_NEXT (TOP_TOTEM_APPROACHING, robot_move_failure);
+}
+
+FSM_TRANS (TOP_TOTEM_PUSHING, robot_move_failure,
+ TOP_TOTEM_ERROR_GOING_BACK)
+{
+ strat_failure ();
+ move_start_noangle (top.decision_pos, ASSERV_BACKWARD, 0);
+ return FSM_NEXT (TOP_TOTEM_PUSHING, robot_move_failure);
+}
+
+FSM_TRANS (TOP_TOTEM_ERROR_GOING_BACK, move_success, TOP_TOTEM_CLAMP_UPPING)
+{
+ FSM_HANDLE (AI, stop_tree_approach);
+ return FSM_NEXT (TOP_TOTEM_ERROR_GOING_BACK, move_success);
+}
+
+FSM_TRANS (TOP_TOTEM_ERROR_GOING_BACK, move_failure, TOP_TOTEM_CLAMP_UPPING)
+{
+ FSM_HANDLE (AI, stop_tree_approach);
+ return FSM_NEXT (TOP_TOTEM_ERROR_GOING_BACK, move_failure);
+}
+
/** BOTTLE */
FSM_TRANS (TOP_BOTTLE_GOING, move_success, TOP_BOTTLE_APPROACHING)
@@ -255,6 +302,15 @@ FSM_TRANS (TOP_BOTTLE_PUSHING, robot_move_success, TOP_BOTTLE_GOING_BACK)
return FSM_NEXT (TOP_BOTTLE_PUSHING, robot_move_success);
}
+FSM_TRANS (TOP_BOTTLE_PUSHING, robot_move_failure, TOP_BOTTLE_GOING_BACK)
+{
+ /* OK, ignore failure. */
+ asserv_stop_motor ();
+ strat_success ();
+ move_start_noangle (top.decision_pos, 0, 0);
+ return FSM_NEXT (TOP_BOTTLE_PUSHING, robot_move_failure);
+}
+
FSM_TRANS (TOP_BOTTLE_GOING_BACK, move_success,
totem, TOP_TOTEM_GOING,
bottle, TOP_BOTTLE_GOING,
@@ -263,6 +319,48 @@ FSM_TRANS (TOP_BOTTLE_GOING_BACK, move_success,
RETURN_TOP_DECISION_SWITCH (TOP_BOTTLE_GOING_BACK, move_success);
}
+FSM_TRANS (TOP_BOTTLE_GOING_BACK, move_failure,
+ totem, TOP_TOTEM_GOING,
+ bottle, TOP_BOTTLE_GOING,
+ unload, TOP_UNLOAD_GOING)
+{
+ /* Ignore and continue. */
+ RETURN_TOP_DECISION_SWITCH (TOP_BOTTLE_GOING_BACK, move_failure);
+}
+
+/** BOTTLE failures. */
+
+FSM_TRANS (TOP_BOTTLE_GOING, move_failure,
+ totem, TOP_TOTEM_GOING,
+ bottle, TOP_BOTTLE_GOING,
+ unload, TOP_UNLOAD_GOING)
+{
+ strat_failure ();
+ RETURN_TOP_DECISION_SWITCH (TOP_BOTTLE_GOING, move_failure);
+}
+
+FSM_TRANS (TOP_BOTTLE_APPROACHING, robot_move_failure,
+ try_anyway, TOP_BOTTLE_PUSHING,
+ totem, TOP_TOTEM_GOING,
+ bottle, TOP_BOTTLE_GOING,
+ unload, TOP_UNLOAD_GOING)
+{
+ position_t robot_pos;
+ asserv_get_position (&robot_pos);
+ /* Try to continue anyway? */
+ if (robot_pos.v.y < BOT_SIZE_BACK + 100)
+ {
+ asserv_push_the_wall (ASSERV_BACKWARD, -1, -1, -1);
+ return FSM_NEXT (TOP_BOTTLE_APPROACHING, robot_move_failure,
+ try_anyway);
+ }
+ else
+ {
+ strat_failure ();
+ RETURN_TOP_DECISION_SWITCH (TOP_BOTTLE_APPROACHING, robot_move_failure);
+ }
+}
+
/** UNLOAD */
FSM_TRANS (TOP_UNLOAD_GOING, move_success, TOP_UNLOADING)
@@ -283,3 +381,14 @@ FSM_TRANS_TIMEOUT (TOP_UNLOADING, 250,
RETURN_TOP_DECISION_SWITCH (TOP_UNLOADING, TOP_UNLOADING_TIMEOUT);
}
+/** UNLOAD failures. */
+
+FSM_TRANS (TOP_UNLOAD_GOING, move_failure,
+ totem, TOP_TOTEM_GOING,
+ bottle, TOP_BOTTLE_GOING,
+ unload, TOP_UNLOAD_GOING)
+{
+ strat_failure ();
+ RETURN_TOP_DECISION_SWITCH (TOP_UNLOAD_GOING, move_failure);
+}
+