From ad6d5779aa6dd3a0cedadc4bee0934763eab5ceb Mon Sep 17 00:00:00 2001 From: Jerome Jutteau Date: Thu, 9 May 2013 00:29:19 +0200 Subject: apbteam/digital/io-hub/src/apbirthday: add servo in candles arm --- digital/io-hub/src/apbirthday/candles.cc | 51 +++++++++++++++++++++++++------- digital/io-hub/src/apbirthday/candles.hh | 2 ++ 2 files changed, 42 insertions(+), 11 deletions(-) (limited to 'digital/io-hub') diff --git a/digital/io-hub/src/apbirthday/candles.cc b/digital/io-hub/src/apbirthday/candles.cc index 80afe55f..1faf58bc 100644 --- a/digital/io-hub/src/apbirthday/candles.cc +++ b/digital/io-hub/src/apbirthday/candles.cc @@ -162,6 +162,16 @@ void Candles::unpush_far () robot->hardware.cake_push_far_in.set (true); } +void Candles::servo_near_deploy () +{ + robot->hardware.servos.set_position (Servo::SERVO_CAKE, 0x1995); +} + +void Candles::servo_near_undeploy () +{ + robot->hardware.servos.set_position (Servo::SERVO_CAKE, 0x0600); +} + void Candles::deploy_arm () { // Deploy arm. @@ -193,11 +203,14 @@ void Candles::undeploy_arm_3 () FSM_STATES (AI_CANDLE_OFF, AI_CANDLE_INIT, AI_CANDLE_SLEEPING, - AI_CANDLE_DEPLOYING, + AI_CANDLE_DEPLOYING_SERVO, + AI_CANDLE_DEPLOYING_ARM, + AI_CANDLE_FALLING_BACK_TO_UNDEPLOYED_SERVO, AI_CANDLE_FALLING_BACK_TO_UNDEPLOYED_1, AI_CANDLE_FALLING_BACK_TO_UNDEPLOYED_2, AI_CANDLE_READY, - AI_CANDLE_UNDEPLOYING, + AI_CANDLE_UNDEPLOYING_SERVO, + AI_CANDLE_UNDEPLOYING_1, AI_CANDLE_UNDEPLOYING_2, AI_CANDLE_UNDEPLOYING_3) @@ -214,12 +227,12 @@ FSM_TRANS (AI_CANDLE_OFF, init_actuators, AI_CANDLE_INIT) Candles::deploy_arm (); } -FSM_TRANS_TIMEOUT (AI_CANDLE_INIT, 200, AI_CANDLE_UNDEPLOYING) +FSM_TRANS_TIMEOUT (AI_CANDLE_INIT, 200, AI_CANDLE_UNDEPLOYING_SERVO) { - Candles::undeploy_arm_1 (); + Candles::servo_near_undeploy (); } -FSM_TRANS (AI_CANDLE_SLEEPING, ai_candle_deploy, AI_CANDLE_DEPLOYING) +FSM_TRANS (AI_CANDLE_SLEEPING, ai_candle_deploy, AI_CANDLE_DEPLOYING_ARM) { Candles::deploy_arm (); } @@ -229,9 +242,14 @@ FSM_TRANS (AI_CANDLE_SLEEPING, ai_candle_undeploy, AI_CANDLE_SLEEPING) robot->fsm_queue.post (FSM_EVENT (ai_candle_success)); } -FSM_TRANS_TIMEOUT (AI_CANDLE_DEPLOYING, 200, +FSM_TRANS_TIMEOUT (AI_CANDLE_DEPLOYING_ARM, 300, AI_CANDLE_DEPLOYING_SERVO) +{ + Candles::servo_near_deploy (); +} + +FSM_TRANS_TIMEOUT (AI_CANDLE_DEPLOYING_SERVO, 300, success, AI_CANDLE_READY, - failure, AI_CANDLE_FALLING_BACK_TO_UNDEPLOYED_1) + failure, AI_CANDLE_FALLING_BACK_TO_UNDEPLOYED_SERVO) { // TODO: connect contact. if (1 || !robot->hardware.cake_arm_out_contact.get ()) @@ -242,12 +260,18 @@ FSM_TRANS_TIMEOUT (AI_CANDLE_DEPLOYING, 200, else { // Get back to Sleep. - Candles::undeploy_arm_1 (); + Candles::servo_near_undeploy (); robot->fsm_queue.post (FSM_EVENT (ai_candle_failure)); return FSM_BRANCH (failure); } } +FSM_TRANS_TIMEOUT (AI_CANDLE_FALLING_BACK_TO_UNDEPLOYED_SERVO, 300, + AI_CANDLE_FALLING_BACK_TO_UNDEPLOYED_1) +{ + Candles::undeploy_arm_1 (); +} + FSM_TRANS_TIMEOUT (AI_CANDLE_FALLING_BACK_TO_UNDEPLOYED_1, 160, AI_CANDLE_FALLING_BACK_TO_UNDEPLOYED_2) { Candles::undeploy_arm_2 (); @@ -311,12 +335,17 @@ FSM_TRANS (AI_CANDLE_READY, ai_candle_deploy, AI_CANDLE_READY) robot->fsm_queue.post (FSM_EVENT (ai_candle_success)); } -FSM_TRANS (AI_CANDLE_READY, ai_candle_undeploy, AI_CANDLE_UNDEPLOYING) +FSM_TRANS (AI_CANDLE_READY, ai_candle_undeploy, AI_CANDLE_UNDEPLOYING_SERVO) +{ + Candles::servo_near_undeploy (); +} + +FSM_TRANS_TIMEOUT (AI_CANDLE_UNDEPLOYING_SERVO, 300, AI_CANDLE_UNDEPLOYING_1) { Candles::undeploy_arm_1 (); } -FSM_TRANS_TIMEOUT (AI_CANDLE_UNDEPLOYING, 200, AI_CANDLE_UNDEPLOYING_2) +FSM_TRANS_TIMEOUT (AI_CANDLE_UNDEPLOYING_1, 200, AI_CANDLE_UNDEPLOYING_2) { Candles::undeploy_arm_2 (); } @@ -328,7 +357,7 @@ FSM_TRANS_TIMEOUT (AI_CANDLE_UNDEPLOYING_2, 42, AI_CANDLE_UNDEPLOYING_3) FSM_TRANS_TIMEOUT (AI_CANDLE_UNDEPLOYING_3, 100, success, AI_CANDLE_SLEEPING, - failure, AI_CANDLE_READY) + failure, AI_CANDLE_FALLING_BACK_TO_UNDEPLOYED_SERVO) { // TODO: connect contact. if (1 || !robot->hardware.cake_arm_in_contact.get ()) diff --git a/digital/io-hub/src/apbirthday/candles.hh b/digital/io-hub/src/apbirthday/candles.hh index 25069cec..1523b705 100644 --- a/digital/io-hub/src/apbirthday/candles.hh +++ b/digital/io-hub/src/apbirthday/candles.hh @@ -80,6 +80,8 @@ class Candles static void flamby_far (); static void crampe_arm (); static void crampe_far (); + static void servo_near_deploy (); + static void servo_near_undeploy (); private: static const int total_count = 20; static const int near_count = 12; -- cgit v1.2.3