From 155e962088c0e321ae9e7aa8d8904166b3b701f8 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 3 May 2012 00:24:08 +0200 Subject: host/simu, digital/io-hub/src/guybrush: handle new US sensors positions --- digital/io-hub/src/guybrush/radar_defs.c | 21 ++++++++++++++++----- digital/io-hub/src/guybrush/radar_defs.h | 8 ++++---- 2 files changed, 20 insertions(+), 9 deletions(-) (limited to 'digital/io-hub') diff --git a/digital/io-hub/src/guybrush/radar_defs.c b/digital/io-hub/src/guybrush/radar_defs.c index 663b2371..d344f958 100644 --- a/digital/io-hub/src/guybrush/radar_defs.c +++ b/digital/io-hub/src/guybrush/radar_defs.c @@ -30,10 +30,14 @@ /** Define radar configuration. */ struct radar_sensor_t radar_sensors[RADAR_SENSOR_NB] = { - { &usdist_mm[0], { 20, 20 }, G_ANGLE_UF016_DEG (10) }, - { &usdist_mm[1], { 20, -20 }, G_ANGLE_UF016_DEG (-10) }, - { &usdist_mm[2], { -20, -20 }, G_ANGLE_UF016_DEG (180 + 10) }, - { &usdist_mm[3], { -20, 20 }, G_ANGLE_UF016_DEG (180 - 10) }, +#define RADAR_SENSOR_FRONT 0 + { &usdist_mm[0], { 120, 0 }, G_ANGLE_UF016_DEG (0) }, +#define RADAR_SENSOR_LEFT 1 + { &usdist_mm[1], { 120, 160 }, G_ANGLE_UF016_DEG (0) }, +#define RADAR_SENSOR_RIGHT 2 + { &usdist_mm[2], { 120, -160 }, G_ANGLE_UF016_DEG (0) }, +#define RADAR_SENSOR_BACK 3 + { &usdist_mm[3], { -130, 0 }, G_ANGLE_UF016_DEG (180) }, }; /** Define exclusion area (considered as invalid point). */ @@ -41,6 +45,13 @@ uint8_t radar_valid (vect_t p, uint8_t sensor) { return p.x >= RADAR_MARGIN_MM && p.x < PG_WIDTH - RADAR_MARGIN_MM - && p.y >= RADAR_MARGIN_MM && p.y < PG_LENGTH - RADAR_MARGIN_MM; + && p.y >= RADAR_MARGIN_MM && p.y < PG_LENGTH - RADAR_MARGIN_MM + && (sensor == RADAR_SENSOR_FRONT || sensor == RADAR_SENSOR_BACK + || !(p.y >= PG_TOTEM_Y - PG_TOTEM_WIDTH_MM / 2 - RADAR_MARGIN_MM + && p.y < PG_TOTEM_Y + PG_TOTEM_WIDTH_MM / 2 + RADAR_MARGIN_MM + && ((p.x >= PG_TOTEM_LEFT_X - PG_TOTEM_WIDTH_MM / 2 - RADAR_MARGIN_MM + && p.x < PG_TOTEM_LEFT_X + PG_TOTEM_WIDTH_MM / 2 + RADAR_MARGIN_MM) + || (p.x >= PG_TOTEM_RIGHT_X - PG_TOTEM_WIDTH_MM / 2 - RADAR_MARGIN_MM + && p.x < PG_TOTEM_RIGHT_X + PG_TOTEM_WIDTH_MM / 2 + RADAR_MARGIN_MM)))); } diff --git a/digital/io-hub/src/guybrush/radar_defs.h b/digital/io-hub/src/guybrush/radar_defs.h index 5f6a2a73..83f74407 100644 --- a/digital/io-hub/src/guybrush/radar_defs.h +++ b/digital/io-hub/src/guybrush/radar_defs.h @@ -25,7 +25,7 @@ * * }}} */ -#define RADAR_OBSTACLE_EDGE_RADIUS_MM 40 +#define RADAR_OBSTACLE_EDGE_RADIUS_MM 150 #define RADAR_OBSTACLE_RADIUS_MM 150 #define RADAR_STOP_MM 350 #define RADAR_CLEARANCE_MM 100 @@ -34,8 +34,8 @@ #define RADAR_SENSOR_NB 4 #define RADAR_SENSOR_FRONT_FIRST 0 -#define RADAR_SENSOR_FRONT_NB 2 -#define RADAR_SENSOR_BACK_FIRST 2 -#define RADAR_SENSOR_BACK_NB 2 +#define RADAR_SENSOR_FRONT_NB 3 +#define RADAR_SENSOR_BACK_FIRST 3 +#define RADAR_SENSOR_BACK_NB 1 #endif /* radar_defs_h */ -- cgit v1.2.3