summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src
diff options
context:
space:
mode:
authorNicolas Schodet2010-04-13 23:39:31 +0200
committerNicolas Schodet2010-04-13 23:39:31 +0200
commit442a4e4da3699c6ea6390512bf67276c30a9e4ba (patch)
tree282cbfe0de44359fa7537ccf28d8bd123962dbb7 /digital/io/src
parent2d84fb9a861fc12c2af2da35e22dbf30d9dc206e (diff)
digital/io/src: reduce blocking rectangle when near destination, refs #129
Diffstat (limited to 'digital/io/src')
-rw-r--r--digital/io/src/radar.c11
-rw-r--r--digital/io/src/radar.h7
2 files changed, 15 insertions, 3 deletions
diff --git a/digital/io/src/radar.c b/digital/io/src/radar.c
index 0d3611dc..31da7c58 100644
--- a/digital/io/src/radar.c
+++ b/digital/io/src/radar.c
@@ -154,10 +154,17 @@ radar_blocking (const vect_t *robot_pos, const vect_t *dest_pos,
/* If destination is realy near, stop here. */
if (d < RADAR_EPSILON_MM)
return 0;
+ /* If destination is near, use clearance to destination point instead of
+ * stop length. */
+ uint16_t length;
+ if (d < RADAR_STOP_MM - RADAR_CLEARANCE_MM)
+ length = d + BOT_SIZE_FRONT + RADAR_CLEARANCE_MM
+ + RADAR_OBSTACLE_RADIUS_MM;
+ else
+ length = BOT_SIZE_FRONT + RADAR_STOP_MM + RADAR_OBSTACLE_RADIUS_MM;
/* To save divisions, multiply limits by vd (and vdn) length. */
vect_t vdn = vd; vect_normal (&vdn);
- int32_t limit = (uint32_t) d * (BOT_SIZE_FRONT + RADAR_STOP_MM
- + RADAR_OBSTACLE_RADIUS_MM);
+ int32_t limit = (uint32_t) d * length;
int32_t limitn = (uint32_t) d * (BOT_SIZE_SIDE + RADAR_CLEARANCE_MM
+ RADAR_OBSTACLE_RADIUS_MM);
/* Now, look at obstacles. */
diff --git a/digital/io/src/radar.h b/digital/io/src/radar.h
index e486b069..1e541a28 100644
--- a/digital/io/src/radar.h
+++ b/digital/io/src/radar.h
@@ -49,7 +49,12 @@
* OK, more explanations: when moving, a rectangle is placed in front of the
* robot, of length RADAR_STOP_MM and width 2 * (RADAR_CLEARANCE_MM +
* BOT_SIZE_SIDE). If an obstacle is inside this rectangle, it is considered
- * in the way. */
+ * in the way.
+ *
+ * If the destination point is near (< RADAR_STOP_MM - RADAR_CLEARANCE_MM),
+ * this reduce the rectangle length.
+ *
+ * If the destination is really near (< RADAR_EPSILON_MM), ignore all this. */
#define RADAR_CLEARANCE_MM 100
/** Destination distance near enough so that obstacles could be ignored. */