summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2013-05-10 07:37:37 +0200
committerNicolas Schodet2013-05-10 09:19:59 +0200
commite9f01fefddae4be19a98161a91bf262c046428ef (patch)
tree783f7fa4823325c4f1f6547a318ddbc329bc5028 /digital
parent529ba1f20c7c38e662673ebe37920abd0a8a27c6 (diff)
digital/io-hub/src/apbirthday: replace the strat stub
Diffstat (limited to 'digital')
-rw-r--r--digital/io-hub/src/apbirthday/gifts.hh3
-rw-r--r--digital/io-hub/src/apbirthday/plate.cc15
-rw-r--r--digital/io-hub/src/apbirthday/plate.hh15
-rw-r--r--digital/io-hub/src/apbirthday/strat.cc278
-rw-r--r--digital/io-hub/src/apbirthday/strat.hh29
-rw-r--r--digital/io-hub/src/apbirthday/top.cc11
6 files changed, 289 insertions, 62 deletions
diff --git a/digital/io-hub/src/apbirthday/gifts.hh b/digital/io-hub/src/apbirthday/gifts.hh
index f6d7e1ee..453b03e7 100644
--- a/digital/io-hub/src/apbirthday/gifts.hh
+++ b/digital/io-hub/src/apbirthday/gifts.hh
@@ -38,8 +38,9 @@ class Gifts
/// Update gifts positions according to team color.
void compute_pos ()
{
+ int sign = team_color ? -1 : 1;
for (int i = 0; i < nb; i++)
- x[i] = pg_x (600 * (i + 1) - pg_gift_width / 2);
+ x[i] = 600 * (i + 1) + sign * pg_gift_width / 2;
}
public:
/// Number of gifts.
diff --git a/digital/io-hub/src/apbirthday/plate.cc b/digital/io-hub/src/apbirthday/plate.cc
index fd539b75..3111b749 100644
--- a/digital/io-hub/src/apbirthday/plate.cc
+++ b/digital/io-hub/src/apbirthday/plate.cc
@@ -31,21 +31,6 @@ Plate::Plate ()
is_up = 0;
}
-inline int Plate::get_plate_nb ()
-{
- return nb_plate;
-}
-
-inline void Plate::reset_plate_nb ()
-{
- nb_plate = 0;
-}
-
-inline void Plate::ppp ()
-{
- nb_plate++;
-}
-
inline void Plate::arm_down ()
{
robot->hardware.cherry_plate_down.set (true);
diff --git a/digital/io-hub/src/apbirthday/plate.hh b/digital/io-hub/src/apbirthday/plate.hh
index b4538253..8d913170 100644
--- a/digital/io-hub/src/apbirthday/plate.hh
+++ b/digital/io-hub/src/apbirthday/plate.hh
@@ -44,4 +44,19 @@ class Plate
int nb_plate;
};
+inline int Plate::get_plate_nb ()
+{
+ return nb_plate;
+}
+
+inline void Plate::reset_plate_nb ()
+{
+ nb_plate = 0;
+}
+
+inline void Plate::ppp ()
+{
+ nb_plate++;
+}
+
#endif // plate_hh
diff --git a/digital/io-hub/src/apbirthday/strat.cc b/digital/io-hub/src/apbirthday/strat.cc
index c2ab4d61..d4bec18a 100644
--- a/digital/io-hub/src/apbirthday/strat.cc
+++ b/digital/io-hub/src/apbirthday/strat.cc
@@ -40,10 +40,11 @@ static const vect_t plate_pos[Strat::plate_nb] = {
{ 3000 - 200, 250 },
};
+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 - 60;
+
Strat::Strat ()
{
- for (int i = 0; i < plate_nb; i++)
- plate_visited_[i] = false;
}
void
@@ -52,14 +53,94 @@ Strat::color_init ()
if (team_color)
plate_visited_[1] = false;
else
+ {
plate_visited_[5 + 1] = false;
+ // TODO: plate out of reach.
+ plate_visited_[0] = true;
+ // TODO: this gift is out of reach.
+ robot->gifts.open[3] = true;
+ }
+ // TODO: plate out of reach.
+ plate_visited_[9] = true;
+ // Pre-computed positions.
+ pos_[POS_CANDLES] = pg_cake_pos;
+ for (int i = 0; i < plate_nb; i++)
+ {
+ plate_visited_[i] = false;
+ int sign = i < 5 ? 1 : -1;
+ pos_[POS_PLATE_FIRST + i].x = plate_pos[i].x + sign * plate_app;
+ pos_[POS_PLATE_FIRST + i].y = plate_pos[i].y + sign * 35;
+ }
+ if (team_color)
+ {
+ pos_[POS_CANNON].x = 1462;
+ pos_[POS_CANNON].y = 1256;
+ }
+ else
+ {
+ pos_[POS_CANNON].x = 1642;
+ pos_[POS_CANNON].y = 1276;
+ }
+ pos_[POS_GIFT_1].x = 900;
+ pos_[POS_GIFT_1].y = pg_gifts_distance + BOT_SIZE_SIDE;
+ pos_[POS_GIFT_2].x = 1500;
+ pos_[POS_GIFT_2].y = pg_gifts_distance + BOT_SIZE_SIDE;
+ pos_[POS_GIFT_3].x = 2100;
+ pos_[POS_GIFT_3].y = pg_gifts_distance + BOT_SIZE_SIDE;
+}
+
+void
+Strat::score_pos ()
+{
+ bool escape = false;
+ Position current_pos = robot->asserv.get_position ();
+ robot->path.reset ();
+ robot->obstacles.add_obstacles (robot->path);
+ robot->path.endpoints (current_pos.v, current_pos.v);
+ robot->path.prepare_score (current_pos.v);
+ for (int i = 0; i < POS_NB; i++)
+ {
+ robot->path.endpoints (current_pos.v, pos_[i]);
+ pos_score_[i] = (int16_t) robot->path.get_score (pos_[i]);
+ if (pos_score_[i] == -1)
+ escape = true;
+ }
+ if (escape)
+ {
+ robot->path.prepare_score (current_pos.v, 8);
+ for (int i = 0; i < POS_NB; i++)
+ {
+ if (pos_score_[i] == -1)
+ {
+ robot->path.endpoints (current_pos.v, pos_[i]);
+ pos_score_[i] = (int16_t) robot->path.get_score (pos_[i]);
+ }
+ }
+ }
+}
+
+int
+Strat::score_candles (Position &pos)
+{
+ if (pos_score_[POS_CANDLES] == -1)
+ return -1;
+ // TODO: +1/-1 until candles at ends can be reached.
+ int candles = candles_score (0 + 1, 7 - 1)
+ + candles_score (8 + 1, 19 - 1);
+ if (!candles)
+ return -1;
+ int score = 1000 * candles + 10000 - pos_score_[POS_CANDLES];
+#ifdef TARGET_host
+ robot->hardware.simu_report.draw_number (pg_cake_pos, score);
+#endif
+ pos.v = pg_cake_pos;
+ pos.a = 0;
+ return score;
}
int
Strat::score_plate (Position &pos)
{
- 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 - 60;
int plate = -1, score = -1;
bool above = false, below = false;
bool leave = true;
@@ -96,6 +177,31 @@ Strat::score_plate (Position &pos)
score = 100000;
}
}
+#ifdef TARGET_host
+ if (plate != -1)
+ robot->hardware.simu_report.draw_number (plate_pos[plate], score);
+#endif
+ if (plate == -1 && robot->plate.get_plate_nb () < 2)
+ {
+ for (int i = 0; i < plate_nb; i++)
+ {
+ if (plate_visited_[i] || pos_score_[i] == -1)
+ continue;
+ int tscore = 10000 - pos_score_[i];
+ // Prefer our side.
+ if ((team_color && i >= 5)
+ || (!team_color && i < 5))
+ tscore -= 1500;
+#ifdef TARGET_host
+ robot->hardware.simu_report.draw_number (plate_pos[i], tscore);
+#endif
+ if (tscore > score)
+ {
+ plate = i;
+ score = tscore;
+ }
+ }
+ }
// One plate chosen?
if (plate != -1)
{
@@ -138,6 +244,86 @@ Strat::score_plate (Position &pos)
return score;
}
+int
+Strat::score_cannon (Position &pos)
+{
+ if (pos_score_[POS_CANNON] == -1)
+ return -1;
+ if (!robot->plate.get_plate_nb ())
+ return -1;
+ pos.v = pos_[POS_CANNON];
+ pos.a = G_ANGLE_UF016_DEG (90);
+ int score = 1000 * robot->plate.get_plate_nb ()
+ + 10000 - pos_score_[POS_CANNON];
+#ifdef TARGET_host
+ robot->hardware.simu_report.draw_number (pos.v, score);
+#endif
+ return score;
+}
+
+int
+Strat::score_gifts_sub (Position &pos, int gift_min, int gift_max,
+ int best_score)
+{
+ // Find first and last gifts.
+ int min = -1, max = -1, nb = 0;
+ for (int i = gift_min; i <= gift_max; i++)
+ {
+ if (!robot->gifts.open[i])
+ {
+ if (min == -1) min = i;
+ max = i;
+ nb++;
+ }
+ }
+ if (!nb)
+ return best_score;
+ // Compute positions.
+ int p;
+ if (min <= 1) p = POS_GIFT_1;
+ else if (min == 2) p = POS_GIFT_2;
+ else p = POS_GIFT_3;
+ // Compute score.
+ if (pos_score_[p] == -1)
+ return best_score;
+ int score = 10000 - pos_score_[p] + nb * 1000;
+#ifdef TARGET_host
+ robot->hardware.simu_report.draw_number
+ ((vect_t) { robot->gifts.x[gift_min], pg_gifts_distance }, score);
+#endif
+ // Update if better.
+ if (score > best_score)
+ {
+ pos.v = pos_[p];
+ pos.a = 0;
+ if (min == 0)
+ {
+ gifts_decision_.go_first = true;
+ gifts_decision_.begin_pos.x = robot->gifts.x[0]
+ - bot_gift_arm_x;
+ gifts_decision_.begin_pos.y = pg_gifts_distance + BOT_SIZE_SIDE;
+ }
+ else
+ {
+ gifts_decision_.go_first = false;
+ }
+ gifts_decision_.end_pos.x = robot->gifts.x[max] - bot_gift_arm_x;
+ gifts_decision_.end_pos.y = pg_gifts_distance + BOT_SIZE_SIDE;
+ return score;
+ }
+ else
+ return best_score;
+}
+
+int
+Strat::score_gifts (Position &pos)
+{
+ int best_score = -1;
+ for (int i = 0; i < Gifts::nb; i++)
+ best_score = score_gifts_sub (pos, i, Gifts::nb - 1, best_score);
+ return best_score;
+}
+
Strat::Decision
Strat::decision (Position &pos)
{
@@ -145,6 +331,10 @@ Strat::decision (Position &pos)
int best_score = -1;
Position tpos;
int tscore;
+ score_pos ();
+#ifdef TARGET_host
+ robot->hardware.simu_report.draw_start ();
+#endif
// Plate?
tscore = score_plate (tpos);
if (tscore > best_score)
@@ -153,49 +343,45 @@ Strat::decision (Position &pos)
pos = tpos;
best_decision = PLATE;
}
- // Good score, XXX temp hack.
- if (best_score >= 100000)
+ // Cannon?
+ tscore = score_cannon (tpos);
+ if (tscore > best_score)
+ {
+ best_score = tscore;
+ pos = tpos;
+ best_decision = CANNON;
+ }
+ // Candles?
+ tscore = score_candles (tpos);
+ if (tscore > best_score)
{
- last_decision_ = best_decision;
- return best_decision;
+ best_score = tscore;
+ pos = tpos;
+ best_decision = CANDLES;
}
- // TODO: this is a stub.
- static int step;
- if (step > 2)
- step = 0;
- switch (step++)
+ // Gifts?
+ tscore = score_gifts (tpos);
+ if (tscore > best_score)
{
- case 1:
- gifts_decision_.go_first = true;
- 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 = (vect_t) { 900, pg_gifts_distance + BOT_SIZE_SIDE };
- pos.a = 0;
- last_decision_ = GIFTS;
- return GIFTS;
- default:
- case 0:
- pos.v = pg_cake_pos;
- pos.a = 0;
- last_decision_ = CANDLES;
- candles_tries_ = 0;
- return CANDLES;
- case 2:
- if (team_color)
- pos = (Position) { { 1462, 1256 }, G_ANGLE_UF016_DEG (90) };
- else
- pos = (Position) { { 1642, 1276 }, G_ANGLE_UF016_DEG (90) };
- last_decision_ = CANNON;
- return CANNON;
+ best_score = tscore;
+ pos = tpos;
+ best_decision = GIFTS;
+ }
+#ifdef TARGET_host
+ robot->hardware.simu_report.draw_send ();
+#endif
+ // Good score.
+ if (best_score > 0)
+ {
+ last_decision_ = best_decision;
+ return best_decision;
}
+ else
+ return WAIT;
}
-/// Compute score for candles between first and last.
-static int
-strat_candles_score (int first, int last)
+int
+Strat::candles_score (int first, int last)
{
int score = 0;
Candles::Color other_color = team_color == TEAM_COLOR_RIGHT
@@ -219,11 +405,11 @@ Strat::decision_candles (CandlesDecision &decision, uint16_t robot_angle)
// TODO: +1/-1 until candles at ends can be reached.
int limit, score_forward, score_backward;
limit = top_candle_for_angle (robot_angle, Candles::FAR, 1);
- score_backward = strat_candles_score (0 + 1, limit);
- score_forward = strat_candles_score (limit + 1, 7 - 1);
+ score_backward = candles_score (0 + 1, limit);
+ score_forward = candles_score (limit + 1, 7 - 1);
limit = top_candle_for_angle (robot_angle, Candles::NEAR, 1);
- score_backward += strat_candles_score (8 + 1, limit);
- score_forward += strat_candles_score (limit + 1, 19 - 1);
+ score_backward += candles_score (8 + 1, limit);
+ score_forward += candles_score (limit + 1, 19 - 1);
// Can not choose a direction with an obstacle.
if (score_backward && top_follow_blocking (-1))
score_backward = 0;
@@ -282,5 +468,7 @@ Strat::success ()
{
if (last_decision_ == PLATE)
plate_visited_[plate_decision_.plate] = true;
+ else if (last_decision_ == CANNON)
+ robot->plate.reset_plate_nb ();
}
diff --git a/digital/io-hub/src/apbirthday/strat.hh b/digital/io-hub/src/apbirthday/strat.hh
index 76729bb7..ad41e752 100644
--- a/digital/io-hub/src/apbirthday/strat.hh
+++ b/digital/io-hub/src/apbirthday/strat.hh
@@ -37,6 +37,7 @@ class Strat
PLATE,
CANNON,
GIFTS,
+ WAIT,
};
/// Information on a candle decision.
struct CandlesDecision
@@ -93,9 +94,37 @@ class Strat
/// Report a success.
void success ();
private:
+ /// Pre-compute positions scores.
+ void score_pos ();
+ /// Compute best score for candles.
+ int score_candles (Position &pos);
/// Compute best score for a plate.
int score_plate (Position &pos);
+ /// Compute score for cannon firing.
+ int score_cannon (Position &pos);
+ /// Compute score for a gifts serie.
+ int score_gifts_sub (Position &pos, int gift_min, int gift_max,
+ int best_score);
+ /// Compute score for gifts.
+ int score_gifts (Position &pos);
+ /// Compute score for candles between first and last.
+ int candles_score (int first, int last);
private:
+ /// Pre-computed destinations.
+ enum {
+ POS_CANDLES = 0,
+ POS_PLATE_FIRST = 1,
+ POS_PLATE_LAST = 10,
+ POS_CANNON = 11,
+ POS_GIFT_1 = 12,
+ POS_GIFT_2 = 13,
+ POS_GIFT_3 = 14,
+ POS_NB = 15,
+ };
+ /// Position of pre-computed destinations.
+ vect_t pos_[POS_NB];
+ /// Score of pre-computed destinations.
+ int pos_score_[POS_NB];
/// Last taken decision.
Decision last_decision_;
/// Number of candles tries.
diff --git a/digital/io-hub/src/apbirthday/top.cc b/digital/io-hub/src/apbirthday/top.cc
index c54d97c4..26b8a019 100644
--- a/digital/io-hub/src/apbirthday/top.cc
+++ b/digital/io-hub/src/apbirthday/top.cc
@@ -309,6 +309,8 @@ ANGFSM_STATES (
TOP_INIT,
// Decision state, one stop, one cycle.
TOP_DECISION,
+ // No decision taken, wait and see.
+ TOP_DECISION_WAIT,
// Candles: go to cake, with a normal move.
TOP_CANDLES_GOTO_NORMAL,
// Candles: deploy arm.
@@ -416,6 +418,7 @@ FSM_TRANS_TIMEOUT (TOP_DECISION, 1,
plate, TOP_PLATE_GOTO,
cannon, TOP_CANNON_GOTO,
gifts, TOP_GIFTS_GOTO,
+ wait, TOP_DECISION_WAIT,
none, TOP_START)
{
if (robot->demo)
@@ -441,10 +444,14 @@ FSM_TRANS_TIMEOUT (TOP_DECISION, 1,
robot->move.start (d_pos, Asserv::REVERT_OK);
return FSM_BRANCH (gifts);
default:
- ucoo::assert_unreachable ();
+ return FSM_BRANCH (wait);
}
}
+FSM_TRANS_TIMEOUT (TOP_DECISION_WAIT, 250, TOP_DECISION)
+{
+}
+
///
/// Candles.
///
@@ -627,10 +634,12 @@ FSM_TRANS (TOP_CANNON_GOTO, move_success, TOP_CANNON_FIRING)
FSM_TRANS (TOP_CANNON_GOTO, move_failure, TOP_DECISION)
{
+ robot->strat.failure ();
}
FSM_TRANS (TOP_CANNON_FIRING, cannon_fire_ok, TOP_DECISION)
{
+ robot->strat.success ();
}
///