From 4b2e09a630c2242197d39920173b27d1968e4f85 Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Sat, 28 May 2011 17:52:26 +0200 Subject: digital/io-hub: add function to get nearest element id and add test_element --- digital/io-hub/src/robospierre/element.c | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) (limited to 'digital/io-hub/src/robospierre/element.c') 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 -- cgit v1.2.3