summaryrefslogtreecommitdiffhomepage
path: root/digital/io-hub/src/apbirthday/top.cc
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io-hub/src/apbirthday/top.cc')
-rw-r--r--digital/io-hub/src/apbirthday/top.cc72
1 files changed, 72 insertions, 0 deletions
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.
///