From 1d00b79b1e179cf947d915ca2033ce690ed5b7a5 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 16 May 2012 20:55:22 +0200 Subject: digital/io-hub/src/guybrush: handle more blocking --- digital/io-hub/src/guybrush/top.c | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/digital/io-hub/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c index 187edcab..540d0385 100644 --- a/digital/io-hub/src/guybrush/top.c +++ b/digital/io-hub/src/guybrush/top.c @@ -258,6 +258,14 @@ 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) { @@ -274,6 +282,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)); -- cgit v1.2.3