summaryrefslogtreecommitdiff
path: root/digital/io
diff options
context:
space:
mode:
authorNélio Laranjeiro2008-05-20 21:39:54 +0200
committerNélio Laranjeiro2008-05-20 21:39:54 +0200
commita11e81609c018d695faec58849b9f91d35b2b522 (patch)
treeb2666637b997ec70b377ff54514f0f0b095e02c5 /digital/io
parentd8d05fbbbd925d37fe846be91715d57ef5c19d21 (diff)
Compute the position of the obstacle on the value returned by the sharps.
Diffstat (limited to 'digital/io')
-rw-r--r--digital/io/src/move_cb.c37
-rw-r--r--digital/io/src/sharp.c34
-rw-r--r--digital/io/src/sharp.h12
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.