summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorJerome Jutteau2013-04-21 06:35:16 +0200
committerJerome Jutteau2013-04-21 06:35:16 +0200
commit89180a5289dad3238563bd86377aa41944af756f (patch)
tree13075a4b054588646492bacebc868ca42d6c765e /digital
parent5e550fa2a08633dd2276be4839d6373112573fd4 (diff)
digital/io-hub/src/apbirthday: add tests in proto and functions for candles
Diffstat (limited to 'digital')
-rw-r--r--digital/io-hub/src/apbirthday/candles.cc103
-rw-r--r--digital/io-hub/src/apbirthday/candles.hh7
-rw-r--r--digital/io-hub/src/apbirthday/robot.cc96
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 ('?');