summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/robospierre/element.c
diff options
context:
space:
mode:
authorJérôme Jutteau2011-05-28 17:52:26 +0200
committerJérôme Jutteau2011-05-28 17:52:26 +0200
commit4b2e09a630c2242197d39920173b27d1968e4f85 (patch)
tree11f57bbf798b63831149c2acc8165a8a3639e005 /digital/io-hub/src/robospierre/element.c
parentd2e4e1f473154775de48d19a31a97e9604e30905 (diff)
digital/io-hub: add function to get nearest element id and add test_element
Diffstat (limited to 'digital/io-hub/src/robospierre/element.c')
-rw-r--r--digital/io-hub/src/robospierre/element.c21
1 files changed, 17 insertions, 4 deletions
diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c
index 341206b0..28c72e4d 100644
--- a/digital/io-hub/src/robospierre/element.c
+++ b/digital/io-hub/src/robospierre/element.c
@@ -662,11 +662,24 @@ element_down (uint8_t element_id, uint8_t element_type)
}
uint8_t
-element_give_position (position_t pos)
+element_nearest_element_id (position_t robot_pos)
{
- uint8_t e = 0xff;
-
- return e;
+ uint8_t i;
+ uint8_t id = 0;
+ int32_t distance = 4242;
+ element_t e;
+ for (i = 0; i < UTILS_COUNT (element_table); i++)
+ {
+ e = element_get (i);
+ vect_t v = e.pos;
+ int32_t dr = distance_point_point (&v, &robot_pos.v);
+ if (dr < distance)
+ {
+ id = i;
+ distance = dr;
+ }
+ }
+ return id;
}
uint8_t