summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/guybrush/top.c
diff options
context:
space:
mode:
authorNicolas Schodet2012-05-14 15:56:22 +0200
committerNicolas Schodet2012-05-14 15:56:22 +0200
commit70020d019a3057a58251b713622a681f91a0cf5a (patch)
treee1420269545c34ff04ddba2277c2ae96d19b4528 /digital/io-hub/src/guybrush/top.c
parentd4e44fe78c67c47f8286175d31a6fa45b200a374 (diff)
digital/io-hub/src/guybrush: close door when the hold has been left
Diffstat (limited to 'digital/io-hub/src/guybrush/top.c')
-rw-r--r--digital/io-hub/src/guybrush/top.c42
1 files changed, 26 insertions, 16 deletions
diff --git a/digital/io-hub/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c
index 2197b0f3..ffd96767 100644
--- a/digital/io-hub/src/guybrush/top.c
+++ b/digital/io-hub/src/guybrush/top.c
@@ -96,9 +96,7 @@ FSM_STATES (
/* Going to an unload position. */
TOP_UNLOAD_GOING,
/* Unloading, waiting for elements to fall. */
- TOP_UNLOADING,
- /* Unloading, going back to playground. */
- TOP_UNLOADING_GOING_BACK)
+ TOP_UNLOADING)
FSM_START_WITH (TOP_START)
@@ -109,11 +107,30 @@ struct top_t
vect_t decision_pos;
/** Current distance to totem. */
int16_t totem_distance;
+ /** Close door when out of unloading zone. */
+ uint8_t close_door;
};
/** Global context. */
struct top_t top;
+void
+top_update (void)
+{
+ 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))
+ {
+ IO_CLR (OUTPUT_DOOR_OPEN);
+ IO_SET (OUTPUT_DOOR_CLOSE);
+ top.close_door = 0;
+ }
+ }
+}
+
/** Go collect a totem. */
static void
top_go_totem (void)
@@ -432,21 +449,14 @@ FSM_TRANS (TOP_UNLOAD_GOING, move_success, TOP_UNLOADING)
return FSM_NEXT (TOP_UNLOAD_GOING, move_success);
}
-FSM_TRANS_TIMEOUT (TOP_UNLOADING, 250, TOP_UNLOADING_GOING_BACK)
+FSM_TRANS_TIMEOUT (TOP_UNLOADING, 250,
+ totem, TOP_TOTEM_GOING,
+ bottle, TOP_BOTTLE_GOING,
+ unload, TOP_UNLOAD_GOING)
{
strat_success ();
- asserv_move_linearly (100);
- return FSM_NEXT_TIMEOUT (TOP_UNLOADING);
-}
-
-FSM_TRANS (TOP_UNLOADING_GOING_BACK, robot_move_success,
- totem, TOP_TOTEM_GOING,
- bottle, TOP_BOTTLE_GOING,
- unload, TOP_UNLOAD_GOING)
-{
- IO_CLR (OUTPUT_DOOR_OPEN);
- IO_SET (OUTPUT_DOOR_CLOSE);
- RETURN_TOP_DECISION_SWITCH (TOP_UNLOADING_GOING_BACK, robot_move_success);
+ top.close_door = 1;
+ RETURN_TOP_DECISION_SWITCH (TOP_UNLOADING, TOP_UNLOADING_TIMEOUT);
}
/** UNLOAD failures. */