summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2013-04-10 01:48:15 +0200
committerNicolas Schodet2013-04-10 01:48:15 +0200
commit90f09316ebb9bbb717300b12f9bac8821441e61b (patch)
tree70b0395157faaca3fafe3f0550b2bce127b62e77 /digital
parent20c4b6371c02bdb52e93f101d6983c4bf6dee5b5 (diff)
digital/io-hub/src/apbirthday: handle escape movements from cake
Diffstat (limited to 'digital')
-rw-r--r--digital/io-hub/src/apbirthday/strat.cc4
-rw-r--r--digital/io-hub/src/apbirthday/top.cc35
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);
+ }
}
}