From 6548d83aa655176ef939817e36cd05d0e6cfaccd Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 23 May 2011 01:23:43 +0200 Subject: digital/{io,io-hub,ai}: add generic radar, add radar to robospierre --- digital/ai/src/move/radar.c | 168 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 168 insertions(+) create mode 100644 digital/ai/src/move/radar.c (limited to 'digital/ai/src/move/radar.c') diff --git a/digital/ai/src/move/radar.c b/digital/ai/src/move/radar.c new file mode 100644 index 00000000..a572afba --- /dev/null +++ b/digital/ai/src/move/radar.c @@ -0,0 +1,168 @@ +/* radar.c */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2010 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "radar.h" + +#include "bot.h" + +#include "modules/math/geometry/geometry.h" +#include "modules/math/geometry/distance.h" +#include "modules/utils/utils.h" + +/** Maximum distance for a sensor reading to be ignored if another sensor is + * nearer. */ +#define RADAR_FAR_MM 250 + +/** Define radar configuration. */ +extern struct radar_sensor_t radar_sensors[RADAR_SENSOR_NB]; + +uint8_t +radar_valid (vect_t p); + +/** Compute the center position from several radars sensors, return 1 if + * any. */ +static uint8_t +radar_hit_center (uint8_t valid[], vect_t hit[], uint8_t sensor_nb, + vect_t *obs_pos) +{ + uint8_t i, hit_nb = 0; + vect_t hit_center = { 0, 0 }; + for (i = 0; i < sensor_nb; i++) + { + if (valid[i]) + { + vect_add (&hit_center, &hit[i]); + hit_nb++; + } + } + if (hit_nb > 1) + vect_scale_f824 (&hit_center, 0x1000000l / hit_nb); + if (hit_nb) + { + *obs_pos = hit_center; + return 1; + } + else + return 0; +} + +uint8_t +radar_update (const position_t *robot_pos, vect_t *obs_pos) +{ + uint8_t i, j; + vect_t ray; + uint8_t obs_nb = 0; + /* Compute hit points for each sensor and eliminate invalid ones. */ + vect_t hit[UTILS_COUNT (radar_sensors)]; + uint8_t valid[UTILS_COUNT (radar_sensors)]; + uint16_t dist_mm[UTILS_COUNT (radar_sensors)]; + for (i = 0; i < UTILS_COUNT (radar_sensors); i++) + { + dist_mm[i] = *radar_sensors[i].dist_mm; + if (dist_mm[i] != 0xffff) + { + hit[i] = radar_sensors[i].pos; + vect_rotate_uf016 (&hit[i], robot_pos->a); + vect_translate (&hit[i], &robot_pos->v); + vect_from_polar_uf016 (&ray, dist_mm[i], + robot_pos->a + radar_sensors[i].a); + vect_translate (&hit[i], &ray); + valid[i] = radar_valid (hit[i]); + vect_from_polar_uf016 (&ray, RADAR_OBSTACLE_EDGE_RADIUS_MM, + robot_pos->a + radar_sensors[i].a); + vect_translate (&hit[i], &ray); + } + else + valid[i] = 0; + } + /* Ignore sensor results too far from other sensors. */ + for (i = 0; i < UTILS_COUNT (radar_sensors) - 1; i++) + { + for (j = i + 1; valid[i] && j < UTILS_COUNT (radar_sensors); j++) + { + if (valid[j]) + { + if (dist_mm[i] + RADAR_FAR_MM < dist_mm[j]) + valid[j] = 0; + else if (dist_mm[j] + RADAR_FAR_MM < dist_mm[i]) + valid[i] = 0; + } + } + } + /* Specific treatment about sensor topology. */ + obs_nb += radar_hit_center (valid + RADAR_SENSOR_FRONT_FIRST, + hit + RADAR_SENSOR_FRONT_FIRST, + RADAR_SENSOR_FRONT_NB, &obs_pos[obs_nb]); + obs_nb += radar_hit_center (valid + RADAR_SENSOR_BACK_FIRST, + hit + RADAR_SENSOR_BACK_FIRST, + RADAR_SENSOR_BACK_NB, &obs_pos[obs_nb]); + /* Done. */ + 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; + /* If destination is near, use clearance to destination point instead of + * stop length. */ + vect_t t; + if (d < RADAR_STOP_MM) + t = *dest_pos; + else + { + vect_scale_f824 (&vd, (1ll << 24) / d * RADAR_STOP_MM); + t = *robot_pos; + vect_translate (&t, &vd); + } + /* 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); + /* Ignore if in our back. */ + int32_t dp = vect_dot_product (&vd, &vo); + if (dp < 0) + continue; + /* Check distance. */ + int16_t od = distance_segment_point (robot_pos, &t, &obs_pos[i]); + if (od > BOT_SIZE_SIDE + RADAR_CLEARANCE_MM / 2 + + RADAR_OBSTACLE_RADIUS_MM) + continue; + /* Else, obstacle is blocking. */ + return 1; + } + return 0; +} + -- cgit v1.2.3