From 6dc3c4baf9c37095b0042857cc771a04b9dd4593 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 9 May 2012 00:38:12 +0200 Subject: digital/io-hub/src/guybrush: handle cases when clamp is not ready --- digital/io-hub/src/guybrush/top.c | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) (limited to 'digital/io-hub/src/guybrush') 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) -- cgit v1.2.3