summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
authorJérôme Jutteau2011-05-28 18:36:26 +0200
committerJérôme Jutteau2011-05-28 19:04:47 +0200
commit4515445795af425fa91920a28ec3f3108f0fa83e (patch)
tree97bcc5af3cd53971ef29ad222c40748454a7f7f1 /digital
parent4b2e09a630c2242197d39920173b27d1968e4f85 (diff)
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.
Diffstat (limited to 'digital')
-rw-r--r--digital/io-hub/src/robospierre/element.c23
-rw-r--r--digital/io-hub/src/robospierre/element.h9
-rw-r--r--digital/io-hub/src/robospierre/test_element.c7
3 files changed, 39 insertions, 0 deletions
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;