From e9f01fefddae4be19a98161a91bf262c046428ef Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 10 May 2013 07:37:37 +0200 Subject: digital/io-hub/src/apbirthday: replace the strat stub --- digital/io-hub/src/apbirthday/gifts.hh | 3 +- digital/io-hub/src/apbirthday/plate.cc | 15 -- digital/io-hub/src/apbirthday/plate.hh | 15 ++ digital/io-hub/src/apbirthday/strat.cc | 278 +++++++++++++++++++++++++++------ digital/io-hub/src/apbirthday/strat.hh | 29 ++++ digital/io-hub/src/apbirthday/top.cc | 11 +- 6 files changed, 289 insertions(+), 62 deletions(-) (limited to 'digital') 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 (); } /// -- cgit v1.2.3