From fb5fc876169d9f9963e9d07d1b5d573a28320866 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Thu, 28 Mar 2013 22:15:41 +0100 Subject: digital/beacon: add API to compute my robot position from xy --- digital/beacon/src/formula.c | 31 +++++++++++++++++++++++++++++++ digital/beacon/src/formula.h | 3 +++ digital/beacon/src/twi_specific.c | 7 ++++++- 3 files changed, 40 insertions(+), 1 deletion(-) 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) { -- cgit v1.2.3