From 621fd67190770e3f8bace929d5fb9f5633220475 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 13 Apr 2010 00:22:28 +0200 Subject: digital/io/src: add blocking obstacle event, closes #129 --- digital/io/src/radar.c | 37 +++++++++++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) (limited to 'digital/io/src/radar.c') diff --git a/digital/io/src/radar.c b/digital/io/src/radar.c index 93aaa047..0d3611dc 100644 --- a/digital/io/src/radar.c +++ b/digital/io/src/radar.c @@ -141,3 +141,40 @@ radar_update (const position_t *robot_pos, vect_t *obs_pos) return obs_nb; } +uint8_t +radar_blocking (const vect_t *robot_pos, const vect_t *dest_pos, + const vect_t *obs_pos, uint8_t obs_nb) +{ + uint8_t i; + /* Stop here if no obstacle. */ + if (!obs_nb) + return 0; + vect_t vd = *dest_pos; vect_sub (&vd, robot_pos); + uint16_t d = vect_norm (&vd); + /* If destination is realy near, stop here. */ + if (d < RADAR_EPSILON_MM) + return 0; + /* 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 limitn = (uint32_t) d * (BOT_SIZE_SIDE + RADAR_CLEARANCE_MM + + RADAR_OBSTACLE_RADIUS_MM); + /* Now, look at obstacles. */ + for (i = 0; i < obs_nb; i++) + { + /* Vector from robot to obstacle. */ + vect_t vo = obs_pos[i]; vect_sub (&vo, robot_pos); + /* Test if within rectangle (see RADAR_CLEARANCE_MM comment). */ + int32_t dp = vect_dot_product (&vd, &vo); + if (dp < 0 || dp > limit) + continue; + int32_t dpn = vect_dot_product (&vdn, &vo); + if (dpn < -limitn || dpn > limitn) + continue; + /* Else, obstacle is blocking. */ + return 1; + } + return 0; +} + -- cgit v1.2.3