From 16fcdb2ec558570850f7afd2a5b797f67a450842 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 9 May 2013 09:50:45 +0200 Subject: digital/io-hub/src/apbirthday: wait for slow candle arm undeploying --- digital/io-hub/src/apbirthday/top.cc | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 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 b8c04b94..b325d54e 100644 --- a/digital/io-hub/src/apbirthday/top.cc +++ b/digital/io-hub/src/apbirthday/top.cc @@ -297,6 +297,8 @@ ANGFSM_STATES ( TOP_CANDLES_LEAVE_TANGENT_MOVE, // Candles: turn to leave, undeploy arm as soon as possible. TOP_CANDLES_LEAVE_TURN, + // Candles: wait for slow undeploying. + TOP_CANDLES_LEAVE_UNDEPLOY, // Candles: go away so that the robot is free to turn. TOP_CANDLES_LEAVE_GO_AWAY, // Plate: go to plate, normal move. @@ -472,17 +474,21 @@ FSM_TRANS (TOP_CANDLES_LEAVE_TANGENT_MOVE, robot_move_failure, } FSM_TRANS (TOP_CANDLES_LEAVE_TURN, robot_move_success, - TOP_CANDLES_LEAVE_GO_AWAY) + TOP_CANDLES_LEAVE_UNDEPLOY) { // TODO: undeploy earlier, by computing arm end position. ANGFSM_HANDLE (AI, ai_candle_undeploy); - robot->asserv.move_distance (BOT_SIZE_RADIUS - BOT_SIZE_SIDE); } FSM_TRANS (TOP_CANDLES_LEAVE_TURN, robot_move_failure, - TOP_CANDLES_LEAVE_GO_AWAY) + TOP_CANDLES_LEAVE_UNDEPLOY) { ANGFSM_HANDLE (AI, ai_candle_undeploy); +} + +FSM_TRANS (TOP_CANDLES_LEAVE_UNDEPLOY, ai_candle_success, + TOP_CANDLES_LEAVE_GO_AWAY) +{ robot->asserv.move_distance (BOT_SIZE_RADIUS - BOT_SIZE_SIDE); } -- cgit v1.2.3