summaryrefslogtreecommitdiffhomepage
path: root/digital/io-hub/src
diff options
context:
space:
mode:
authorNicolas Schodet2012-05-16 17:51:46 +0200
committerNicolas Schodet2012-05-16 21:01:45 +0200
commitb2dbe0ad84501849aa016f9d9cf5b9444f83822e (patch)
tree636082289640ad6e7b6767a3c22704dc1f736a3d /digital/io-hub/src
parentf073e6ad9d968bd0ae02fceb848f7473e4bb81f8 (diff)
digital/io-hub/src/guybrush: do not go back if not possible
Diffstat (limited to 'digital/io-hub/src')
-rw-r--r--digital/io-hub/src/guybrush/bot.h3
-rw-r--r--digital/io-hub/src/guybrush/move.c38
2 files changed, 36 insertions, 5 deletions
diff --git a/digital/io-hub/src/guybrush/bot.h b/digital/io-hub/src/guybrush/bot.h
index aef18b3b..32e7ff64 100644
--- a/digital/io-hub/src/guybrush/bot.h
+++ b/digital/io-hub/src/guybrush/bot.h
@@ -44,6 +44,9 @@
#define BOT_SIZE_SIDE 172
/** Maximum distance from the robot base center to one of its edge. */
#define BOT_SIZE_RADIUS 230
+/** Maximum distance from the robot base center to one of its edge, at the
+ * back. */
+#define BOT_SIZE_BACK_RADIUS 180
/** Distance between the front contact point and the robot center. */
#define BOT_BACK_CONTACT_DIST_MM BOT_SIZE_BACK
diff --git a/digital/io-hub/src/guybrush/move.c b/digital/io-hub/src/guybrush/move.c
index 42e78ccc..327caed6 100644
--- a/digital/io-hub/src/guybrush/move.c
+++ b/digital/io-hub/src/guybrush/move.c
@@ -39,6 +39,8 @@
#include <math.h>
+#include "playground_2012.h"
+
/** Number of tries to reach destination. */
#define MOVE_TRY_AGAIN 1
@@ -378,24 +380,50 @@ FSM_TRANS (MOVE_MOVING, robot_move_success,
//return FSM_NEXT (MOVE_MOVING, robot_move_success, no_path_found);
}
+/** Test if a point is ok to move back. */
+static uint8_t
+move_test_point (vect_t p, int16_t margin)
+{
+ return p.x >= margin && p.x < PG_WIDTH - margin
+ && p.y >= margin && p.y < PG_LENGTH - margin
+ && !(p.y >= PG_TOTEM_Y - PG_TOTEM_WIDTH_MM / 2 - margin
+ && p.y < PG_TOTEM_Y + PG_TOTEM_WIDTH_MM / 2 + margin
+ && p.x >= PG_TOTEM_LEFT_X - PG_TOTEM_WIDTH_MM / 2 - margin
+ && p.x < PG_TOTEM_RIGHT_X + PG_TOTEM_WIDTH_MM / 2 + margin);
+}
+
static void
move_moving_backward_to_turn_freely (void)
{
move_data.final_move = 0;
+ int16_t dist, back_dist, margin;
+ if (asserv_get_last_moving_direction () == DIRECTION_FORWARD)
+ {
+ dist = BOT_SIZE_FRONT + MOVE_REAL_OBSTACLE_RADIUS;
+ back_dist = -300;
+ margin = BOT_SIZE_BACK_RADIUS;
+ }
+ else
+ {
+ dist = -(BOT_SIZE_FRONT + MOVE_REAL_OBSTACLE_RADIUS);
+ back_dist = 300;
+ margin = BOT_SIZE_RADIUS;
+ }
/* Assume there is an obstacle in front of the robot. */
position_t robot_pos;
asserv_get_position (&robot_pos);
vect_t obstacle_pos;
- int16_t dist = asserv_get_last_moving_direction () == DIRECTION_FORWARD
- ? BOT_SIZE_FRONT + MOVE_REAL_OBSTACLE_RADIUS
- : -(BOT_SIZE_BACK + MOVE_REAL_OBSTACLE_RADIUS);
vect_from_polar_uf016 (&obstacle_pos, dist, robot_pos.a);
vect_translate (&obstacle_pos, &robot_pos.v);
path_obstacle (0, obstacle_pos, MOVE_OBSTACLE_RADIUS, 0,
MOVE_OBSTACLE_VALIDITY);
/* Move backward to turn freely. */
- asserv_move_linearly (asserv_get_last_moving_direction ()
- == DIRECTION_FORWARD ? -300 : 300);
+ vect_t back_pos;
+ vect_from_polar_uf016 (&back_pos, back_dist, robot_pos.a);
+ vect_translate (&back_pos, &robot_pos.v);
+ if (!move_test_point (back_pos, margin))
+ back_dist /= 8;
+ asserv_move_linearly (back_dist);
}
FSM_TRANS (MOVE_MOVING,