summaryrefslogtreecommitdiffhomepage
path: root/digital/io-hub/src/apbirthday
diff options
context:
space:
mode:
authorNicolas Schodet2013-05-09 04:54:57 +0200
committerNicolas Schodet2013-05-09 04:54:57 +0200
commitf6d463f8f2e30cd4a633a7aab36f17ce56e513f5 (patch)
tree79e5bd5a0d5a56e7e764425e75af9ad7625202b4 /digital/io-hub/src/apbirthday
parente64c4c67b78a6adb26f592e7c5015f7a775b06d6 (diff)
digital/io-hub/src/apbirthday: change initial plate taking
Diffstat (limited to 'digital/io-hub/src/apbirthday')
-rw-r--r--digital/io-hub/src/apbirthday/init_defs.hh2
-rw-r--r--digital/io-hub/src/apbirthday/strat.cc163
-rw-r--r--digital/io-hub/src/apbirthday/strat.hh22
-rw-r--r--digital/io-hub/src/apbirthday/top.cc25
4 files changed, 191 insertions, 21 deletions
diff --git a/digital/io-hub/src/apbirthday/init_defs.hh b/digital/io-hub/src/apbirthday/init_defs.hh
index a2468f71..f634c18c 100644
--- a/digital/io-hub/src/apbirthday/init_defs.hh
+++ b/digital/io-hub/src/apbirthday/init_defs.hh
@@ -29,7 +29,7 @@
Asserv::FORWARD, pg_x (650), pg_y (pg_length - BOT_FRONT_CONTACT_DIST), \
pg_a_deg (90 + BOT_BACK_CONTACT_ANGLE_ERROR_DEG)
/// Parameters to go away from the first wall.
-#define INIT_FIRST_WALL_AWAY (-(1000 - BOT_FRONT_CONTACT_DIST))
+#define INIT_FIRST_WALL_AWAY (-(600 - BOT_FRONT_CONTACT_DIST))
/// Parameter to face the second wall.
#define INIT_SECOND_WALL_ANGLE pg_a_deg (180)
/// Parameters to push the second wall.
diff --git a/digital/io-hub/src/apbirthday/strat.cc b/digital/io-hub/src/apbirthday/strat.cc
index ea1c1ff6..0f21d5c0 100644
--- a/digital/io-hub/src/apbirthday/strat.cc
+++ b/digital/io-hub/src/apbirthday/strat.cc
@@ -27,30 +27,145 @@
#include "debug.host.hh"
#include "bot.hh"
+static const vect_t plate_pos[Strat::plate_nb] = {
+ { 200, 1750 },
+ { 200, 1400 },
+ { 200, 1000 },
+ { 200, 600 },
+ { 200, 250 },
+ { 3000 - 200, 1750 },
+ { 3000 - 200, 1400 },
+ { 3000 - 200, 1000 },
+ { 3000 - 200, 600 },
+ { 3000 - 200, 250 },
+};
+
+Strat::Strat ()
+{
+ for (int i = 0; i < plate_nb; i++)
+ plate_visited_[i] = false;
+}
+
+void
+Strat::color_init ()
+{
+ if (team_color)
+ plate_visited_[1] = false;
+ else
+ plate_visited_[5 + 1] = false;
+}
+
+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 - 40;
+ int plate = -1, score = -1;
+ bool above = false, below = false;
+ bool leave = true;
+ // Important start points.
+ if (team_color)
+ {
+ if (!plate_visited_[2])
+ {
+ plate = 2;
+ below = true;
+ score = 100000;
+ leave = false;
+ }
+ else if (!plate_visited_[0])
+ {
+ plate = 0;
+ above = true;
+ score = 100000;
+ }
+ }
+ else
+ {
+ if (!plate_visited_[0 + 5])
+ {
+ plate = 0 + 5;
+ above = true;
+ score = 100000;
+ leave = false;
+ }
+ else if (!plate_visited_[2 + 5])
+ {
+ plate = 2 + 5;
+ below = true;
+ score = 100000;
+ }
+ }
+ // One plate chosen?
+ if (plate != -1)
+ {
+ if (above)
+ {
+ pos.v.x = plate_pos[plate].x + 35;
+ pos.v.y = plate_pos[plate].y - plate_app;
+ pos.a = G_ANGLE_UF016_DEG (-90);
+ plate_decision_.loading_pos.x = plate_pos[plate].x + 35;
+ plate_decision_.loading_pos.y = plate_pos[plate].y - plate_load;
+ }
+ else if (below)
+ {
+ pos.v.x = plate_pos[plate].x - 35;
+ pos.v.y = plate_pos[plate].y + plate_app;
+ pos.a = G_ANGLE_UF016_DEG (90);
+ plate_decision_.loading_pos.x = plate_pos[plate].x - 35;
+ plate_decision_.loading_pos.y = plate_pos[plate].y + plate_load;
+ }
+ else if (plate < 5)
+ {
+ pos.v.x = plate_pos[plate].x + plate_app;
+ pos.v.y = plate_pos[plate].y + 35;
+ pos.a = G_ANGLE_UF016_DEG (0);
+ plate_decision_.loading_pos.x = plate_pos[plate].x + plate_load;
+ plate_decision_.loading_pos.y = plate_pos[plate].y + 35;
+ }
+ else
+ {
+ pos.v.x = plate_pos[plate].x - plate_app;
+ pos.v.y = plate_pos[plate].y - 35;
+ pos.a = G_ANGLE_UF016_DEG (180);
+ plate_decision_.loading_pos.x = plate_pos[plate].x - plate_load;
+ plate_decision_.loading_pos.y = plate_pos[plate].y - 35;
+ }
+ plate_decision_.approaching_pos = pos;
+ plate_decision_.plate = plate;
+ plate_decision_.leave = leave;
+ }
+ return score;
+}
+
Strat::Decision
Strat::decision (Position &pos)
{
+ Decision best_decision;
+ int best_score = -1;
+ Position tpos;
+ int tscore;
+ // Plate?
+ tscore = score_plate (tpos);
+ if (tscore > best_score)
+ {
+ best_score = tscore;
+ pos = tpos;
+ best_decision = PLATE;
+ }
+ // Good score, XXX temp hack.
+ if (best_score >= 100000)
+ {
+ last_decision_ = best_decision;
+ return best_decision;
+ }
// TODO: this is a stub.
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;
- if (step > 4)
- step = 2;
+ if (step > 2)
+ step = 0;
switch (step++)
{
- case 0:
- pos = pg_position_deg (200, 600 + plate_app, 90);
- pos.v.x -= 35;
- plate_decision_.loading_pos = pg_vect (200, 600 + plate_load);
- plate_decision_.loading_pos.x -= 35;
- return PLATE;
case 1:
- pos = pg_position_deg (200, 1400 - plate_app, -90);
- pos.v.x += 35;
- plate_decision_.loading_pos = pg_vect (200, 1400 - plate_load);
- plate_decision_.loading_pos.x += 35;
- return PLATE;
- case 3:
gifts_decision_.go_first = true;
gifts_decision_.begin_pos = (vect_t) { 600, pg_gifts_distance
+ BOT_SIZE_SIDE };
@@ -59,17 +174,20 @@ Strat::decision (Position &pos)
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 2:
+ case 0:
pos.v = pg_cake_pos;
pos.a = 0;
+ last_decision_ = CANDLES;
return CANDLES;
- case 4:
+ case 2:
if (team_color)
pos = pg_position_deg (1500, 1200, 90);
else
pos = pg_position_deg (1500 - 200, 1200, 90);
+ last_decision_ = CANNON;
return CANNON;
}
}
@@ -151,5 +269,14 @@ Strat::decision_gifts (GiftsDecision &decision)
void
Strat::failure ()
{
+ if (last_decision_ == PLATE)
+ plate_visited_[plate_decision_.plate] = true;
+}
+
+void
+Strat::success ()
+{
+ if (last_decision_ == PLATE)
+ plate_visited_[plate_decision_.plate] = true;
}
diff --git a/digital/io-hub/src/apbirthday/strat.hh b/digital/io-hub/src/apbirthday/strat.hh
index 3e6c7264..1f4365c6 100644
--- a/digital/io-hub/src/apbirthday/strat.hh
+++ b/digital/io-hub/src/apbirthday/strat.hh
@@ -49,6 +49,13 @@ class Strat
/// Information on a plate decision.
struct PlateDecision
{
+ /// Chosen plate.
+ int plate;
+ /// Leave movement after plate is taken.
+ bool leave;
+ /// 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;
@@ -65,7 +72,13 @@ class Strat
/// Movement direction.
Asserv::DirectionConsign dir;
};
+ /// Number of plates.
+ static const int plate_nb = 10;
public:
+ /// Constructor.
+ Strat ();
+ /// Color specific initialisation.
+ void color_init ();
/// Return new decision and associated position.
Decision decision (Position &pos);
/// Take a decision related to candles, return false to give up candles.
@@ -77,11 +90,20 @@ class Strat
void decision_gifts (GiftsDecision &decision);
/// Report a failure to apply the previous decision.
void failure ();
+ /// Report a success.
+ void success ();
private:
+ /// Compute best score for a plate.
+ int score_plate (Position &pos);
+ private:
+ /// Last taken decision.
+ Decision last_decision_;
/// Last plate decision.
PlateDecision plate_decision_;
/// Last gifts decision.
GiftsDecision gifts_decision_;
+ /// Plate visited?
+ bool plate_visited_[plate_nb];
};
#endif // strat_hh
diff --git a/digital/io-hub/src/apbirthday/top.cc b/digital/io-hub/src/apbirthday/top.cc
index 7372576b..b8c04b94 100644
--- a/digital/io-hub/src/apbirthday/top.cc
+++ b/digital/io-hub/src/apbirthday/top.cc
@@ -307,6 +307,8 @@ ANGFSM_STATES (
TOP_PLATE_LOADING,
// Plate: drop plate.
TOP_PLATE_DROPING,
+ // Plate: leaving to turn freely.
+ TOP_PLATE_LEAVE,
// Cannon: go to fire position.
TOP_CANNON_GOTO,
// Cannon: Firing.
@@ -351,6 +353,7 @@ FSM_TRANS (TOP_INIT_ACTUATORS, init_done, TOP_INIT)
{
// Color dependent init can go here.
robot->gifts.compute_pos ();
+ robot->strat.color_init ();
}
FSM_TRANS (TOP_INIT, init_start_round, TOP_DECISION)
@@ -519,16 +522,34 @@ FSM_TRANS (TOP_PLATE_APPROACH, move_failure, TOP_DECISION)
FSM_TRANS (TOP_PLATE_APPROACH, top_plate_present, TOP_PLATE_LOADING)
{
+ robot->strat.success ();
robot->move.stop ();
ANGFSM_HANDLE (AI, plate_take);
}
FSM_TRANS (TOP_PLATE_LOADING, plate_taken, TOP_PLATE_DROPING)
{
- ANGFSM_HANDLE (AI, plate_drop);
+ ANGFSM_HANDLE (AI, plate_drop);
}
-FSM_TRANS (TOP_PLATE_DROPING, plate_droped, TOP_DECISION)
+FSM_TRANS (TOP_PLATE_DROPING, plate_droped,
+ leave, TOP_PLATE_LEAVE,
+ end, TOP_DECISION)
+{
+ if (top.plate.leave)
+ {
+ robot->move.start (top.plate.approaching_pos.v, Asserv::REVERT_OK);
+ return FSM_BRANCH (leave);
+ }
+ else
+ return FSM_BRANCH (end);
+}
+
+FSM_TRANS (TOP_PLATE_LEAVE, move_success, TOP_DECISION)
+{
+}
+
+FSM_TRANS (TOP_PLATE_LEAVE, move_failure, TOP_DECISION)
{
}