From 89180a5289dad3238563bd86377aa41944af756f Mon Sep 17 00:00:00 2001 From: Jerome Jutteau Date: Sun, 21 Apr 2013 06:35:16 +0200 Subject: digital/io-hub/src/apbirthday: add tests in proto and functions for candles --- digital/io-hub/src/apbirthday/candles.cc | 103 ++++++++++++++++++++++--------- digital/io-hub/src/apbirthday/candles.hh | 7 +++ digital/io-hub/src/apbirthday/robot.cc | 96 +++++++++++++++++++++++++--- 3 files changed, 169 insertions(+), 37 deletions(-) diff --git a/digital/io-hub/src/apbirthday/candles.cc b/digital/io-hub/src/apbirthday/candles.cc index 2e0f3661..c60281f4 100644 --- a/digital/io-hub/src/apbirthday/candles.cc +++ b/digital/io-hub/src/apbirthday/candles.cc @@ -96,43 +96,47 @@ inline bool Candles::is_near (int pos) return pos >= far_count; } - inline bool Candles::is_far (int pos) { return pos < far_count; } -void Candles::deploy_arm () +void Candles::flamby_arm () { - // Deploy arm. - robot->hardware.cake_arm_out.set (true); - robot->hardware.cake_arm_in.set (false); - // Prepare punhers. - unpush_far (); - unpush_near (); -} - -void Candles::undeploy_arm_1 () -{ - // Get a flabby arm (usefull in case of failure). robot->hardware.cake_arm_out.set (false); robot->hardware.cake_arm_in.set (false); - // Prepare far puncher to undeploy. - Candles::push_far (); - // Be sure the near punched in not punching. - Candles::unpush_near (); } -void Candles::undeploy_arm_2 () +void Candles::flamby_far () { - // Unleach far puncher. robot->hardware.cake_push_far_out.set (false); robot->hardware.cake_push_far_in.set (false); - // Put arm back. +} + +void Candles::crampe_arm () +{ + robot->hardware.cake_arm_out.set (true); + robot->hardware.cake_arm_in.set (true); +} + +void Candles::crampe_far () +{ + robot->hardware.cake_push_far_out.set (true); + robot->hardware.cake_push_far_in.set (true); +} + +void Candles::arm_back () +{ robot->hardware.cake_arm_out.set (false); robot->hardware.cake_arm_in.set (true); } +void Candles::arm_out () +{ + robot->hardware.cake_arm_out.set (true); + robot->hardware.cake_arm_in.set (false); +} + void Candles::push_near () { robot->hardware.cake_push_near_out.set (true); @@ -157,15 +161,44 @@ void Candles::unpush_far () robot->hardware.cake_push_far_in.set (true); } +void Candles::deploy_arm () +{ + // Deploy arm. + arm_out (); + // Prepare punhers. + unpush_far (); + unpush_near (); +} + +void Candles::undeploy_arm_1 () +{ + arm_back (); + // Just to be sure. + unpush_far (); + unpush_near (); +} + +void Candles::undeploy_arm_2 () +{ + crampe_far (); +} + +void Candles::undeploy_arm_3 () +{ + flamby_far (); +} + // Global candle FSM. FSM_STATES (AI_CANDLE_OFF, AI_CANDLE_INIT, AI_CANDLE_SLEEPING, AI_CANDLE_DEPLOYING, - AI_CANDLE_FALLING_BACK_TO_UNDEPLOYED, + AI_CANDLE_FALLING_BACK_TO_UNDEPLOYED_1, + AI_CANDLE_FALLING_BACK_TO_UNDEPLOYED_2, AI_CANDLE_READY, AI_CANDLE_UNDEPLOYING, - AI_CANDLE_UNDEPLOYING_2) + AI_CANDLE_UNDEPLOYING_2, + AI_CANDLE_UNDEPLOYING_3) FSM_EVENTS (ai_candle_deploy, ai_candle_undeploy, @@ -180,7 +213,7 @@ FSM_TRANS (AI_CANDLE_OFF, init_actuators, AI_CANDLE_INIT) Candles::deploy_arm (); } -FSM_TRANS_TIMEOUT (AI_CANDLE_INIT, 125, AI_CANDLE_UNDEPLOYING) +FSM_TRANS_TIMEOUT (AI_CANDLE_INIT, 200, AI_CANDLE_UNDEPLOYING) { Candles::undeploy_arm_1 (); } @@ -195,9 +228,9 @@ 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, 125, +FSM_TRANS_TIMEOUT (AI_CANDLE_DEPLOYING, 200, success, AI_CANDLE_READY, - failure, AI_CANDLE_FALLING_BACK_TO_UNDEPLOYED) + failure, AI_CANDLE_FALLING_BACK_TO_UNDEPLOYED_1) { if (!robot->hardware.cake_arm_out_contact.get ()) { @@ -213,11 +246,16 @@ FSM_TRANS_TIMEOUT (AI_CANDLE_DEPLOYING, 125, } } -FSM_TRANS_TIMEOUT (AI_CANDLE_FALLING_BACK_TO_UNDEPLOYED, 12, AI_CANDLE_SLEEPING) +FSM_TRANS_TIMEOUT (AI_CANDLE_FALLING_BACK_TO_UNDEPLOYED_1, 160, AI_CANDLE_FALLING_BACK_TO_UNDEPLOYED_2) { Candles::undeploy_arm_2 (); } +FSM_TRANS_TIMEOUT (AI_CANDLE_FALLING_BACK_TO_UNDEPLOYED_2, 56, AI_CANDLE_SLEEPING) +{ + Candles::undeploy_arm_3 (); +} + FSM_TRANS (AI_CANDLE_READY, ai_candle_blow, AI_CANDLE_READY) { int i; @@ -259,12 +297,17 @@ FSM_TRANS (AI_CANDLE_READY, ai_candle_undeploy, AI_CANDLE_UNDEPLOYING) Candles::undeploy_arm_1 (); } -FSM_TRANS_TIMEOUT (AI_CANDLE_UNDEPLOYING, 10, AI_CANDLE_UNDEPLOYING_2) //TODO timeout value +FSM_TRANS_TIMEOUT (AI_CANDLE_UNDEPLOYING, 160, AI_CANDLE_UNDEPLOYING_2) { Candles::undeploy_arm_2 (); } -FSM_TRANS_TIMEOUT (AI_CANDLE_UNDEPLOYING_2, 125, +FSM_TRANS_TIMEOUT (AI_CANDLE_UNDEPLOYING_2, 56, AI_CANDLE_UNDEPLOYING_3) +{ + Candles::undeploy_arm_3 (); +} + +FSM_TRANS_TIMEOUT (AI_CANDLE_UNDEPLOYING_3, 100, success, AI_CANDLE_SLEEPING, failure, AI_CANDLE_READY) { @@ -294,7 +337,7 @@ FSM_TRANS (AI_CANDLE_FAR_SLEEPING, ai_candle_far_punch, AI_CANDLE_FAR_PUNCHING) Candles::push_far (); } -FSM_TRANS_TIMEOUT (AI_CANDLE_FAR_PUNCHING, 25, AI_CANDLE_FAR_SLEEPING) //TODO timeout value +FSM_TRANS_TIMEOUT (AI_CANDLE_FAR_PUNCHING, 48, AI_CANDLE_FAR_SLEEPING) { Candles::unpush_far (); } @@ -312,7 +355,7 @@ FSM_TRANS (AI_CANDLE_NEAR_SLEEPING, ai_candle_near_punch, AI_CANDLE_NEAR_PUNCHIN Candles::push_near (); } -FSM_TRANS_TIMEOUT (AI_CANDLE_NEAR_PUNCHING, 25, AI_CANDLE_NEAR_SLEEPING) //TODO timeout value +FSM_TRANS_TIMEOUT (AI_CANDLE_NEAR_PUNCHING, 37, AI_CANDLE_NEAR_SLEEPING) { Candles::unpush_near (); } diff --git a/digital/io-hub/src/apbirthday/candles.hh b/digital/io-hub/src/apbirthday/candles.hh index fb28aefd..25069cec 100644 --- a/digital/io-hub/src/apbirthday/candles.hh +++ b/digital/io-hub/src/apbirthday/candles.hh @@ -69,10 +69,17 @@ class Candles static void deploy_arm (); static void undeploy_arm_1 (); static void undeploy_arm_2 (); + static void undeploy_arm_3 (); + static void arm_out (); + static void arm_back (); static void push_near (); static void unpush_near (); static void push_far (); static void unpush_far (); + static void flamby_arm (); + static void flamby_far (); + static void crampe_arm (); + static void crampe_far (); private: static const int total_count = 20; static const int near_count = 12; diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc index 3082df3e..76a26d7a 100644 --- a/digital/io-hub/src/apbirthday/robot.cc +++ b/digital/io-hub/src/apbirthday/robot.cc @@ -320,17 +320,99 @@ Robot::proto_handle (ucoo::Proto &proto, char cmd, const uint8_t *args, int size break; case c ('b', 2): // Candles arm manipulation. - // - 1B: manipulate arm (0) or punchers (1) - // - 1B: action: - // * deploy arm (0) - // * undeploy arm (1) - // * candle value + // - 00: arm events + // - 00: deploy arm + // - 01: undeploy arm + // - 01: near + // - 00: push + // - 01: unpush + // - 02: far + // - 00: push + // - 01: unpush + // - 03: deploying + // - 00: deploy + // - 01: undeploy 1 + // - 02: undeploy 2 + // - 04: flamby + // - 00: arm + // - 01: far puncher + // - 05: arm + // - 00: out + // - 01: in + // - 06: crampe + // - 00: arm + // - 01: far + // - 07: test undeploy + // - bb: sleep 1 + // - bb: sleep 2 + // - 08: test sleep + // - 01: near + // - bb: sleep + // - 02: far + // - bb: sleep if (args[0] == 0 && args[1] == 0) FSM_HANDLE (AI, ai_candle_deploy); else if (args[0] == 0 && args[1] == 1) FSM_HANDLE (AI, ai_candle_undeploy); - else - candles.blow (args[1]); + else if (args[0] == 1 && args[1] == 0) + Candles::push_near (); + else if (args[0] == 1 && args[1] == 1) + Candles::unpush_near (); + else if (args[0] == 2 && args[1] == 0) + Candles::push_far (); + else if (args[0] == 2 && args[1] == 1) + Candles::unpush_far (); + else if (args[0] == 3 && args[1] == 0) + Candles::deploy_arm (); + else if (args[0] == 3 && args[1] == 1) + Candles::undeploy_arm_1 (); + else if (args[0] == 3 && args[1] == 2) + Candles::undeploy_arm_2 (); + else if (args[0] == 4 && args[1] == 0) + Candles::flamby_arm (); + else if (args[0] == 4 && args[1] == 1) + Candles::flamby_far (); + else if (args[0] == 5 && args[1] == 0) + Candles::arm_out (); + else if (args[0] == 5 && args[1] == 1) + Candles::arm_back (); + else if (args[0] == 6 && args[1] == 0) + Candles::crampe_arm (); + else if (args[0] == 6 && args[1] == 1) + Candles::crampe_far (); + + break; + case c ('b', 3): + if (args[0] == 7) + { + int a = 0; + Candles::arm_back (); + while (a++ != args[1]) + robot->hardware.wait (); + Candles::crampe_far (); + a = 0; + while (a++ != args[2]) + robot->hardware.wait (); + Candles::flamby_far (); + } + if (args[0] == 8) + { + int a = 0; + if (args[1] == 1) + { + Candles::push_near (); + while (a++ != args[2]) + robot->hardware.wait (); + Candles::unpush_near (); + } + if (args[1] == 2) + { + Candles::push_far (); + while (a++ != args[2]) + robot->hardware.wait (); + Candles::unpush_far (); + } + } break; default: proto.send ('?'); -- cgit v1.2.3