From 442a4e4da3699c6ea6390512bf67276c30a9e4ba Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 13 Apr 2010 23:39:31 +0200 Subject: digital/io/src: reduce blocking rectangle when near destination, refs #129 --- digital/io/src/radar.c | 11 +++++++++-- digital/io/src/radar.h | 7 ++++++- 2 files changed, 15 insertions(+), 3 deletions(-) (limited to 'digital/io/src') 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. */ -- cgit v1.2.3