summaryrefslogtreecommitdiffhomepage
path: root/digital/io-hub/src/apbirthday
diff options
context:
space:
mode:
authorNicolas Schodet2013-05-04 16:06:35 +0200
committerNicolas Schodet2013-05-04 16:07:24 +0200
commitf1369e5a073e6ed7f8211ab93934a2b8bbe5b3f4 (patch)
tree08fca5460a0f3a221e5ff1a263df10c8a9d2999c /digital/io-hub/src/apbirthday
parent14441591529dee7d1d22a6d9515ace8bb4270b8b (diff)
digital/io-hub/src/apbirthday: basic gifts handling
Diffstat (limited to 'digital/io-hub/src/apbirthday')
-rw-r--r--digital/io-hub/src/apbirthday/gifts.hh53
-rw-r--r--digital/io-hub/src/apbirthday/playground_2013.hh6
-rw-r--r--digital/io-hub/src/apbirthday/robot.hh3
-rw-r--r--digital/io-hub/src/apbirthday/strat.cc15
-rw-r--r--digital/io-hub/src/apbirthday/strat.hh16
-rw-r--r--digital/io-hub/src/apbirthday/top.cc72
6 files changed, 165 insertions, 0 deletions
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;
@@ -126,6 +135,12 @@ Strat::decision_plate (PlateDecision &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 <cmath>
+#include <cstdlib>
/// 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 ();
}
@@ -489,6 +532,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.
///