summaryrefslogtreecommitdiffhomepage
path: root/digital/beacon/src/formula.c
diff options
context:
space:
mode:
authorFlorent Duchon2013-03-28 22:15:41 +0100
committerFlorent Duchon2013-03-28 23:07:35 +0100
commitfb5fc876169d9f9963e9d07d1b5d573a28320866 (patch)
treed0e279dc6c5c6c7857ae0e93b3af3310036e00af /digital/beacon/src/formula.c
parent894e8307503e67088b114cbbe644f58c7c2aa089 (diff)
digital/beacon: add API to compute my robot position from xy
Diffstat (limited to 'digital/beacon/src/formula.c')
-rw-r--r--digital/beacon/src/formula.c31
1 files changed, 31 insertions, 0 deletions
diff --git a/digital/beacon/src/formula.c b/digital/beacon/src/formula.c
index 03f34dd4..f58c1b8b 100644
--- a/digital/beacon/src/formula.c
+++ b/digital/beacon/src/formula.c
@@ -27,6 +27,7 @@
#include "debug_simu.h"
#include "position.h"
#include "formula.h"
+#include "misc.h"
/* This function computes coords using formula 3 mode ie for beacon 1 + 2*/
TFormulaStatus formula3_compute_coord(coord_s * position, float angle_beacon1, float angle_beacon2)
@@ -164,3 +165,33 @@ TFormulaStatus formula_compute_position(int formula, int current_beacon_ID, floa
}
+/* This function udpates robot position from given x y */
+TFormulaStatus formula_update_robot_from_xy(robot_s * robot, uint16_t x, uint16_t y)
+{
+ if(color_get_value() == COLOR_RIGHT)
+ {
+ robot->x = LONGUEUR_TABLE - x;
+ robot->y = LARGEUR_TABLE - y;
+ }
+ else
+ {
+ robot->x = x;
+ robot->y = y;
+ }
+
+ robot->angle[POV_BEACON_1] = atan(robot->x / (LARGEUR_TABLE - robot->y));
+ robot->angle[POV_BEACON_2] = atan(robot->x/robot->y);
+
+ if(y <= LARGEUR_DEMI_TABLE)
+ {
+ robot->angle[POV_BEACON_3] = atan((LONGUEUR_TABLE - robot->x) / (LARGEUR_DEMI_TABLE - robot->y));
+ }
+ else
+ {
+ robot->angle[POV_BEACON_3] = M_PI/2 + atan((robot->y - LARGEUR_DEMI_TABLE) / (LONGUEUR_TABLE-robot->x));
+ }
+
+ return FORMULA_VALID_POSITION;
+}
+
+