From a11e81609c018d695faec58849b9f91d35b2b522 Mon Sep 17 00:00:00 2001 From: NĂ©lio Laranjeiro Date: Tue, 20 May 2008 21:39:54 +0200 Subject: Compute the position of the obstacle on the value returned by the sharps. --- digital/io/src/move_cb.c | 37 +++++++++++++++++++++++++++++++++++-- digital/io/src/sharp.c | 34 ++++++++++++++++++++++++++++++++++ digital/io/src/sharp.h | 12 ++++++++++++ 3 files changed, 81 insertions(+), 2 deletions(-) diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c index c55bfa30..a70e61ab 100644 --- a/digital/io/src/move_cb.c +++ b/digital/io/src/move_cb.c @@ -154,6 +154,9 @@ void move_compute_obstacle_position (asserv_position_t cur, move_position_t *obstacle) { + uint8_t i; + uint16_t dist; + /* Convert the angle */ uint32_t angle = cur.a; /* Change angle when going backward */ @@ -161,12 +164,42 @@ move_compute_obstacle_position (asserv_position_t cur, angle += 0x8000; angle = angle << 8; DPRINTF ("We are at (%d ; %d ; %x)\n", cur.x, cur.y, cur.a); + + // Forward moving. + if (asserv_get_moving_direction () == 1) + { + uint16_t dist_computed; + dist = 0xFFFF; + for (i = 0; i < 3; i ++) + { + if (dist > + (dist_computed = sharp_get_distance_mm (sharp_get_raw(i)))) + { + dist = dist_computed; + } + } + } + // Backward moving. + else + { + uint16_t dist_computed; + dist = 0xFFFF; + for (i = 3; i < 5; i ++) + { + if (dist > + (dist_computed = sharp_get_distance_mm (sharp_get_raw(i)))) + { + dist = dist_computed; + } + } + } + /* X */ obstacle->x = cur.x + fixed_mul_f824 (fixed_cos_f824 (angle), - MOVE_OBSTACLE_DISTANCE); + dist); /* Y */ obstacle->y = cur.y + fixed_mul_f824 (fixed_sin_f824 (angle), - MOVE_OBSTACLE_DISTANCE); + dist); DPRINTF ("Computed obstacle (%d ; %d)\n", obstacle->x, obstacle->y); } diff --git a/digital/io/src/sharp.c b/digital/io/src/sharp.c index 47865ed1..4330b546 100644 --- a/digital/io/src/sharp.c +++ b/digital/io/src/sharp.c @@ -29,6 +29,17 @@ #include "modules/adc/adc.h" /* ADC functions */ #include "io.h" +/** Sharp conversion table. + * The first value is the distance in millimeters, the second on the distance + * returned by the sharps. + */ +uint16_t sharp_conv_table[SHARP_NB_ELEMENT_TABLE_CONV][2] = + {{650, 100}, {550, 120}, {450, 140}, + {400, 160}, {350, 180}, {300, 200}, + {250, 220}, {200, 280}, {140, 400}, + {100, 480}, {90, 500}, {80, 500}}; + + /** * Cached array of raw sharp values. */ @@ -198,3 +209,26 @@ sharp_path_obstrued (uint8_t moving_direction) } return 0; } + +/** + * Get the distance of the sharp computed on the value. It does a search in the + * table. + * @param sharp_value the value of the seen by the sharp. + * @return the value in mm of the object seen. + */ +uint16_t +sharp_get_distance_mm (uint16_t sharp_value) +{ + uint8_t index; + + for (index = 1; index < SHARP_NB_ELEMENT_TABLE_CONV; index++) + { + if (sharp_conv_table[index][1] > sharp_value) + { + return sharp_conv_table[index - 1][0]; + } + } + + return 0; +} + diff --git a/digital/io/src/sharp.h b/digital/io/src/sharp.h index 3011e095..537208cd 100644 --- a/digital/io/src/sharp.h +++ b/digital/io/src/sharp.h @@ -65,6 +65,9 @@ */ #define SHARP_BACK_RIGHT 4 +/** Sharps conversion table number of elements. */ +#define SHARP_NB_ELEMENT_TABLE_CONV 12 + /** * Low (0 index) and high (1 index) thresholds for interpreted sharp values. */ @@ -91,6 +94,15 @@ void sharp_update (uint8_t sharp_mask); */ uint16_t sharp_get_raw (uint8_t sharp_id); +/** + * Get the distance of the sharp computed on the value. It does a search in the + * table. + * @param sharp_value the value of the seen by the sharp. + * @return the value in mm of the object seen. + */ +uint16_t +sharp_get_distance_mm (uint16_t sharp_value); + /** * Configure the thresholds of a sharp. * @param sharp_id the sharp id to configure the thresholds. -- cgit v1.2.3