summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2013-04-20 16:48:20 +0200
committerNicolas Schodet2013-04-22 23:45:50 +0200
commitdff9259a55383a5892987f8a3b640a0c0e26cbfc (patch)
tree840042f87dece2061c46c66291c12ff9ef52a78c /digital
parent89e487abcff9d0f08befa5cc9ea69dc316a9e315 (diff)
digital/io-hub/src/apbirthday: plate loading behaviour
Diffstat (limited to 'digital')
-rw-r--r--digital/io-hub/src/apbirthday/bot.hh2
-rw-r--r--digital/io-hub/src/apbirthday/playground_2013.hh3
-rw-r--r--digital/io-hub/src/apbirthday/strat.cc35
-rw-r--r--digital/io-hub/src/apbirthday/strat.hh20
-rw-r--r--digital/io-hub/src/apbirthday/top.cc82
5 files changed, 136 insertions, 6 deletions
diff --git a/digital/io-hub/src/apbirthday/bot.hh b/digital/io-hub/src/apbirthday/bot.hh
index 5817a478..405a8c3c 100644
--- a/digital/io-hub/src/apbirthday/bot.hh
+++ b/digital/io-hub/src/apbirthday/bot.hh
@@ -55,5 +55,7 @@
#endif
/// Normal cruise speed.
#define BOT_SPEED_NORMAL 0x50, 0x60, 0x20, 0x20
+/// Speed used for plate loading.
+#define BOT_SPEED_PLATE 0x20, 0x20, 0x20, 0x20
#endif // bot_hh
diff --git a/digital/io-hub/src/apbirthday/playground_2013.hh b/digital/io-hub/src/apbirthday/playground_2013.hh
index d0911ba4..2e2224d5 100644
--- a/digital/io-hub/src/apbirthday/playground_2013.hh
+++ b/digital/io-hub/src/apbirthday/playground_2013.hh
@@ -34,4 +34,7 @@ static const vect_t pg_cake_pos = { pg_width / 2, pg_length };
/// Distance from the cake to blow candles.
static const int pg_cake_distance = 30;
+/// Plate width / 2.
+static const int pg_plate_size_border = 170 / 2;
+
#endif // playground_2013_hh
diff --git a/digital/io-hub/src/apbirthday/strat.cc b/digital/io-hub/src/apbirthday/strat.cc
index 328c1298..9950cdc6 100644
--- a/digital/io-hub/src/apbirthday/strat.cc
+++ b/digital/io-hub/src/apbirthday/strat.cc
@@ -25,13 +25,36 @@
#include "robot.hh"
#include "top.hh"
#include "debug.host.hh"
+#include "bot.hh"
Strat::Decision
-Strat::decision (vect_t &pos)
+Strat::decision (Position &pos)
{
// TODO: this is a stub.
- pos = pg_cake_pos;
- return CANDLES;
+ static int step;
+ static const int plate_app = pg_plate_size_border + BOT_SIZE_RADIUS + 20;
+ static const int plate_load = pg_plate_size_border + BOT_SIZE_BACK - 40;
+ switch (step++)
+ {
+ case 0:
+ plate_decision_.drop = false;
+ plate_decision_.approaching_pos =
+ pg_position_deg (200, 250 + plate_app, 90);
+ plate_decision_.loading_pos = pg_vect (200, 250 + plate_load);
+ pos = plate_decision_.approaching_pos;
+ return PLATE;
+ case 1:
+ plate_decision_.drop = true;
+ plate_decision_.approaching_pos =
+ pg_position_deg (200, 1000 - plate_app, -90);
+ plate_decision_.loading_pos = pg_vect (200, 1000 - plate_load);
+ pos = pg_position_deg (200, 250 + plate_load, 90);
+ return PLATE;
+ default:
+ pos.v = pg_cake_pos;
+ pos.a = 0;
+ return CANDLES;
+ }
}
/// Compute score for candles between first and last.
@@ -97,6 +120,12 @@ Strat::decision_candles (CandlesDecision &decision, uint16_t robot_angle)
}
void
+Strat::decision_plate (PlateDecision &decision)
+{
+ decision = plate_decision_;
+}
+
+void
Strat::failure ()
{
}
diff --git a/digital/io-hub/src/apbirthday/strat.hh b/digital/io-hub/src/apbirthday/strat.hh
index fb1a8bf3..15c6a45d 100644
--- a/digital/io-hub/src/apbirthday/strat.hh
+++ b/digital/io-hub/src/apbirthday/strat.hh
@@ -33,6 +33,7 @@ class Strat
enum Decision
{
CANDLES,
+ PLATE,
};
/// Information on a candle decision.
struct CandlesDecision
@@ -42,14 +43,31 @@ class Strat
/// Angle relative to cake to end the movement.
uint16_t end_angle;
};
+ /// Information on a plate decision.
+ struct PlateDecision
+ {
+ /// Should drop plate before.
+ bool drop;
+ /// Approach position, where the robot should be before starting
+ /// approaching.
+ Position approaching_pos;
+ /// Loading position, point where to go backward to load the plate. If
+ /// the point is reached, there is no plate.
+ vect_t loading_pos;
+ };
public:
/// Return new decision and associated position.
- Decision decision (vect_t &pos);
+ Decision decision (Position &pos);
/// Take a decision related to candles, return false to give up candles.
bool decision_candles (CandlesDecision &decision,
uint16_t robot_angle);
+ /// Take a decision related to plate.
+ void decision_plate (PlateDecision &decision);
/// Report a failure to apply the previous decision.
void failure ();
+ private:
+ /// Last plate decision.
+ PlateDecision plate_decision_;
};
#endif // strat_hh
diff --git a/digital/io-hub/src/apbirthday/top.cc b/digital/io-hub/src/apbirthday/top.cc
index c96b8c12..28d95811 100644
--- a/digital/io-hub/src/apbirthday/top.cc
+++ b/digital/io-hub/src/apbirthday/top.cc
@@ -35,6 +35,8 @@ struct top_t
Strat::CandlesDecision candles;
/// Last blown candles.
int candles_last_blown[Candles::FLOOR_NB];
+ /// Plate decision information.
+ Strat::PlateDecision plate;
};
static top_t top;
@@ -221,6 +223,13 @@ top_fsm_gen_event ()
}
}
}
+ // Plate contacts.
+ if (!robot->hardware.cherry_plate_left_contact.get ()
+ && !robot->hardware.cherry_plate_right_contact.get ())
+ {
+ if (ANGFSM_HANDLE (AI, top_plate_present))
+ return true;
+ }
return false;
}
@@ -257,6 +266,14 @@ ANGFSM_STATES (
TOP_CANDLES_LEAVE_TURN,
// Candles: go away so that the robot is free to turn.
TOP_CANDLES_LEAVE_GO_AWAY,
+ // Plate: go to plate, normal move.
+ TOP_PLATE_GOTO,
+ // Plate: go backward until a plate is seen.
+ TOP_PLATE_APPROACH,
+ // Plate: loading plate.
+ TOP_PLATE_LOADING,
+ // Plate: drop plate.
+ TOP_PLATE_DROPING,
// Demo mode: push the wall near the cake.
TOP_DEMO_CANDLES_PUSH_WALL,
// Demo mode: move away from the wall.
@@ -269,6 +286,8 @@ ANGFSM_EVENTS (
top_follow_finished,
// Problem with cake following.
top_follow_blocked,
+ // Plate present, can be taken.
+ top_plate_present,
// Start candle demo.
top_demo_candles,
// Start follow the cake demo.
@@ -293,18 +312,24 @@ FSM_TRANS (TOP_INIT, init_start_round, TOP_DECISION)
FSM_TRANS_TIMEOUT (TOP_DECISION, 1,
candles, TOP_CANDLES_GOTO_NORMAL,
+ plate, TOP_PLATE_GOTO,
none, TOP_START)
{
if (robot->demo)
return FSM_BRANCH (none);
- vect_t d_pos;
+ robot->asserv.set_speed (BOT_SPEED_NORMAL);
+ Position d_pos;
Strat::Decision d = robot->strat.decision (d_pos);
switch (d)
{
case Strat::CANDLES:
- robot->move.start (d_pos, Asserv::BACKWARD, pg_cake_radius
+ robot->move.start (d_pos.v, Asserv::BACKWARD, pg_cake_radius
+ pg_cake_distance + BOT_SIZE_SIDE);
return FSM_BRANCH (candles);
+ case Strat::PLATE:
+ robot->strat.decision_plate (top.plate);
+ robot->move.start (d_pos);
+ return FSM_BRANCH (plate);
default:
ucoo::assert_unreachable ();
}
@@ -409,6 +434,59 @@ FSM_TRANS (TOP_CANDLES_LEAVE_GO_AWAY, robot_move_failure, TOP_DECISION)
}
///
+/// Plate.
+///
+
+FSM_TRANS (TOP_PLATE_GOTO, move_success,
+ load, TOP_PLATE_APPROACH,
+ drop, TOP_PLATE_DROPING)
+{
+ if (top.plate.drop)
+ {
+ ANGFSM_HANDLE (AI, plate_drop);
+ return FSM_BRANCH (drop);
+ }
+ else
+ {
+ robot->asserv.set_speed (BOT_SPEED_PLATE);
+ robot->move.start (top.plate.loading_pos, Asserv::BACKWARD);
+ return FSM_BRANCH (load);
+ }
+}
+
+FSM_TRANS (TOP_PLATE_GOTO, move_failure, TOP_DECISION)
+{
+ robot->strat.failure ();
+}
+
+FSM_TRANS (TOP_PLATE_DROPING, plate_droped, TOP_PLATE_GOTO)
+{
+ top.plate.drop = false;
+ robot->move.start (top.plate.approaching_pos);
+}
+
+FSM_TRANS (TOP_PLATE_APPROACH, move_success, TOP_DECISION)
+{
+ // TODO: no plate.
+ robot->strat.failure ();
+}
+
+FSM_TRANS (TOP_PLATE_APPROACH, move_failure, TOP_DECISION)
+{
+ robot->strat.failure ();
+}
+
+FSM_TRANS (TOP_PLATE_APPROACH, top_plate_present, TOP_PLATE_LOADING)
+{
+ robot->move.stop ();
+ ANGFSM_HANDLE (AI, plate_take);
+}
+
+FSM_TRANS (TOP_PLATE_LOADING, plate_taken, TOP_DECISION)
+{
+}
+
+///
/// Demo mode.
///