summaryrefslogtreecommitdiff
path: root/digital/beacon/simu/position.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/beacon/simu/position.c')
-rw-r--r--digital/beacon/simu/position.c168
1 files changed, 110 insertions, 58 deletions
diff --git a/digital/beacon/simu/position.c b/digital/beacon/simu/position.c
index 5bc4a8f8..ca98b7b5 100644
--- a/digital/beacon/simu/position.c
+++ b/digital/beacon/simu/position.c
@@ -23,81 +23,133 @@
*
* }}} */
-#include <stdio.h>
#include "position.h"
+#include "debug.h"
+#include "recovery.h"
+#include "update.h"
+#include "formula.h"
+#include "trust.h"
-extern beacon_value_s beacon;
-extern opponent_s opponent;
+beacon_s beacon[MAX_BEACON+1];
+opponent_s opponent[MAX_OBSTACLE+1];
+/* This function is used to initialize all needed structures */
void init_struct(void)
{
- beacon.angle[0]=0;
- beacon.angle[1]=0;
- beacon.angle[2]=0;
- beacon.angle[3]=0;
- beacon.last_updated_beacon = 0;
- beacon.before_last_updated_beacon = 0;
- opponent.x = 0;
- opponent.y = 0;
-}
-
-float degree_to_radian(int value)
-{
- float temp = 0;
- temp = (value * 2 * M_PI) / 360;
- return temp;
+ int i = 0;
+ int j = 0;
+ for(i = 1; i <= MAX_BEACON; i++)
+ {
+ beacon[i].angleNumber = 0;
+ for(j = 1; j <= MAX_OBSTACLE ; j++)
+ {
+ beacon[i].angle[j] = 1;
+ }
+ }
+
+ for(i = 1; i <= MAX_OBSTACLE; i++)
+ {
+ opponent[i].x = 0;
+ opponent[i].y = 0;
+ opponent[i].trust = TRUST_MIN;
+ }
}
-void update_position(int beacon_number,double angle)
+int update_position(int beaconID, int angleID, double angle)
{
+ static int last_ID[2] = {0};
+ int last_valid_id = 0;
int which_formula = 0;
- if(beacon_number == beacon.last_updated_beacon)
- {
- beacon.last_updated_beacon = beacon_number;
- }
- else
+ coord_s temp_position[MAX_TEMP_POSITION];
+ int i = 0;
+
+ int formula_status = 0;
+ int global_status = 0;
+ int update_status = UPDATE_OBSTACLE_NOT_FOUND;
+ int recovery_status = 0;
+
+ DEBUG_POSITION("Update_position with beaconID = %d and angleID = %d and angle = %f\n",(int)beaconID,(int) angleID, (double)angle);
+ DEBUG_POSITION("last_ID[0] = %d last_ID[1] = %d\n",(int)last_ID[0],(int)last_ID[1]);
+
+ /* Calculate which formula need to be used to compute position */
+ for(i = 0 ; i < 2; i++)
{
- beacon.before_last_updated_beacon = beacon.last_updated_beacon;
- beacon.last_updated_beacon = beacon_number;
+ if(beaconID != last_ID[i])
+ {
+ last_valid_id = last_ID[i];
+ }
}
- which_formula = beacon.before_last_updated_beacon + beacon.last_updated_beacon;
-// printf("[position.c] => Update_position beacon_number = %d angle = %f\n",(int)beacon_number,(double)angle);
- beacon.angle[beacon_number] = angle;
+ which_formula = beaconID + last_valid_id;
+
- switch(which_formula)
+ if(last_valid_id != 0)
{
- case 3:
-// printf("[position.c] => Formula 3\r\n");
-// printf("[position.c] => angle[1] = %f angle[2] = %f\n",beacon.angle[1],beacon.angle[2]);
- opponent.x = LARGEUR_TABLE * tan(beacon.angle[2]) * tan(beacon.angle[1]) / (tan(beacon.angle[2])+tan(beacon.angle[1]));
- opponent.y = LARGEUR_TABLE * tan(beacon.angle[1]) / (tan(beacon.angle[2])+tan(beacon.angle[1]));
- break;
- case 4:
-// printf("[position.c] => Formula 4\r\n");
- if(beacon.angle[3] > M_PI/2)
+ if(trust_check_level() == TRUST_LEVEL_OK)
+ {
+ /* Compute all hypotheticals positions and save them into temporary_position tab*/
+ for(i = 1 ; i <= beacon[last_valid_id].angleNumber ; i++)
{
- opponent.y = (LARGEUR_DEMI_TABLE*tan(M_PI - beacon.angle[3]) - LARGEUR_TABLE*tan(beacon.angle[1]) + LONGUEUR_TABLE) / (tan(M_PI - beacon.angle[3]) - tan(beacon.angle[1]));
+ formula_status = formula_compute_position(which_formula,beaconID,beacon[last_valid_id].angle[i],angle,&temp_position[i]);
+ if(formula_status == FORMULA_VALID_POSITION)
+ {
+ update_status += update(&temp_position[i]);
+ if(update_status == UPDATE_OBSTACLE_FOUND)
+ {
+ break;
+ }
+ }
}
- else
+ if(update_status == UPDATE_OBSTACLE_NOT_FOUND)
{
- opponent.y = (LARGEUR_DEMI_TABLE*tan(beacon.angle[3]) + LARGEUR_TABLE*tan(beacon.angle[1])-LONGUEUR_TABLE) / (tan(beacon.angle[1]) + tan(beacon.angle[3]));
+ /* Obstacle not found */
+ trust_decrease();
}
- opponent.x = (LARGEUR_TABLE - opponent.y)*tan(beacon.angle[1]);
- break;
- case 5:
-// printf("[position.c] => formula 5\r\n");
- if(beacon.angle[3] > M_PI/2)
- {
- opponent.y = (LONGUEUR_TABLE + LARGEUR_DEMI_TABLE * tan(M_PI - beacon.angle[3])) / (tan(beacon.angle[2]) + tan(M_PI - beacon.angle[3]));
+ global_status = POSITION_NO_ERROR;
+ }
+ else /* Need Recovery */
+ {
+ /* Compute all hypotheticals positions and save them into temporary_position tab*/
+ for(i = 1 ; i <= beacon[last_valid_id].angleNumber ; i++)
+ {
+ if(beacon[last_valid_id].angle[i] != IGNORE_ANGLE)
+ {
+ formula_status = formula_compute_position(which_formula,beaconID,beacon[last_valid_id].angle[i],angle,&temp_position[i]);
+ if(formula_status == FORMULA_VALID_POSITION)
+ {
+ /* If the angle is not ignored and the computed position is valid, feed the recovery system */
+ recovery_status = recovery(&temp_position[i],&opponent[0]);
+ if((recovery_status == RECOVERY_IN_PROGRESS)||(recovery_status == RECOVERY_FINISHED))
+ {
+ global_status = POSITION_NO_ERROR;
+ break;
+ }
+ else if(recovery_status == RECOVERY_IGNORE_ANGLE_NEXT_TIME)
+ {
+ global_status = POSITION_IGNORE_ANGLE;
+ break;
+ }
+ }
+ }
}
- else
- {
- opponent.y = (LARGEUR_DEMI_TABLE*tan(beacon.angle[3]) - LONGUEUR_TABLE) / (tan(beacon.angle[3]) - tan(beacon.angle[2]));
- }
- opponent.x = tan(beacon.angle[2]) * opponent.y;
- break;
- default:
-// printf("[position.c] => Unknown Formula\r\n");
- break;
+ }
+ }
+
+ /* Save angle context */
+ beacon[beaconID].angleNumber = angleID;
+ if(global_status == POSITION_NO_ERROR)
+ {
+ beacon[beaconID].angle[angleID] = angle;
+ }
+ else /* Angle must be ignored next time */
+ {
+ beacon[beaconID].angle[angleID] = IGNORE_ANGLE;
+ }
+
+ /* Save ID context */
+ if(beaconID != last_valid_id)
+ {
+ last_ID[1] = last_ID[0];
+ last_ID[0] = beaconID;
}
+ return 0;
} \ No newline at end of file