summaryrefslogtreecommitdiffhomepage
path: root/digital/beacon
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
parent894e8307503e67088b114cbbe644f58c7c2aa089 (diff)
digital/beacon: add API to compute my robot position from xy
Diffstat (limited to 'digital/beacon')
-rw-r--r--digital/beacon/src/formula.c31
-rw-r--r--digital/beacon/src/formula.h3
-rw-r--r--digital/beacon/src/twi_specific.c7
3 files changed, 40 insertions, 1 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;
+}
+
+
diff --git a/digital/beacon/src/formula.h b/digital/beacon/src/formula.h
index 24b6b2d7..9beacf07 100644
--- a/digital/beacon/src/formula.h
+++ b/digital/beacon/src/formula.h
@@ -69,4 +69,7 @@ TFormulaStatus formula4_compute_coord(coord_s * position, float angle_beacon1, f
/* This function computes coords using formula 5 mode ie for beacon 2 + 3*/
TFormulaStatus formula5_compute_coord(coord_s * position, float angle_beacon2, float angle_beacon3);
+/* This function udpates robot position from given x y */
+TFormulaStatus formula_update_robot_from_xy(robot_s * robot, uint16_t x, uint16_t y);
+
#endif
diff --git a/digital/beacon/src/twi_specific.c b/digital/beacon/src/twi_specific.c
index 9bd35f5e..7e349d16 100644
--- a/digital/beacon/src/twi_specific.c
+++ b/digital/beacon/src/twi_specific.c
@@ -34,6 +34,7 @@
static HAL_AppTimer_t twiTimer;
+extern robot_s my_robot;
/* This function manages the TWI RX/RX transferts */
void twi_task(void)
@@ -76,8 +77,11 @@ void twi_task(void)
jack_update_status(RXbuffer[TWI_RX_JACK_FIELD]);
if(RXbuffer[TWI_RX_JACK_FIELD] == 1)
{
+ /* Get color value from IA and save it*/
color_set_value(RXbuffer[TWI_RX_COLOR_FIELD]);
- formula_update_apb_position((RXbuffer[TWI_RX_X_MSB_FIELD]<<8)+RXbuffer[TWI_RX_X_LSB_FIELD],(RXbuffer[TWI_RX_Y_MSB_FIELD]<<8)+RXbuffer[TWI_RX_Y_LSB_FIELD]);
+
+ /* Update my robot structure with xy from IA */
+ formula_update_robot_from_xy(&my_robot,(RXbuffer[TWI_RX_X_MSB_FIELD]<<8)+RXbuffer[TWI_RX_X_LSB_FIELD],(RXbuffer[TWI_RX_Y_MSB_FIELD]<<8)+RXbuffer[TWI_RX_Y_LSB_FIELD]);
}
}
else
@@ -87,6 +91,7 @@ void twi_task(void)
}
}
+
/* Initialisze specific TWI */
void twi_init_specific(void)
{