From f1369e5a073e6ed7f8211ab93934a2b8bbe5b3f4 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 4 May 2013 16:06:35 +0200 Subject: digital/io-hub/src/apbirthday: basic gifts handling --- digital/io-hub/src/apbirthday/gifts.hh | 53 +++++++++++++++++ digital/io-hub/src/apbirthday/playground_2013.hh | 6 ++ digital/io-hub/src/apbirthday/robot.hh | 3 + digital/io-hub/src/apbirthday/strat.cc | 15 +++++ digital/io-hub/src/apbirthday/strat.hh | 16 ++++++ digital/io-hub/src/apbirthday/top.cc | 72 ++++++++++++++++++++++++ 6 files changed, 165 insertions(+) create mode 100644 digital/io-hub/src/apbirthday/gifts.hh diff --git a/digital/io-hub/src/apbirthday/gifts.hh b/digital/io-hub/src/apbirthday/gifts.hh new file mode 100644 index 00000000..f6d7e1ee --- /dev/null +++ b/digital/io-hub/src/apbirthday/gifts.hh @@ -0,0 +1,53 @@ +#ifndef gifts_hh +#define gifts_hh +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 Nicolas Schodet +// +// APBTeam: +// Web: http://apbteam.org/ +// Email: team AT apbteam DOT org +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "playground_2013.hh" + +/// Keep gift status. +class Gifts +{ + public: + /// Constructor. + Gifts () + { + for (int i = 0; i < nb; i++) + open[i] = false; + } + /// Update gifts positions according to team color. + void compute_pos () + { + for (int i = 0; i < nb; i++) + x[i] = pg_x (600 * (i + 1) - pg_gift_width / 2); + } + public: + /// Number of gifts. + static const int nb = 4; + /// Are gifts open? + bool open[nb]; + /// Gifts x positions. + int x[nb]; +}; + +#endif // gifts_hh diff --git a/digital/io-hub/src/apbirthday/playground_2013.hh b/digital/io-hub/src/apbirthday/playground_2013.hh index d18014bf..b85585c7 100644 --- a/digital/io-hub/src/apbirthday/playground_2013.hh +++ b/digital/io-hub/src/apbirthday/playground_2013.hh @@ -37,4 +37,10 @@ static const int pg_cake_distance = 40; /// Plate width / 2. static const int pg_plate_size_border = 170 / 2; +/// Distance from the border to open gifts. +static const int pg_gifts_distance = 70; + +/// Width of a gift. +static const int pg_gift_width = 176; + #endif // playground_2013_hh diff --git a/digital/io-hub/src/apbirthday/robot.hh b/digital/io-hub/src/apbirthday/robot.hh index a92b36ef..e4c66477 100644 --- a/digital/io-hub/src/apbirthday/robot.hh +++ b/digital/io-hub/src/apbirthday/robot.hh @@ -43,6 +43,7 @@ #include "path_2013.hh" #include "strat.hh" #include "candles.hh" +#include "gifts.hh" #include "drinks.hh" #include "plate.hh" #include "cannon.hh" @@ -122,6 +123,8 @@ class Robot : public ucoo::Proto::Handler Strat strat; /// Candles. Candles candles; + /// Gifts. + Gifts gifts; /// Drinks. Drinks drinks; /// Plate. diff --git a/digital/io-hub/src/apbirthday/strat.cc b/digital/io-hub/src/apbirthday/strat.cc index 4fcfb79b..dd284b5b 100644 --- a/digital/io-hub/src/apbirthday/strat.cc +++ b/digital/io-hub/src/apbirthday/strat.cc @@ -50,6 +50,15 @@ Strat::decision (Position &pos) plate_decision_.loading_pos = pg_vect (200, 1400 - plate_load); pos = pg_position_deg (200, 600 + plate_load, 90); return PLATE; + case 2: + gifts_decision_.begin_pos = (vect_t) { 600, pg_gifts_distance + + BOT_SIZE_SIDE }; + gifts_decision_.end_pos = (vect_t) { 2400, pg_gifts_distance + + BOT_SIZE_SIDE }; + gifts_decision_.dir = Asserv::FORWARD; + pos.v = gifts_decision_.begin_pos; + pos.a = 0; + return GIFTS; default: pos.v = pg_cake_pos; pos.a = 0; @@ -125,6 +134,12 @@ Strat::decision_plate (PlateDecision &decision) decision = plate_decision_; } +void +Strat::decision_gifts (GiftsDecision &decision) +{ + decision = gifts_decision_; +} + void Strat::failure () { diff --git a/digital/io-hub/src/apbirthday/strat.hh b/digital/io-hub/src/apbirthday/strat.hh index 15c6a45d..4e962824 100644 --- a/digital/io-hub/src/apbirthday/strat.hh +++ b/digital/io-hub/src/apbirthday/strat.hh @@ -25,6 +25,7 @@ // }}} #include "defs.hh" #include "playground_2013.hh" +#include "asserv.hh" /// High level strategy decision making. class Strat @@ -34,6 +35,7 @@ class Strat { CANDLES, PLATE, + GIFTS, }; /// Information on a candle decision. struct CandlesDecision @@ -55,6 +57,16 @@ class Strat /// the point is reached, there is no plate. vect_t loading_pos; }; + /// Information on a gift decision. + struct GiftsDecision + { + /// Begin of movement position, same as reported by decision method. + vect_t begin_pos; + /// End of movement position. + vect_t end_pos; + /// Movement direction. + Asserv::DirectionConsign dir; + }; public: /// Return new decision and associated position. Decision decision (Position &pos); @@ -63,11 +75,15 @@ class Strat uint16_t robot_angle); /// Take a decision related to plate. void decision_plate (PlateDecision &decision); + /// Take a decision related to gifts. + void decision_gifts (GiftsDecision &decision); /// Report a failure to apply the previous decision. void failure (); private: /// Last plate decision. PlateDecision plate_decision_; + /// Last gifts decision. + GiftsDecision gifts_decision_; }; #endif // strat_hh diff --git a/digital/io-hub/src/apbirthday/top.cc b/digital/io-hub/src/apbirthday/top.cc index 84e7be1a..b3a91825 100644 --- a/digital/io-hub/src/apbirthday/top.cc +++ b/digital/io-hub/src/apbirthday/top.cc @@ -27,6 +27,7 @@ #include "bot.hh" #include +#include /// Top context. struct top_t @@ -37,6 +38,10 @@ struct top_t int candles_last_blown[Candles::FLOOR_NB]; /// Plate decision information. Strat::PlateDecision plate; + /// Gifts decision information. + Strat::GiftsDecision gifts; + /// Gift being opened, remember to close the arm after a delay. + int gifts_opening; }; static top_t top; @@ -192,6 +197,15 @@ top_update () cons = robot->hardware.adc_cake_back.read () - back_offset; robot->asserv.follow_update (cons * k / 1000); } + if (top.gifts_opening) + { + top.gifts_opening--; + if (!top.gifts_opening) + { + robot->hardware.gift_out.reset (); + robot->hardware.gift_in.set (); + } + } } bool @@ -231,6 +245,23 @@ top_fsm_gen_event () if (ANGFSM_HANDLE (AI, top_plate_present)) return true; } + // Gifts. + if (ANGFSM_CAN_HANDLE (AI, top_gifts_open)) + { + Position pos = robot->asserv.get_position (); + for (int i = 0; i < Gifts::nb; i++) + { + if (!robot->gifts.open[i] + && std::abs (pos.v.x - robot->gifts.x[i]) < 25) + { + if (ANGFSM_HANDLE (AI, top_gifts_open)) + { + robot->gifts.open[i] = true; + return true; + } + } + } + } return false; } @@ -275,6 +306,10 @@ ANGFSM_STATES ( TOP_PLATE_LOADING, // Plate: drop plate. TOP_PLATE_DROPING, + // Gifts: go to gifts. + TOP_GIFTS_GOTO, + // Gifts: going along the wall, opening gifts. + TOP_GIFTS_OPEN, // Demo mode: push the wall near the cake. TOP_DEMO_CANDLES_PUSH_WALL, // Demo mode: move away from the wall. @@ -289,6 +324,8 @@ ANGFSM_EVENTS ( top_follow_blocked, // Plate present, can be taken. top_plate_present, + // Open a gift now. + top_gifts_open, // Start candle demo. top_demo_candles, // Start follow the cake demo. @@ -306,6 +343,7 @@ FSM_TRANS (TOP_START, init_actuators, TOP_INIT_ACTUATORS) FSM_TRANS (TOP_INIT_ACTUATORS, init_done, TOP_INIT) { // Color dependent init can go here. + robot->gifts.compute_pos (); } FSM_TRANS (TOP_INIT, init_start_round, TOP_DECISION) @@ -315,6 +353,7 @@ FSM_TRANS (TOP_INIT, init_start_round, TOP_DECISION) FSM_TRANS_TIMEOUT (TOP_DECISION, 1, candles, TOP_CANDLES_GOTO_NORMAL, plate, TOP_PLATE_GOTO, + gifts, TOP_GIFTS_GOTO, none, TOP_START) { if (robot->demo) @@ -332,6 +371,10 @@ FSM_TRANS_TIMEOUT (TOP_DECISION, 1, robot->strat.decision_plate (top.plate); robot->move.start (d_pos, Asserv::BACKWARD_REVERT_OK); return FSM_BRANCH (plate); + case Strat::GIFTS: + robot->strat.decision_gifts (top.gifts); + robot->move.start (d_pos, Asserv::REVERT_OK); + return FSM_BRANCH (gifts); default: ucoo::assert_unreachable (); } @@ -488,6 +531,35 @@ FSM_TRANS (TOP_PLATE_LOADING, plate_taken, TOP_DECISION) { } +/// +/// Gifts mode. +/// + +FSM_TRANS (TOP_GIFTS_GOTO, move_success, TOP_GIFTS_OPEN) +{ + robot->move.start (top.gifts.end_pos, top.gifts.dir); +} + +FSM_TRANS (TOP_GIFTS_GOTO, move_failure, TOP_DECISION) +{ +} + +FSM_TRANS (TOP_GIFTS_OPEN, move_success, TOP_DECISION) +{ +} + +FSM_TRANS (TOP_GIFTS_OPEN, move_failure, TOP_DECISION) +{ +} + +FSM_TRANS (TOP_GIFTS_OPEN, top_gifts_open, TOP_GIFTS_OPEN) +{ + robot->hardware.gift_out.set (); + robot->hardware.gift_in.reset (); + // This is the delay to keep the arm out. + top.gifts_opening = 75; +} + /// /// Demo mode. /// -- cgit v1.2.3