From d4d0cbc0a317a62347016a802ba26d55158204cf Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 10 May 2013 17:53:36 +0200 Subject: digital/io-hub/src/apbirthday: wait before turn when leaving the cake on problem --- digital/io-hub/src/apbirthday/top.cc | 24 ++++++++++++++++-------- 1 file changed, 16 insertions(+), 8 deletions(-) (limited to 'digital/io-hub') diff --git a/digital/io-hub/src/apbirthday/top.cc b/digital/io-hub/src/apbirthday/top.cc index 59d2b0c8..8c401014 100644 --- a/digital/io-hub/src/apbirthday/top.cc +++ b/digital/io-hub/src/apbirthday/top.cc @@ -121,7 +121,7 @@ top_follow_start () /// Can follow cake, decide if this is a good thing, or leave. static Branch -top_follow_or_leave () +top_follow_or_leave (bool wait_before_turn) { if (top_follow_start ()) { @@ -161,7 +161,8 @@ top_follow_or_leave () } else { - robot->asserv.goto_angle (robot_angle); + if (!wait_before_turn) + robot->asserv.goto_angle (robot_angle); return FSM_BRANCH (turn); } } @@ -322,6 +323,8 @@ ANGFSM_STATES ( TOP_CANDLES_FOLLOW, // Candles: tangent move to escape from an obstacle. TOP_CANDLES_LEAVE_TANGENT_MOVE, + // Candles: wait a little bit before turning. + TOP_CANDLES_LEAVE_TURN_WAIT, // Candles: turn to leave, undeploy arm as soon as possible. TOP_CANDLES_LEAVE_TURN, // Candles: wait for slow undeploying. @@ -487,7 +490,7 @@ FSM_TRANS (TOP_CANDLES_ENTER_TURN, robot_move_success, turn, TOP_CANDLES_LEAVE_TURN, candles, TOP_CANDLES_FOLLOW) { - return top_follow_or_leave (); + return top_follow_or_leave (false); } FSM_TRANS (TOP_CANDLES_ENTER_TURN, robot_move_failure, TOP_CANDLES_LEAVE_TURN) @@ -500,23 +503,23 @@ FSM_TRANS (TOP_CANDLES_FOLLOW, top_follow_finished, turn, TOP_CANDLES_LEAVE_TURN, candles, TOP_CANDLES_FOLLOW) { - return top_follow_or_leave (); + return top_follow_or_leave (false); } FSM_TRANS (TOP_CANDLES_FOLLOW, top_follow_blocked, tangent, TOP_CANDLES_LEAVE_TANGENT_MOVE, - turn, TOP_CANDLES_LEAVE_TURN, + turn, TOP_CANDLES_LEAVE_TURN_WAIT, candles, TOP_CANDLES_FOLLOW) { - return top_follow_or_leave (); + return top_follow_or_leave (true); } FSM_TRANS (TOP_CANDLES_FOLLOW, robot_move_failure, tangent, TOP_CANDLES_LEAVE_TANGENT_MOVE, - turn, TOP_CANDLES_LEAVE_TURN, + turn, TOP_CANDLES_LEAVE_TURN_WAIT, candles, TOP_CANDLES_FOLLOW) { - return top_follow_or_leave (); + return top_follow_or_leave (true); } FSM_TRANS (TOP_CANDLES_LEAVE_TANGENT_MOVE, robot_move_success, @@ -531,6 +534,11 @@ FSM_TRANS (TOP_CANDLES_LEAVE_TANGENT_MOVE, robot_move_failure, robot->asserv.goto_angle (top_cake_angle_robot ()); } +FSM_TRANS_TIMEOUT (TOP_CANDLES_LEAVE_TURN_WAIT, 125, TOP_CANDLES_LEAVE_TURN) +{ + robot->asserv.goto_angle (top_cake_angle_robot ()); +} + FSM_TRANS (TOP_CANDLES_LEAVE_TURN, robot_move_success, TOP_CANDLES_LEAVE_UNDEPLOY) { -- cgit v1.2.3