From 340e50f8b31115b117156576c6204c8d6c2dfe56 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 12 May 2012 17:19:59 +0200 Subject: digital/io-hub/src/guybrush: handle movement failures --- digital/io-hub/src/guybrush/top.c | 109 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 109 insertions(+) diff --git a/digital/io-hub/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c index 1b5731eb..e6d156f6 100644 --- a/digital/io-hub/src/guybrush/top.c +++ b/digital/io-hub/src/guybrush/top.c @@ -67,6 +67,8 @@ FSM_STATES ( TOP_TOTEM_GOING_BACK, /* Put clamps up. */ TOP_TOTEM_CLAMP_UPPING, + /* Going back after an error. */ + TOP_TOTEM_ERROR_GOING_BACK, /* Going to push a bottle. */ TOP_BOTTLE_GOING, @@ -225,6 +227,12 @@ FSM_TRANS (TOP_TOTEM_GOING_BACK, move_success, TOP_TOTEM_CLAMP_UPPING) return FSM_NEXT (TOP_TOTEM_GOING_BACK, move_success); } +FSM_TRANS (TOP_TOTEM_GOING_BACK, move_failure, TOP_TOTEM_CLAMP_UPPING) +{ + FSM_HANDLE (AI, robot_is_back); + return FSM_NEXT (TOP_TOTEM_GOING_BACK, move_failure); +} + FSM_TRANS (TOP_TOTEM_CLAMP_UPPING, clamps_ready, totem, TOP_TOTEM_GOING, bottle, TOP_BOTTLE_GOING, @@ -233,6 +241,45 @@ FSM_TRANS (TOP_TOTEM_CLAMP_UPPING, clamps_ready, RETURN_TOP_DECISION_SWITCH (TOP_TOTEM_CLAMP_UPPING, clamps_ready); } +/** TOTEM failures. */ + +FSM_TRANS (TOP_TOTEM_GOING, move_failure, + totem, TOP_TOTEM_GOING, + bottle, TOP_BOTTLE_GOING, + unload, TOP_UNLOAD_GOING) +{ + strat_failure (); + RETURN_TOP_DECISION_SWITCH (TOP_TOTEM_GOING, move_failure); +} + +FSM_TRANS (TOP_TOTEM_APPROACHING, robot_move_failure, + TOP_TOTEM_ERROR_GOING_BACK) +{ + strat_failure (); + move_start_noangle (top.decision_pos, ASSERV_BACKWARD, 0); + return FSM_NEXT (TOP_TOTEM_APPROACHING, robot_move_failure); +} + +FSM_TRANS (TOP_TOTEM_PUSHING, robot_move_failure, + TOP_TOTEM_ERROR_GOING_BACK) +{ + strat_failure (); + move_start_noangle (top.decision_pos, ASSERV_BACKWARD, 0); + return FSM_NEXT (TOP_TOTEM_PUSHING, robot_move_failure); +} + +FSM_TRANS (TOP_TOTEM_ERROR_GOING_BACK, move_success, TOP_TOTEM_CLAMP_UPPING) +{ + FSM_HANDLE (AI, stop_tree_approach); + return FSM_NEXT (TOP_TOTEM_ERROR_GOING_BACK, move_success); +} + +FSM_TRANS (TOP_TOTEM_ERROR_GOING_BACK, move_failure, TOP_TOTEM_CLAMP_UPPING) +{ + FSM_HANDLE (AI, stop_tree_approach); + return FSM_NEXT (TOP_TOTEM_ERROR_GOING_BACK, move_failure); +} + /** BOTTLE */ FSM_TRANS (TOP_BOTTLE_GOING, move_success, TOP_BOTTLE_APPROACHING) @@ -255,6 +302,15 @@ FSM_TRANS (TOP_BOTTLE_PUSHING, robot_move_success, TOP_BOTTLE_GOING_BACK) return FSM_NEXT (TOP_BOTTLE_PUSHING, robot_move_success); } +FSM_TRANS (TOP_BOTTLE_PUSHING, robot_move_failure, TOP_BOTTLE_GOING_BACK) +{ + /* OK, ignore failure. */ + asserv_stop_motor (); + strat_success (); + move_start_noangle (top.decision_pos, 0, 0); + 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, @@ -263,6 +319,48 @@ FSM_TRANS (TOP_BOTTLE_GOING_BACK, move_success, RETURN_TOP_DECISION_SWITCH (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) +{ + /* Ignore and continue. */ + RETURN_TOP_DECISION_SWITCH (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) +{ + strat_failure (); + RETURN_TOP_DECISION_SWITCH (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) +{ + position_t robot_pos; + asserv_get_position (&robot_pos); + /* Try to continue anyway? */ + if (robot_pos.v.y < BOT_SIZE_BACK + 100) + { + asserv_push_the_wall (ASSERV_BACKWARD, -1, -1, -1); + return FSM_NEXT (TOP_BOTTLE_APPROACHING, robot_move_failure, + try_anyway); + } + else + { + strat_failure (); + RETURN_TOP_DECISION_SWITCH (TOP_BOTTLE_APPROACHING, robot_move_failure); + } +} + /** UNLOAD */ FSM_TRANS (TOP_UNLOAD_GOING, move_success, TOP_UNLOADING) @@ -283,3 +381,14 @@ FSM_TRANS_TIMEOUT (TOP_UNLOADING, 250, RETURN_TOP_DECISION_SWITCH (TOP_UNLOADING, TOP_UNLOADING_TIMEOUT); } +/** UNLOAD failures. */ + +FSM_TRANS (TOP_UNLOAD_GOING, move_failure, + totem, TOP_TOTEM_GOING, + bottle, TOP_BOTTLE_GOING, + unload, TOP_UNLOAD_GOING) +{ + strat_failure (); + RETURN_TOP_DECISION_SWITCH (TOP_UNLOAD_GOING, move_failure); +} + -- cgit v1.2.3