From 373e63b6831a987d8c4afd9979d9ea829f784506 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 9 May 2013 11:22:32 +0200 Subject: digital/io-hub/src/apbirthday: add plate and candle arm demo --- digital/io-hub/src/apbirthday/robot.cc | 24 +++++++++++++++++ digital/io-hub/src/apbirthday/top.cc | 49 +++++++++++++++++++++++++++++++++- 2 files changed, 72 insertions(+), 1 deletion(-) (limited to 'digital') diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc index 9a634cff..64f2c93f 100644 --- a/digital/io-hub/src/apbirthday/robot.cc +++ b/digital/io-hub/src/apbirthday/robot.cc @@ -248,8 +248,32 @@ Robot::fsm_gen_event () bool Robot::demo_fsm_gen_event () { + static bool color_switch_last = true; + static bool robot_nb_switch_last = true; + static int sleep; + // Bounce detection. + if (sleep) + { + sleep--; + return false; + } + // Strat for candles demo mode. if (robot->hardware.ihm_strat.get ()) fsm_handle_and_return (top_demo_candles); + // Color for candle arm. + if (robot->hardware.ihm_color.get () != color_switch_last) + { + color_switch_last = robot->hardware.ihm_color.get (); + sleep = 125; + fsm_handle_and_return (top_demo_candle_arm); + } + // Robot nb for plate arm. + if (robot->hardware.ihm_robot_nb.get () != robot_nb_switch_last) + { + robot_nb_switch_last = robot->hardware.ihm_robot_nb.get (); + sleep = 125; + fsm_handle_and_return (top_demo_plate); + } return false; } diff --git a/digital/io-hub/src/apbirthday/top.cc b/digital/io-hub/src/apbirthday/top.cc index 0950bcbe..4cc99028 100644 --- a/digital/io-hub/src/apbirthday/top.cc +++ b/digital/io-hub/src/apbirthday/top.cc @@ -346,7 +346,14 @@ ANGFSM_STATES ( // Demo mode: move away from the wall. TOP_DEMO_CANDLES_MOVE_AWAY, // Demo mode: follow the cake (or anything else actually). - TOP_DEMO_FOLLOW) + TOP_DEMO_FOLLOW, + // Demo mode: candle arm steps. + TOP_DEMO_CANDLE_ARM_DEPLOY, + TOP_DEMO_CANDLE_ARM_FAR, + TOP_DEMO_CANDLE_ARM_NEAR, + // Demo mode: plate arm steps. + TOP_DEMO_PLATE_UP, + TOP_DEMO_PLATE_DOWN) ANGFSM_EVENTS ( // Cake following finished (end point reached). @@ -359,6 +366,10 @@ ANGFSM_EVENTS ( top_gifts_open, // Start candle demo. top_demo_candles, + // Start candle arm demo. + top_demo_candle_arm, + // Start plate demo. + top_demo_plate, // Start follow the cake demo. top_demo_follow) @@ -714,3 +725,39 @@ FSM_TRANS (TOP_DEMO_FOLLOW, top_follow_finished, TOP_DEMO_FOLLOW) // Transition needed for top_update. } +FSM_TRANS (TOP_INIT_ACTUATORS, top_demo_candle_arm, TOP_DEMO_CANDLE_ARM_DEPLOY) +{ + ANGFSM_HANDLE (AI, ai_candle_deploy); +} + +FSM_TRANS (TOP_DEMO_CANDLE_ARM_DEPLOY, top_demo_candle_arm, TOP_DEMO_CANDLE_ARM_FAR) +{ + ANGFSM_HANDLE (AI, ai_candle_far_punch); +} + +FSM_TRANS (TOP_DEMO_CANDLE_ARM_FAR, top_demo_candle_arm, + TOP_DEMO_CANDLE_ARM_NEAR) +{ + ANGFSM_HANDLE (AI, ai_candle_near_punch); +} + +FSM_TRANS (TOP_DEMO_CANDLE_ARM_NEAR, top_demo_candle_arm, TOP_INIT_ACTUATORS) +{ + ANGFSM_HANDLE (AI, ai_candle_undeploy); +} + +FSM_TRANS (TOP_INIT_ACTUATORS, top_demo_plate, TOP_DEMO_PLATE_UP) +{ + ANGFSM_HANDLE (AI, plate_take); +} + +FSM_TRANS (TOP_DEMO_PLATE_UP, top_demo_plate, TOP_DEMO_PLATE_DOWN) +{ + ANGFSM_HANDLE (AI, plate_drop); +} + +FSM_TRANS (TOP_DEMO_PLATE_DOWN, top_demo_plate, TOP_INIT_ACTUATORS) +{ + ANGFSM_HANDLE (AI, cannon_fire); +} + -- cgit v1.2.3