summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/guybrush/top.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io-hub/src/guybrush/top.c')
-rw-r--r--digital/io-hub/src/guybrush/top.c377
1 files changed, 301 insertions, 76 deletions
diff --git a/digital/io-hub/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c
index 869721b3..2bc6c8e1 100644
--- a/digital/io-hub/src/guybrush/top.c
+++ b/digital/io-hub/src/guybrush/top.c
@@ -40,6 +40,7 @@
#include "strat.h"
#include "path.h"
#include "bottom_clamp.h"
+#include "radar_defs.h"
#include "output_defs.h"
@@ -48,6 +49,9 @@
* impression of intelligence... Well...
*/
+/** Number of time to try to unblock clamp. */
+#define TOP_CLAMP_BLOCKED_THRESHOLD 5
+
FSM_INIT
FSM_STATES (
@@ -57,6 +61,12 @@ FSM_STATES (
TOP_INIT_DOOR,
/* Init done, waiting for rount start. */
TOP_INIT,
+ /* Transition state to take a new decision. */
+ TOP_DECISION,
+ /* Move to unblock clamp. */
+ TOP_CLAMP_ERROR_MOVING_DECISION,
+ /* Unblock clamp and take a new decision. */
+ TOP_CLAMP_ERROR_DECISION,
/* Going to a collect position above or below a totem. */
TOP_TOTEM_GOING,
@@ -82,8 +92,12 @@ 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,
+ /* Going back after a clamp error. */
+ TOP_TOTEM_CLAMP_ERROR_GOING_BACK,
/* Going to push a bottle. */
TOP_BOTTLE_GOING,
@@ -96,8 +110,8 @@ FSM_STATES (
/* Going to an unload position. */
TOP_UNLOAD_GOING,
- /* Unloading, waiting for elements to fall. */
- TOP_UNLOADING)
+ /* Going back in unload zone. */
+ TOP_UNLOAD_GOING_BACK)
FSM_START_WITH (TOP_START)
@@ -110,6 +124,12 @@ struct top_t
int16_t totem_distance;
/** Close door when out of unloading zone. */
uint8_t close_door;
+ /** Check door is closed once this reach 0. */
+ uint8_t close_door_check;
+ /** Close the door once this reach 0. */
+ uint8_t close_door_wait;
+ /** Chaos factor. */
+ uint8_t chaos;
};
/** Global context. */
@@ -118,20 +138,70 @@ struct top_t top;
void
top_update (void)
{
+ /* Close door when out of unloading zone. */
if (top.close_door)
{
position_t robot_pos;
asserv_get_position (&robot_pos);
- if (robot_pos.v.x > PG_HOLD_NORTH_X + BOT_SIZE_BACK
- && robot_pos.v.x < PG_MIRROR_X (PG_HOLD_NORTH_X + BOT_SIZE_BACK))
+ int16_t limit = robot_pos.v.y < PG_LENGTH - PG_CAPTAIN_ROOM_LENGTH_MM
+ ? PG_HOLD_NORTH_X + BOT_SIZE_BACK
+ : PG_CAPTAIN_ROOM_LENGTH_MM + BOT_SIZE_BACK;
+ if (robot_pos.v.x > limit && robot_pos.v.x < PG_MIRROR_X (limit))
{
IO_CLR (OUTPUT_DOOR_OPEN);
IO_SET (OUTPUT_DOOR_CLOSE);
top.close_door = 0;
+ top.close_door_check = 250;
+ }
+ }
+ /* If door is closed, cut air to save power. */
+ if (!IO_GET (CONTACT_DOOR_CLOSE))
+ {
+ IO_CLR (OUTPUT_DOOR_CLOSE);
+ }
+ /* Test if it should be closed. */
+ if (top.close_door_check)
+ {
+ top.close_door_check--;
+ if (!top.close_door_check && IO_GET (CONTACT_DOOR_CLOSE))
+ {
+ IO_SET (OUTPUT_DOOR_OPEN);
+ IO_CLR (OUTPUT_DOOR_CLOSE);
+ top.close_door_wait = 250;
+ }
+ }
+ /* Close door after a delay. */
+ if (top.close_door_wait)
+ {
+ top.close_door_wait--;
+ if (!top.close_door_wait)
+ {
+ IO_CLR (OUTPUT_DOOR_OPEN);
+ IO_SET (OUTPUT_DOOR_CLOSE);
}
}
}
+/** Move in a surprising way to unblock clamp. */
+static void
+top_chaos_move (void)
+{
+ if ((top.chaos % 4) == 3)
+ {
+ int16_t angle = (top.chaos % 8) == 3 ? POSITION_A_DEG (3) :
+ -POSITION_A_DEG (3);
+ asserv_move_angularly (angle);
+ }
+ else
+ {
+ int16_t dist = ((top.chaos + 3) % 8 + 1) * 10;
+ if (!(top.chaos % 2))
+ dist = -dist;
+ asserv_move_linearly (dist);
+ }
+ top.chaos++;
+}
+
/** Go collect a totem. */
static void
top_go_totem (void)
@@ -161,7 +231,10 @@ top_go_unload (void)
{
position_t pos;
pos.v = top.decision_pos;
- pos.a = PG_A_DEG (70);
+ if (pos.v.y > PG_LENGTH - PG_CAPTAIN_ROOM_LENGTH_MM)
+ pos.a = PG_A_DEG (0);
+ else
+ pos.a = PG_A_DEG (45);
move_start (pos, 0);
}
@@ -189,19 +262,6 @@ top_decision (void)
return decision;
}
-#define RETURN_TOP_DECISION_SWITCH(state, event) do { \
- switch (top_decision ()) \
- { \
- default: assert (0); \
- case STRAT_DECISION_TOTEM: \
- return FSM_NEXT (state, event, totem); \
- case STRAT_DECISION_BOTTLE: \
- return FSM_NEXT (state, event, bottle); \
- case STRAT_DECISION_UNLOAD: \
- return FSM_NEXT (state, event, unload); \
- } \
-} while (0)
-
FSM_TRANS (TOP_START, init_actuators, TOP_INIT_DOOR)
{
main_demo = !IO_GET (CONTACT_STRAT);
@@ -214,30 +274,98 @@ FSM_TRANS (TOP_INIT_DOOR, init_done, TOP_INIT)
{
IO_CLR (OUTPUT_DOOR_OPEN);
IO_SET (OUTPUT_DOOR_CLOSE);
+ strat_init ();
+ strat_prepare ();
return FSM_NEXT (TOP_INIT_DOOR, init_done);
}
-FSM_TRANS (TOP_INIT, init_start_round,
- totem, TOP_TOTEM_GOING,
- bottle, TOP_BOTTLE_GOING,
- unload, TOP_UNLOAD_GOING)
+FSM_TRANS (TOP_INIT, init_start_round, TOP_DECISION)
{
- strat_init ();
main_usdist = 1;
- RETURN_TOP_DECISION_SWITCH (TOP_INIT, init_start_round);
+ return FSM_NEXT (TOP_INIT, init_start_round);
+}
+
+FSM_TRANS_TIMEOUT (TOP_DECISION, 1,
+ clamp_blocked, TOP_CLAMP_ERROR_MOVING_DECISION,
+ totem, TOP_TOTEM_GOING,
+ bottle, TOP_BOTTLE_GOING,
+ unload, TOP_UNLOAD_GOING)
+{
+ radar_def_upper_clamp_moving (0);
+ if (FSM_CAN_HANDLE (AI, clamp_unblock))
+ {
+ top_chaos_move ();
+ return FSM_NEXT_TIMEOUT (TOP_DECISION, clamp_blocked);
+ }
+ switch (top_decision ())
+ {
+ default:
+ assert (0);
+ case STRAT_DECISION_TOTEM:
+ return FSM_NEXT_TIMEOUT (TOP_DECISION, totem);
+ case STRAT_DECISION_BOTTLE:
+ return FSM_NEXT_TIMEOUT (TOP_DECISION, bottle);
+ case STRAT_DECISION_UNLOAD:
+ return FSM_NEXT_TIMEOUT (TOP_DECISION, unload);
+ }
+}
+
+FSM_TRANS (TOP_CLAMP_ERROR_MOVING_DECISION, robot_move_success,
+ TOP_CLAMP_ERROR_DECISION)
+{
+ radar_def_upper_clamp_moving (0);
+ FSM_HANDLE (AI, clamp_unblock);
+ return FSM_NEXT (TOP_CLAMP_ERROR_MOVING_DECISION, robot_move_success);
+}
+
+FSM_TRANS (TOP_CLAMP_ERROR_MOVING_DECISION, robot_move_failure,
+ TOP_CLAMP_ERROR_DECISION)
+{
+ FSM_HANDLE (AI, clamp_unblock);
+ return FSM_NEXT (TOP_CLAMP_ERROR_MOVING_DECISION, robot_move_failure);
+}
+
+FSM_TRANS (TOP_CLAMP_ERROR_DECISION, clamps_ready, TOP_DECISION)
+{
+ return FSM_NEXT (TOP_CLAMP_ERROR_DECISION, clamps_ready);
+}
+
+FSM_TRANS (TOP_CLAMP_ERROR_DECISION, clamp_blocked,
+ unblock, TOP_CLAMP_ERROR_DECISION,
+ dead, TOP_DECISION)
+{
+ if (clamp_read_blocked_cpt () < TOP_CLAMP_BLOCKED_THRESHOLD)
+ {
+ FSM_HANDLE (AI, clamp_unblock);
+ return FSM_NEXT (TOP_CLAMP_ERROR_DECISION, clamp_blocked, unblock);
+ }
+ else
+ {
+ strat_clamp_dead ();
+ strat_bad_failure ();
+ FSM_HANDLE (AI, clamp_is_dead);
+ return FSM_NEXT (TOP_CLAMP_ERROR_DECISION, clamp_blocked, dead);
+ }
+}
+
+FSM_TRANS (TOP_CLAMP_ERROR_DECISION, upper_set_is_dead, TOP_DECISION)
+{
+ strat_upper_clamp_dead ();
+ return FSM_NEXT (TOP_CLAMP_ERROR_DECISION, upper_set_is_dead);
}
/** TOTEM */
FSM_TRANS (TOP_TOTEM_GOING, move_success, TOP_TOTEM_CLEAN_STARTING)
{
+ radar_def_upper_clamp_moving (1);
clamp_request (FSM_EVENT (AI, clean_start));
return FSM_NEXT (TOP_TOTEM_GOING, move_success);
}
FSM_TRANS (TOP_TOTEM_CLEAN_STARTING, clamps_ready, TOP_TOTEM_CLEAN_APPROACHING)
{
- int16_t move = top.totem_distance - BOT_SIZE_LOWER_CLAMP_FRONT - 140;
+ int16_t move = top.totem_distance - BOT_SIZE_LOWER_CLAMP_FRONT - 135;
top.totem_distance -= move;
asserv_set_speed (BOT_SPEED_APPROACH);
asserv_move_linearly (move);
@@ -251,10 +379,18 @@ FSM_TRANS (TOP_TOTEM_CLEAN_APPROACHING, robot_move_success,
return FSM_NEXT (TOP_TOTEM_CLEAN_APPROACHING, robot_move_success);
}
+FSM_TRANS (TOP_TOTEM_CLEAN_APPROACHING, robot_move_failure,
+ TOP_TOTEM_CLEAN_CATCH_WAITING)
+{
+ /* Must be blocked on a CD, catch and continue. */
+ FSM_HANDLE (AI, clean_catch);
+ return FSM_NEXT (TOP_TOTEM_CLEAN_APPROACHING, robot_move_failure);
+}
+
FSM_TRANS (TOP_TOTEM_CLEAN_CATCH_WAITING, clamps_ready,
TOP_TOTEM_CLEAN_GOING_BACK)
{
- int16_t move = top.totem_distance - BOT_SIZE_LOWER_CLAMP_FRONT - 240;
+ int16_t move = top.totem_distance - BOT_SIZE_LOADING_FRONT - 30;
top.totem_distance -= move;
asserv_move_linearly (move);
return FSM_NEXT (TOP_TOTEM_CLEAN_CATCH_WAITING, clamps_ready);
@@ -267,6 +403,13 @@ FSM_TRANS (TOP_TOTEM_CLEAN_GOING_BACK, robot_move_success,
return FSM_NEXT (TOP_TOTEM_CLEAN_GOING_BACK, robot_move_success);
}
+FSM_TRANS (TOP_TOTEM_CLEAN_GOING_BACK, robot_move_failure,
+ TOP_TOTEM_CLEAN_LOADING)
+{
+ FSM_HANDLE (AI, clean_load);
+ return FSM_NEXT (TOP_TOTEM_CLEAN_GOING_BACK, robot_move_failure);
+}
+
FSM_TRANS (TOP_TOTEM_CLEAN_LOADING, clamps_ready, TOP_TOTEM_CLAMP_DOWNING)
{
clamp_request (FSM_EVENT (AI, tree_detected));
@@ -296,46 +439,78 @@ FSM_TRANS (TOP_TOTEM_PUSHING, robot_move_success, TOP_TOTEM_EMPTYING)
FSM_TRANS (TOP_TOTEM_EMPTYING, clamps_ready, TOP_TOTEM_GOING_BACK)
{
- strat_success ();
move_start_noangle (top.decision_pos, ASSERV_BACKWARD, 0);
return FSM_NEXT (TOP_TOTEM_EMPTYING, clamps_ready);
}
FSM_TRANS (TOP_TOTEM_GOING_BACK, move_success, TOP_TOTEM_CLAMP_UPPING)
{
+ strat_success ();
FSM_HANDLE (AI, robot_is_back);
return FSM_NEXT (TOP_TOTEM_GOING_BACK, move_success);
}
-FSM_TRANS (TOP_TOTEM_GOING_BACK, move_failure, TOP_TOTEM_CLAMP_UPPING)
+FSM_TRANS (TOP_TOTEM_CLAMP_UPPING, clamps_ready, TOP_DECISION)
{
- FSM_HANDLE (AI, robot_is_back);
- return FSM_NEXT (TOP_TOTEM_GOING_BACK, move_failure);
+ return FSM_NEXT (TOP_TOTEM_CLAMP_UPPING, clamps_ready);
}
-FSM_TRANS (TOP_TOTEM_CLAMP_UPPING, clamps_ready,
- totem, TOP_TOTEM_GOING,
- bottle, TOP_BOTTLE_GOING,
- unload, TOP_UNLOAD_GOING)
+/** TOTEM failures. */
+
+FSM_TRANS (TOP_TOTEM_GOING, move_failure, TOP_DECISION)
{
- RETURN_TOP_DECISION_SWITCH (TOP_TOTEM_CLAMP_UPPING, clamps_ready);
+ strat_failure ();
+ return FSM_NEXT (TOP_TOTEM_GOING, move_failure);
}
-/** TOTEM failures. */
+FSM_TRANS (TOP_TOTEM_CLEAN_STARTING, clamp_blocked,
+ TOP_CLAMP_ERROR_MOVING_DECISION)
+{
+ strat_failure ();
+ top_chaos_move ();
+ return FSM_NEXT (TOP_TOTEM_CLEAN_STARTING, clamp_blocked);
+}
+
+FSM_TRANS_TIMEOUT (TOP_TOTEM_CLEAN_STARTING, 5 * 250, TOP_DECISION)
+{
+ strat_bad_failure ();
+ return FSM_NEXT_TIMEOUT (TOP_TOTEM_CLEAN_STARTING);
+}
-FSM_TRANS (TOP_TOTEM_GOING, move_failure,
- totem, TOP_TOTEM_GOING,
- bottle, TOP_BOTTLE_GOING,
- unload, TOP_UNLOAD_GOING)
+FSM_TRANS (TOP_TOTEM_CLEAN_CATCH_WAITING, clamp_blocked,
+ TOP_TOTEM_CLAMP_ERROR_GOING_BACK)
{
strat_failure ();
- RETURN_TOP_DECISION_SWITCH (TOP_TOTEM_GOING, move_failure);
+ move_start_noangle (top.decision_pos, ASSERV_BACKWARD, 0);
+ return FSM_NEXT (TOP_TOTEM_CLEAN_CATCH_WAITING, clamp_blocked);
+}
+
+FSM_TRANS (TOP_TOTEM_CLEAN_LOADING, clamp_blocked,
+ TOP_CLAMP_ERROR_MOVING_DECISION)
+{
+ strat_failure ();
+ top_chaos_move ();
+ return FSM_NEXT (TOP_TOTEM_CLEAN_LOADING, clamp_blocked);
+}
+
+FSM_TRANS (TOP_TOTEM_CLAMP_DOWNING, clamp_blocked,
+ TOP_CLAMP_ERROR_MOVING_DECISION)
+{
+ strat_failure ();
+ top_chaos_move ();
+ return FSM_NEXT (TOP_TOTEM_CLAMP_DOWNING, clamp_blocked);
+}
+
+FSM_TRANS_TIMEOUT (TOP_TOTEM_CLAMP_DOWNING, 5 * 250, TOP_DECISION)
+{
+ strat_bad_failure ();
+ return FSM_NEXT_TIMEOUT (TOP_TOTEM_CLAMP_DOWNING);
}
FSM_TRANS (TOP_TOTEM_APPROACHING, robot_move_failure,
TOP_TOTEM_ERROR_GOING_BACK)
{
- strat_failure ();
+ strat_giveup ();
move_start_noangle (top.decision_pos, ASSERV_BACKWARD, 0);
return FSM_NEXT (TOP_TOTEM_APPROACHING, robot_move_failure);
}
@@ -343,11 +518,40 @@ FSM_TRANS (TOP_TOTEM_APPROACHING, robot_move_failure,
FSM_TRANS (TOP_TOTEM_PUSHING, robot_move_failure,
TOP_TOTEM_ERROR_GOING_BACK)
{
- strat_failure ();
+ strat_giveup ();
move_start_noangle (top.decision_pos, ASSERV_BACKWARD, 0);
return FSM_NEXT (TOP_TOTEM_PUSHING, robot_move_failure);
}
+FSM_TRANS (TOP_TOTEM_EMPTYING, clamp_blocked,
+ TOP_TOTEM_CLAMP_ERROR_GOING_BACK)
+{
+ strat_giveup ();
+ move_start_noangle (top.decision_pos, ASSERV_BACKWARD, 0);
+ return FSM_NEXT (TOP_TOTEM_EMPTYING, clamp_blocked);
+}
+
+FSM_TRANS (TOP_TOTEM_GOING_BACK, move_failure, TOP_TOTEM_ERROR_RELEASE)
+{
+ strat_giveup ();
+ FSM_HANDLE (AI, stop_tree_approach);
+ return FSM_NEXT (TOP_TOTEM_GOING_BACK, move_failure);
+}
+
+FSM_TRANS (TOP_TOTEM_CLAMP_UPPING, clamp_blocked,
+ TOP_CLAMP_ERROR_MOVING_DECISION)
+{
+ strat_giveup ();
+ top_chaos_move ();
+ return FSM_NEXT (TOP_TOTEM_CLAMP_UPPING, clamp_blocked);
+}
+
+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);
@@ -360,6 +564,20 @@ FSM_TRANS (TOP_TOTEM_ERROR_GOING_BACK, move_failure, TOP_TOTEM_CLAMP_UPPING)
return FSM_NEXT (TOP_TOTEM_ERROR_GOING_BACK, move_failure);
}
+FSM_TRANS (TOP_TOTEM_CLAMP_ERROR_GOING_BACK, move_success,
+ TOP_CLAMP_ERROR_MOVING_DECISION)
+{
+ top_chaos_move ();
+ return FSM_NEXT (TOP_TOTEM_CLAMP_ERROR_GOING_BACK, move_success);
+}
+
+FSM_TRANS (TOP_TOTEM_CLAMP_ERROR_GOING_BACK, move_failure,
+ TOP_CLAMP_ERROR_MOVING_DECISION)
+{
+ top_chaos_move ();
+ return FSM_NEXT (TOP_TOTEM_CLAMP_ERROR_GOING_BACK, move_failure);
+}
+
/** BOTTLE */
FSM_TRANS (TOP_BOTTLE_GOING, move_success, TOP_BOTTLE_APPROACHING)
@@ -393,39 +611,28 @@ FSM_TRANS (TOP_BOTTLE_PUSHING, robot_move_failure, TOP_BOTTLE_GOING_BACK)
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,
- unload, TOP_UNLOAD_GOING)
+FSM_TRANS (TOP_BOTTLE_GOING_BACK, move_success, TOP_DECISION)
{
- RETURN_TOP_DECISION_SWITCH (TOP_BOTTLE_GOING_BACK, move_success);
+ return FSM_NEXT (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)
+FSM_TRANS (TOP_BOTTLE_GOING_BACK, move_failure, TOP_DECISION)
{
/* Ignore and continue. */
- RETURN_TOP_DECISION_SWITCH (TOP_BOTTLE_GOING_BACK, move_failure);
+ return FSM_NEXT (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)
+FSM_TRANS (TOP_BOTTLE_GOING, move_failure, TOP_DECISION)
{
strat_failure ();
- RETURN_TOP_DECISION_SWITCH (TOP_BOTTLE_GOING, move_failure);
+ return FSM_NEXT (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)
+ decision, TOP_DECISION)
{
position_t robot_pos;
asserv_get_position (&robot_pos);
@@ -439,37 +646,55 @@ FSM_TRANS (TOP_BOTTLE_APPROACHING, robot_move_failure,
else
{
strat_failure ();
- RETURN_TOP_DECISION_SWITCH (TOP_BOTTLE_APPROACHING, robot_move_failure);
+ return FSM_NEXT (TOP_BOTTLE_APPROACHING, robot_move_failure,
+ decision);
}
}
/** UNLOAD */
-FSM_TRANS (TOP_UNLOAD_GOING, move_success, TOP_UNLOADING)
+static void
+top_do_unload (void)
{
IO_SET (OUTPUT_DOOR_OPEN);
IO_CLR (OUTPUT_DOOR_CLOSE);
- return FSM_NEXT (TOP_UNLOAD_GOING, move_success);
+ strat_success ();
+ top.close_door = 1;
}
-FSM_TRANS_TIMEOUT (TOP_UNLOADING, 250,
- totem, TOP_TOTEM_GOING,
- bottle, TOP_BOTTLE_GOING,
- unload, TOP_UNLOAD_GOING)
+FSM_TRANS (TOP_UNLOAD_GOING, move_success,
+ hold, TOP_UNLOAD_GOING_BACK,
+ captain, TOP_UNLOAD_GOING_BACK)
{
- strat_success ();
- top.close_door = 1;
- RETURN_TOP_DECISION_SWITCH (TOP_UNLOADING, TOP_UNLOADING_TIMEOUT);
+ if (top.decision_pos.y < PG_LENGTH - PG_CAPTAIN_ROOM_LENGTH_MM)
+ {
+ asserv_move_linearly (-100);
+ 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. */
-FSM_TRANS (TOP_UNLOAD_GOING, move_failure,
- totem, TOP_TOTEM_GOING,
- bottle, TOP_BOTTLE_GOING,
- unload, TOP_UNLOAD_GOING)
+FSM_TRANS (TOP_UNLOAD_GOING, move_failure, TOP_DECISION)
{
strat_failure ();
- RETURN_TOP_DECISION_SWITCH (TOP_UNLOAD_GOING, move_failure);
+ return FSM_NEXT (TOP_UNLOAD_GOING, move_failure);
}