From c6fb6460e3b71380049326688a1d5ade10dc964e Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 10 Apr 2013 00:42:00 +0200 Subject: digital/io-hub/src/apbirthday: make a better decision on follow direction --- digital/io-hub/src/apbirthday/strat.cc | 53 +++++++++++++++++++++++++++++----- digital/io-hub/src/apbirthday/top.cc | 4 +-- digital/io-hub/src/apbirthday/top.hh | 7 +++++ 3 files changed, 54 insertions(+), 10 deletions(-) (limited to 'digital/io-hub') diff --git a/digital/io-hub/src/apbirthday/strat.cc b/digital/io-hub/src/apbirthday/strat.cc index e93eaeff..6c79dc0c 100644 --- a/digital/io-hub/src/apbirthday/strat.cc +++ b/digital/io-hub/src/apbirthday/strat.cc @@ -22,6 +22,8 @@ // // }}} #include "strat.hh" +#include "robot.hh" +#include "top.hh" Strat::Decision Strat::decision (vect_t &pos) @@ -31,21 +33,58 @@ Strat::decision (vect_t &pos) return CANDLES; } +/// Compute score for candles between first and last. +static int +strat_candles_score (int first, int last) +{ + int score = 0; + Candles::Color other_color = team_color == TEAM_COLOR_RIGHT + ? Candles::BLUE : Candles::RED; + for (int i = first; i != last + 1; i++) + { + if (robot->candles.state[i] != Candles::PUNCHED + && robot->candles.color[i] != other_color) + score++; + } + return score; +} + bool Strat::decision_candles (CandlesDecision &decision, uint16_t robot_angle) { - // TODO: this is a stub. - if (robot_angle > G_ANGLE_UF016_DEG (-90)) + // Make an evaluation of the best direction to follow. + // 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); + 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); + // Can not choose a direction with an obstacle. + if (score_backward && top_follow_blocking (-1)) + score_backward = 0; + if (score_forward && top_follow_blocking (1)) + score_forward = 0; + // Now choose. + if (score_forward == 0 && score_backward == 0) { - decision.dir_sign = -1; - decision.end_angle = G_ANGLE_UF016_DEG (180 + 180. / 6); + return false; } else { - decision.dir_sign = 1; - decision.end_angle = G_ANGLE_UF016_DEG (-180. / 6); + if (score_forward > score_backward) + { + decision.dir_sign = 1; + decision.end_angle = G_ANGLE_UF016_DEG (-180. / 6); + } + else + { + decision.dir_sign = -1; + decision.end_angle = G_ANGLE_UF016_DEG (180 + 180. / 6); + } + return true; } - return true; } void diff --git a/digital/io-hub/src/apbirthday/top.cc b/digital/io-hub/src/apbirthday/top.cc index 0f3f60f5..f5051b0e 100644 --- a/digital/io-hub/src/apbirthday/top.cc +++ b/digital/io-hub/src/apbirthday/top.cc @@ -68,9 +68,7 @@ top_cake_angle_robot () return top_cake_angle (pos.v); } -/// Compute the candle to blow for a given angle, when going in the given -/// direction. -static int +int top_candle_for_angle (uint16_t a, Candles::Floor floor, int dir_sign) { /// Information for each floor. diff --git a/digital/io-hub/src/apbirthday/top.hh b/digital/io-hub/src/apbirthday/top.hh index 09a1b365..229c909b 100644 --- a/digital/io-hub/src/apbirthday/top.hh +++ b/digital/io-hub/src/apbirthday/top.hh @@ -23,6 +23,13 @@ // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. // // }}} +#include "defs.hh" +#include "candles.hh" + +/// Compute the candle to blow for a given angle, when going in the given +/// direction. +int +top_candle_for_angle (uint16_t a, Candles::Floor floor, int dir_sign); /// Test whether there is an obstacle blocking in the given direction. bool -- cgit v1.2.3