From 4515445795af425fa91920a28ec3f3108f0fa83e Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Sat, 28 May 2011 18:36:26 +0200 Subject: digital/io-hub: add function to get position from element id This is useful to indicate an angle when we want to approach an element the green zone. --- digital/io-hub/src/robospierre/element.c | 23 +++++++++++++++++++++++ digital/io-hub/src/robospierre/element.h | 9 +++++++++ digital/io-hub/src/robospierre/test_element.c | 7 +++++++ 3 files changed, 39 insertions(+) (limited to 'digital') diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index 28c72e4d..ebc5874a 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -696,3 +696,26 @@ element_opposed (uint8_t element_id) } return op; } + +position_t +element_get_pos (uint8_t element_id) +{ + element_t e = element_get (element_id); + position_t pos; + pos.v = e.pos; + pos.a = 0; + if (e.attr == (ELEMENT_GREEN |ELEMENT_RIGHT)) + { + /* Set angle to 90° clockwise. */ + pos.a = 0x4000; + /* Remove 400 mm. */ + pos.v.x -= 400; + } + if (e.attr == (ELEMENT_GREEN |ELEMENT_LEFT)) + { + /* Set angle to 270° clockwise. */ + pos.a = 0xc000; + pos.v.x += 400; + } + return pos; +} diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h index fbe3bee5..4655c3c1 100644 --- a/digital/io-hub/src/robospierre/element.h +++ b/digital/io-hub/src/robospierre/element.h @@ -160,4 +160,13 @@ element_intersec_symetric (uint8_t element_id, uint8_t element_type); uint8_t element_nearest_element_id (position_t robot_pos); +/** + Give the position where the robot need to be placed to get an element. + The position correspond to the emplacement of the element with an angle of + zero. If the element is in the green zone, the returned position include + the angle the robot need to set before moving to the element. + */ +position_t +element_get_pos (uint8_t element_id); + #endif /* element_h */ diff --git a/digital/io-hub/src/robospierre/test_element.c b/digital/io-hub/src/robospierre/test_element.c index 30975751..2de92361 100644 --- a/digital/io-hub/src/robospierre/test_element.c +++ b/digital/io-hub/src/robospierre/test_element.c @@ -259,6 +259,7 @@ int main () printf ("i: pick up an element, "); printf ("n: indicate there is no element, "); printf ("m: id of nearest element, "); + printf ("g: get position and angle of desired element,"); printf ("q: quit\n"); printf ("your choice: "); fflush (stdin); @@ -324,6 +325,12 @@ int main () printf ("nearest element id from robot: %u\n", element_nearest_element_id (test_robot_pos_)); break; + case 'g': + printf ("element id: "); + scanf ("%i", &x); + position_t pos = element_get_pos (x); + printf ("x: %u y: %u a: %u\n", pos.v.x, pos.v.y, pos.a); + break; case 'q': exit = 1; break; -- cgit v1.2.3