From cb52f8ceb8de4fb08f43d15501d845e39202f012 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 10 Apr 2013 23:40:15 +0200 Subject: digital/io-hub/src/apbirthday: add candles demo mode --- digital/io-hub/src/apbirthday/robot.cc | 12 +++++++ digital/io-hub/src/apbirthday/robot.hh | 4 +++ digital/io-hub/src/apbirthday/top.cc | 59 +++++++++++++++++++++++++++++++--- 3 files changed, 71 insertions(+), 4 deletions(-) (limited to 'digital/io-hub') diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc index 8ed99782..36629698 100644 --- a/digital/io-hub/src/apbirthday/robot.cc +++ b/digital/io-hub/src/apbirthday/robot.cc @@ -43,6 +43,7 @@ Robot::Robot () chrono (90000 - 1000), pressure (hardware.adc_pressure, hardware.pneum_open, mimot.motor0), jack (hardware.raw_jack, 50), + demo (false), usdist_control_ (2), usdist0_ (usdist_control_, hardware.adc_dist0, hardware.dist0_sync, 100, 700, 650), usdist1_ (usdist_control_, hardware.adc_dist1, hardware.dist1_sync, 100, 700, 650), @@ -171,6 +172,9 @@ Robot::fsm_gen_event () fsm_handle_and_return (jack_inserted); else fsm_handle_and_return (jack_removed); + // Demo mode. + if (demo && demo_fsm_gen_event ()) + return true; // FSM queue. if (fsm_queue.poll ()) { @@ -187,6 +191,14 @@ Robot::fsm_gen_event () return false; } +bool +Robot::demo_fsm_gen_event () +{ + if (robot->hardware.ihm_strat.get ()) + fsm_handle_and_return (top_demo_candles); + return false; +} + void Robot::proto_handle (ucoo::Proto &proto, char cmd, const uint8_t *args, int size) { diff --git a/digital/io-hub/src/apbirthday/robot.hh b/digital/io-hub/src/apbirthday/robot.hh index 30dabc90..556e29eb 100644 --- a/digital/io-hub/src/apbirthday/robot.hh +++ b/digital/io-hub/src/apbirthday/robot.hh @@ -54,6 +54,8 @@ class Robot : public ucoo::Proto::Handler void main_loop (); /// Generate events for the FSM. bool fsm_gen_event (); + /// Generate events for the FSM for demo mode. + bool demo_fsm_gen_event (); /// Receive proto messages. void proto_handle (ucoo::Proto &proto, char cmd, const uint8_t *args, int size); /// Send stats. @@ -85,6 +87,8 @@ class Robot : public ucoo::Proto::Handler Pressure pressure; /// Jack debouncing. Debounce jack; + /// Demo mode flag. + bool demo; private: /// US distance sensors controller. ucoo::UsDistControl usdist_control_; diff --git a/digital/io-hub/src/apbirthday/top.cc b/digital/io-hub/src/apbirthday/top.cc index 71c568ea..77b99c6f 100644 --- a/digital/io-hub/src/apbirthday/top.cc +++ b/digital/io-hub/src/apbirthday/top.cc @@ -224,6 +224,8 @@ ANGFSM_INIT ANGFSM_STATES ( // Initial state. TOP_START, + // Initialising actuators. + TOP_INIT_ACTUATORS, // Init done, waiting for rount start. TOP_INIT, // Decision state, one stop, one cycle. @@ -242,17 +244,29 @@ ANGFSM_STATES ( // Candles: turn to leave, undeploy arm as soon as possible. TOP_CANDLES_LEAVE_TURN, // Candles: go away so that the robot is free to turn. - TOP_CANDLES_LEAVE_GO_AWAY) + TOP_CANDLES_LEAVE_GO_AWAY, + // Demo mode: push the wall near the cake. + TOP_DEMO_CANDLES_PUSH_WALL, + // Demo mode: move away from the wall. + TOP_DEMO_CANDLES_MOVE_AWAY) ANGFSM_EVENTS ( // Cake following finished (end point reached). top_follow_finished, // Problem with cake following. - top_follow_blocked) + top_follow_blocked, + // Start candle demo. + top_demo_candles) ANGFSM_START_WITH (TOP_START) -FSM_TRANS (TOP_START, init_done, TOP_INIT) +FSM_TRANS (TOP_START, init_actuators, TOP_INIT_ACTUATORS) +{ + // TODO: make sure the operator do not forget this is demo mode! + robot->demo = !robot->hardware.ihm_strat.get (); +} + +FSM_TRANS (TOP_INIT_ACTUATORS, init_done, TOP_INIT) { // Color dependent init can go here. } @@ -263,8 +277,10 @@ FSM_TRANS (TOP_INIT, init_start_round, TOP_DECISION) FSM_TRANS_TIMEOUT (TOP_DECISION, 1, candles, TOP_CANDLES_GOTO_NORMAL, - none, TOP_INIT) + none, TOP_START) { + if (robot->demo) + return FSM_BRANCH (none); vect_t d_pos; Strat::Decision d = robot->strat.decision (d_pos); switch (d) @@ -278,6 +294,10 @@ FSM_TRANS_TIMEOUT (TOP_DECISION, 1, } } +/// +/// Candles. +/// + FSM_TRANS (TOP_CANDLES_GOTO_NORMAL, move_success, TOP_CANDLES_ENTER_DEPLOY) { ANGFSM_HANDLE (AI, ai_candle_deploy); @@ -372,3 +392,34 @@ FSM_TRANS (TOP_CANDLES_LEAVE_GO_AWAY, robot_move_failure, TOP_DECISION) { } +/// +/// Demo mode. +/// + +FSM_TRANS (TOP_INIT_ACTUATORS, top_demo_candles, TOP_DEMO_CANDLES_PUSH_WALL) +{ + team_color = TEAM_COLOR_LEFT; + robot->asserv.push_wall (Asserv::FORWARD, pg_cake_pos.x - pg_cake_radius + - 20 - BOT_SIZE_SIDE, + pg_length - BOT_FRONT_CONTACT_DIST, + G_ANGLE_UF016_DEG (90)); +} + +FSM_TRANS (TOP_DEMO_CANDLES_PUSH_WALL, robot_move_success, + TOP_DEMO_CANDLES_MOVE_AWAY) +{ + robot->asserv.move_distance (-200); +} + +FSM_TRANS (TOP_DEMO_CANDLES_MOVE_AWAY, robot_move_success, TOP_CANDLES_GOTO_NORMAL) +{ + robot->move.start (pg_cake_pos, Asserv::BACKWARD, pg_cake_radius + + pg_cake_distance + BOT_SIZE_SIDE); +} + +FSM_TRANS (TOP_INIT, top_demo_candles, TOP_CANDLES_GOTO_NORMAL) +{ + robot->move.start (pg_cake_pos, Asserv::BACKWARD, pg_cake_radius + + pg_cake_distance + BOT_SIZE_SIDE); +} + -- cgit v1.2.3