From 90f09316ebb9bbb717300b12f9bac8821441e61b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 10 Apr 2013 01:48:15 +0200 Subject: digital/io-hub/src/apbirthday: handle escape movements from cake --- digital/io-hub/src/apbirthday/strat.cc | 4 ++-- digital/io-hub/src/apbirthday/top.cc | 35 +++++++++++++++++++++++++++++++--- 2 files changed, 34 insertions(+), 5 deletions(-) diff --git a/digital/io-hub/src/apbirthday/strat.cc b/digital/io-hub/src/apbirthday/strat.cc index eb4811ea..c755af69 100644 --- a/digital/io-hub/src/apbirthday/strat.cc +++ b/digital/io-hub/src/apbirthday/strat.cc @@ -79,12 +79,12 @@ Strat::decision_candles (CandlesDecision &decision, uint16_t robot_angle) if (score_forward > score_backward) { decision.dir_sign = 1; - decision.end_angle = G_ANGLE_UF016_DEG (-180. / 6); + decision.end_angle = G_ANGLE_UF016_DEG (-180. / 12); } else { decision.dir_sign = -1; - decision.end_angle = G_ANGLE_UF016_DEG (180 + 180. / 6); + decision.end_angle = G_ANGLE_UF016_DEG (180 + 180. / 12); } return true; } diff --git a/digital/io-hub/src/apbirthday/top.cc b/digital/io-hub/src/apbirthday/top.cc index f5051b0e..79b97bc4 100644 --- a/digital/io-hub/src/apbirthday/top.cc +++ b/digital/io-hub/src/apbirthday/top.cc @@ -130,9 +130,38 @@ top_follow_or_leave () } else { - // TODO: take a smart decision to avoid collision. - robot->asserv.stop (); - return FSM_BRANCH (tangent); + Position robot_pos = robot->asserv.get_position (); + int dist = 0; + // If near a border, need to move before turning. + if (robot_pos.v.y > pg_length - BOT_SIZE_RADIUS - 30) + { + if (robot_pos.a < G_ANGLE_UF016_DEG (180)) + dist = - (BOT_SIZE_RADIUS + 30 - BOT_SIZE_FRONT); + else + dist = BOT_SIZE_RADIUS + 30 - BOT_SIZE_BACK; + } + // If near an obstacle, also need to move to undeploy arm. + else if (top_follow_blocking (1)) + { + dist = -100; + } + // Do not use move_distance, it depends too much on current robot + // orientation which is not stable. + uint16_t robot_angle = top_cake_angle (robot_pos.v); + if (dist) + { + vect_t dst; + vect_from_polar_uf016 (&dst, dist, + robot_angle + G_ANGLE_UF016_DEG (90)); + vect_translate (&dst, &robot_pos.v); + robot->asserv.goto_xy (dst, Asserv::REVERT_OK); + return FSM_BRANCH (tangent); + } + else + { + robot->asserv.goto_angle (robot_angle); + return FSM_BRANCH (turn); + } } } -- cgit v1.2.3