From f6d463f8f2e30cd4a633a7aab36f17ce56e513f5 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 9 May 2013 04:54:57 +0200 Subject: digital/io-hub/src/apbirthday: change initial plate taking --- digital/io-hub/src/apbirthday/init_defs.hh | 2 +- digital/io-hub/src/apbirthday/strat.cc | 163 +++++++++++++++++++++++++---- digital/io-hub/src/apbirthday/strat.hh | 22 ++++ digital/io-hub/src/apbirthday/top.cc | 25 ++++- 4 files changed, 191 insertions(+), 21 deletions(-) (limited to 'digital/io-hub/src/apbirthday') 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) { } -- cgit v1.2.3