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/top.cc | 35 ++++++++++++++++++++++++++++++++--- 1 file changed, 32 insertions(+), 3 deletions(-) (limited to 'digital/io-hub/src/apbirthday/top.cc') 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