summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/guybrush/top.c
diff options
context:
space:
mode:
authorNicolas Schodet2012-05-09 00:38:12 +0200
committerNicolas Schodet2012-05-09 00:40:35 +0200
commit6dc3c4baf9c37095b0042857cc771a04b9dd4593 (patch)
tree476e1f0beedc4261b1b561717e5efef82a8a8cd1 /digital/io-hub/src/guybrush/top.c
parent826d0f9b1b2c1ef5b055fed0dd1734eb383a9384 (diff)
digital/io-hub/src/guybrush: handle cases when clamp is not ready
Diffstat (limited to 'digital/io-hub/src/guybrush/top.c')
-rw-r--r--digital/io-hub/src/guybrush/top.c21
1 files changed, 19 insertions, 2 deletions
diff --git a/digital/io-hub/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c
index 01b227b1..1b5731eb 100644
--- a/digital/io-hub/src/guybrush/top.c
+++ b/digital/io-hub/src/guybrush/top.c
@@ -53,6 +53,8 @@ FSM_STATES (
/* Going to a collect position above or below a totem. */
TOP_TOTEM_GOING,
+ /* Waiting clamp FSM to put clamps down. */
+ TOP_TOTEM_CLAMP_WAITING,
/* Put clamps down. */
TOP_TOTEM_CLAMP_DOWNING,
/* Approaching a totem. */
@@ -170,10 +172,25 @@ FSM_TRANS (TOP_START, init_start_round,
/** TOTEM */
-FSM_TRANS (TOP_TOTEM_GOING, move_success, TOP_TOTEM_CLAMP_DOWNING)
+FSM_TRANS (TOP_TOTEM_GOING, move_success,
+ wait, TOP_TOTEM_CLAMP_WAITING,
+ ready, TOP_TOTEM_CLAMP_DOWNING)
+{
+ if (!FSM_CAN_HANDLE (AI, tree_detected))
+ {
+ return FSM_NEXT (TOP_TOTEM_GOING, move_success, wait);
+ }
+ else
+ {
+ FSM_HANDLE (AI, tree_detected);
+ return FSM_NEXT (TOP_TOTEM_GOING, move_success, ready);
+ }
+}
+
+FSM_TRANS (TOP_TOTEM_CLAMP_WAITING, clamps_ready, TOP_TOTEM_CLAMP_DOWNING)
{
FSM_HANDLE (AI, tree_detected);
- return FSM_NEXT (TOP_TOTEM_GOING, move_success);
+ return FSM_NEXT (TOP_TOTEM_CLAMP_WAITING, clamps_ready);
}
FSM_TRANS (TOP_TOTEM_CLAMP_DOWNING, clamps_ready, TOP_TOTEM_APPROACHING)