summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorFlorent Duchon2012-03-04 01:32:10 +0100
committerNicolas Schodet2012-03-07 23:24:10 +0100
commit1355d019cafedbb3d6b191156b80a857c6a618d1 (patch)
treea8e7a48ba4d758b6483fcebe3f1e37a1673b3316
parent1c02b940cad001583dae24cd39a9f55f62120b5e (diff)
digital/beacon/simu: beacon simulator v2 -> Multi Obstacles management
-rw-r--r--digital/beacon/simu/Makefile3
-rw-r--r--digital/beacon/simu/beacon.c108
-rw-r--r--digital/beacon/simu/debug.h74
-rw-r--r--digital/beacon/simu/formula.c166
-rw-r--r--digital/beacon/simu/formula.h72
-rw-r--r--digital/beacon/simu/position.c168
-rw-r--r--digital/beacon/simu/position.h49
-rw-r--r--digital/beacon/simu/recovery.c142
-rw-r--r--digital/beacon/simu/recovery.h45
-rw-r--r--digital/beacon/simu/simulator.py218
-rw-r--r--digital/beacon/simu/trust.c73
-rw-r--r--digital/beacon/simu/trust.h50
-rw-r--r--digital/beacon/simu/update.c63
-rw-r--r--digital/beacon/simu/update.h (renamed from digital/beacon/simu/avrconfig.h)31
14 files changed, 1050 insertions, 212 deletions
diff --git a/digital/beacon/simu/Makefile b/digital/beacon/simu/Makefile
index 132f0b2e..a9b28299 100644
--- a/digital/beacon/simu/Makefile
+++ b/digital/beacon/simu/Makefile
@@ -1,8 +1,7 @@
BASE = ../../avr
HOST_PROGS = beacon
-beacon_SOURCES = beacon.c position.c
+beacon_SOURCES = beacon.c position.c formula.c recovery.c update.c trust.c
MODULES = math/fixed utils
-CONFIGFILE = avrconfig.h
# atmega8, atmega8535, atmega128...
AVR_MCU = atmega128
# -O2 : speed
diff --git a/digital/beacon/simu/beacon.c b/digital/beacon/simu/beacon.c
index 5ea8a263..73fc1c8a 100644
--- a/digital/beacon/simu/beacon.c
+++ b/digital/beacon/simu/beacon.c
@@ -22,85 +22,55 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "position.h"
-#include "common.h"
/* Globals Declaration */
-beacon_value_s beacon;
-opponent_s opponent;
-
-void syntax (void)
-{
- fprintf (stderr,
- "beacon sources_number source1 angle1 ... sourceX angleX\n"
- "example: beacon 2,1,3.2,3,0.82\n"
- );
- exit (1);
-}
-
-void read_tab (const char *s, double *tab, int n)
-{
- assert (n > 0);
- while (n)
- {
- char *sp;
- double v;
- v = strtod (s, &sp);
- if (sp == s)
- syntax ();
- if ((n > 1 && *sp != ',')
- || (n == 1 && *sp != '\0'))
- syntax ();
- s = sp + 1;
- *tab++ = v;
- n--;
- }
-}
+extern opponent_s opponent[MAX_OBSTACLE];
int main (int argc, char **argv)
{
- if (argc < 8 || argc > 10)
- syntax ();
-
- double input[7] = {0};
+ char saisie[30];
+ char ret = 0;
+ int id = 0;
+ float angle = 0;
+ int angle_id = 0;
+ int i = 0;
/* Init global structures */
- init_struct();
-
-
- /* Read Beacon num */
- read_tab (argv[2], input, 1);
-
- /* Read Beacon angle */
- read_tab (argv[3], input+1, 1);
-
- /* Read Beacon num */
- read_tab (argv[4], input+2, 1);
-
- /* Read Beacon angle */
- read_tab (argv[5], input+3, 1);
-
- /* Read Activate filter */
- read_tab (argv[6],input+4, 1);
-
- /* Read last x */
- read_tab (argv[7],input+5, 1);
-
- /* Read last y */
- read_tab (argv[8],input+6, 1);
-
-// printf("Balise %d --- Angle %f\n",(int)input[0],input[1]);
-// printf("Balise %d --- Angle %f\n",(int)input[2],input[3]);
+ init_struct();
- /* Compute position */
- update_position(input[0],input[1]);
- update_position(input[2],input[3]);
-
- /* Return position to the simulator */
- printf("%d\n",(int)opponent.x);
- printf("%d\n",(int)opponent.y);
-
+ while(1)
+ {
+ ret = fgets (saisie, sizeof saisie, stdin);
+ if(ret != NULL)
+ {
+ id = strtol (saisie, NULL, 10);
+ }
+ ret = fgets (saisie, sizeof saisie, stdin);
+ if(ret != NULL)
+ {
+ angle = strtod (saisie, NULL);
+ }
+ ret = fgets (saisie, sizeof saisie, stdin);
+ if(ret != NULL)
+ {
+ angle_id = strtod (saisie, NULL);
+ }
+
+ update_position(id,angle_id,angle);
+
+ /* Return position to the simulator */
+ for(i=1;i<=MAX_OBSTACLE;i++)
+ {
+ printf("%d\n",(int)opponent[i].x);
+ printf("%d\n",(int)opponent[i].y);
+ printf("%d\n",(int)opponent[i].trust);
+ }
+ fflush(stdout);
+ }
return 0;
}
+
diff --git a/digital/beacon/simu/debug.h b/digital/beacon/simu/debug.h
new file mode 100644
index 00000000..ab166fe9
--- /dev/null
+++ b/digital/beacon/simu/debug.h
@@ -0,0 +1,74 @@
+/* debug.h */
+/* Macro for debug traces. {{{
+ *
+ * Copyright (C) 2011 Florent Duchon
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+#ifndef _DEBUG_H
+#define _DEBUG_H
+
+#include <stdio.h>
+
+// #define DEBUG_POSITION_ENABLE
+// #define DEBUG_UPDATE_ENABLE
+// #define DEBUG_RECOVERY_ENABLE
+// #define DEBUG_FORMULA_INFO_ENABLE
+// #define DEBUG_FORMULA_ERROR_ENABLE
+// #define DEBUG_TRUST_ENABLE
+
+
+#ifdef DEBUG_POSITION_ENABLE
+ #define DEBUG_POSITION(f, s...) fprintf(stderr,"[position.c:%d] => " f,__LINE__, ## s)
+#else
+ #define DEBUG_POSITION(f,s...) ((void)0)
+#endif
+
+#ifdef DEBUG_UPDATE_ENABLE
+ #define DEBUG_UPDATE(f, s...) fprintf(stderr,"[update.c:%d] => " f,__LINE__, ## s)
+#else
+ #define DEBUG_UPDATE(f,s...) ((void)0)
+#endif
+
+#ifdef DEBUG_RECOVERY_ENABLE
+ #define DEBUG_RECOVERY(f, s...) fprintf(stderr,"[recovery.c:%d] => " f,__LINE__, ## s)
+#else
+ #define DEBUG_RECOVERY(f,s...) ((void)0)
+#endif
+
+#ifdef DEBUG_FORMULA_INFO_ENABLE
+ #define DEBUG_FORMULA_INFO(f, s...) fprintf(stderr,"[formula.c:%d] => " f,__LINE__, ## s)
+#else
+ #define DEBUG_FORMULA_INFO(f,s...) ((void)0)
+#endif
+
+#ifdef DEBUG_FORMULA_ERROR_ENABLE
+ #define DEBUG_FORMULA_ERROR(f, s...) fprintf(stderr,"[formula.c:%d] => " f,__LINE__, ## s)
+#else
+ #define DEBUG_FORMULA_ERROR(f,s...) ((void)0)
+#endif
+
+#ifdef DEBUG_TRUST_ENABLE
+ #define DEBUG_TRUST(f, s...) fprintf(stderr,"[trust.c:%d] => " f,__LINE__, ## s)
+#else
+ #define DEBUG_TRUST(f,s...) ((void)0)
+#endif
+
+#endif
diff --git a/digital/beacon/simu/formula.c b/digital/beacon/simu/formula.c
new file mode 100644
index 00000000..8391b8f8
--- /dev/null
+++ b/digital/beacon/simu/formula.c
@@ -0,0 +1,166 @@
+/* formula.c */
+/* Beacon triangulation algorithms. {{{
+ *
+ * Copyright (C) 2011 Florent Duchon
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+
+#include <math.h>
+#include "debug.h"
+#include "position.h"
+#include "formula.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)
+{
+ DEBUG_FORMULA_INFO("Formula 3 : angle_beacon1 = %f angle_beacon2 = %f\n",angle_beacon1,angle_beacon2);
+ if((angle_beacon1 < BLIND_ZONE_BEACON1_ANGLE12_MAX) && (angle_beacon2 < BLIND_ZONE_BEACON2_ANGLE12_MAX))
+ {
+ DEBUG_FORMULA_ERROR("BLIND Zone detected\n");
+ return FORMULA_BLIND_ZONE_DETECTED;
+ }
+ else
+ {
+ position->x = LARGEUR_TABLE * tan(angle_beacon2) * tan(angle_beacon1) / (tan(angle_beacon2)+tan(angle_beacon1));
+ position->y = LARGEUR_TABLE * tan(angle_beacon1) / (tan(angle_beacon2)+tan(angle_beacon1));
+
+ if( (position->x < 0) || (position->x > LONGUEUR_TABLE) || (position->y < 0) || (position->y > LARGEUR_TABLE))
+ {
+ DEBUG_FORMULA_ERROR("OUT of the Table\n");
+ return FORMULA_OUT_OF_TABLE;
+ }
+ else
+ {
+ DEBUG_FORMULA_INFO("Formula 3 : X = %d Y = %d\n",position->x,position->y);
+ return FORMULA_VALID_POSITION;
+ }
+ }
+}
+
+/* This function computes coords using formula 4 mode ie for beacon 1 + 3*/
+TFormulaStatus formula4_compute_coord(coord_s * position, float angle_beacon1, float angle_beacon3)
+{
+ DEBUG_FORMULA_INFO("Formula 4 : angle_beacon1 = %f angle_beacon3 = %f\n",angle_beacon1,angle_beacon3);
+
+ if(angle_beacon3 > M_PI/2)
+ {
+ if((angle_beacon1 > BLIND_ZONE_BEACON1_ANGLE13_MIN) && (angle_beacon1 < BLIND_ZONE_BEACON1_ANGLE13_MAX) && ((M_PI - angle_beacon3) > BLIND_ZONE_BEACON3_ANGLE13_MIN) && ((M_PI - angle_beacon3) < BLIND_ZONE_BEACON3_ANGLE13_MAX))
+ {
+ DEBUG_FORMULA_ERROR("BLIND Zone detected\n");
+ return FORMULA_BLIND_ZONE_DETECTED;
+ }
+ position->y = (LARGEUR_DEMI_TABLE*tan(M_PI - angle_beacon3) - LARGEUR_TABLE*tan(angle_beacon1) + LONGUEUR_TABLE) / (tan(M_PI - angle_beacon3) - tan(angle_beacon1));
+ }
+ else
+ {
+ position->y = (LARGEUR_DEMI_TABLE*tan(angle_beacon3) + LARGEUR_TABLE*tan(angle_beacon1)-LONGUEUR_TABLE) / (tan(angle_beacon1) + tan(angle_beacon3));
+ }
+ position->x = (LARGEUR_TABLE - position->y)*tan(angle_beacon1);
+
+ if( (position->x < 0) || (position->x > LONGUEUR_TABLE) || (position->y < 0) || (position->y > LARGEUR_TABLE))
+ {
+ DEBUG_FORMULA_ERROR("OUT of the Table\n");
+ return FORMULA_OUT_OF_TABLE;
+ }
+ else
+ {
+ DEBUG_FORMULA_INFO("Formula 4 : X = %d Y = %d\n",position->x,position->y);
+ return FORMULA_VALID_POSITION;
+ }
+}
+
+/* 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)
+{
+ DEBUG_FORMULA_INFO("Formula 5 : angle_beacon2 = %f angle_beacon3 = %f\n",angle_beacon2,angle_beacon3);
+
+ if(angle_beacon3 > M_PI/2)
+ {
+ position->y = (LONGUEUR_TABLE + LARGEUR_DEMI_TABLE * tan(M_PI - angle_beacon3)) / (tan(angle_beacon2) + tan(M_PI - angle_beacon3));
+ }
+ else
+ {
+ if((angle_beacon2 > BLIND_ZONE_BEACON2_ANGLE23_MIN) && (angle_beacon2 < BLIND_ZONE_BEACON2_ANGLE23_MAX) && (angle_beacon3 > BLIND_ZONE_BEACON3_ANGLE23_MIN) && (angle_beacon3 < BLIND_ZONE_BEACON3_ANGLE23_MAX))
+ {
+ DEBUG_FORMULA_ERROR("BLIND Zone detected\n");
+ return FORMULA_BLIND_ZONE_DETECTED;
+ }
+ position->y = (LARGEUR_DEMI_TABLE*tan(angle_beacon3) - LONGUEUR_TABLE) / (tan(angle_beacon3) - tan(angle_beacon2));
+ }
+ position->x = tan(angle_beacon2) * position->y;
+
+ if( (position->x < 0) || (position->x > LONGUEUR_TABLE) || (position->y < 0) || (position->y > LARGEUR_TABLE))
+ {
+ DEBUG_FORMULA_ERROR("OUT of the Table\n");
+ return FORMULA_OUT_OF_TABLE;
+ }
+ else
+ {
+ DEBUG_FORMULA_INFO("Formula 5 : X = %d Y = %d\n",position->x,position->y);
+ return FORMULA_VALID_POSITION;
+ }
+}
+
+
+/* This function computes an absolute position using 2 angles from 2 beacons */
+TFormulaStatus formula_compute_position(int formula, int current_beacon_ID, float latest_angle, float current_angle, coord_s * result)
+{
+ int error = FORMULA_VALID_POSITION;
+ switch(formula)
+ {
+ case 3:
+ if(current_beacon_ID == 1)
+ {
+ error = formula3_compute_coord(result, current_angle, latest_angle);
+ }
+ else
+ {
+ error = formula3_compute_coord(result, latest_angle, current_angle);
+ }
+ break;
+ case 4:
+ if(current_beacon_ID == 1)
+ {
+ error = formula4_compute_coord(result, current_angle, latest_angle);
+ }
+ else
+ {
+ error = formula4_compute_coord(result, latest_angle, current_angle);
+ }
+ break;
+ case 5:
+ if(current_beacon_ID == 2)
+ {
+ error = formula5_compute_coord(result, current_angle,latest_angle);
+ }
+ else
+ {
+ error = formula5_compute_coord(result, latest_angle, current_angle);
+ }
+ break;
+ default:
+ DEBUG_FORMULA_ERROR("Unknown Formula = %d\r\n",formula);
+ error = FORMULA_UNKNOWN_FORMULA;
+ }
+ return error;
+}
+
+
diff --git a/digital/beacon/simu/formula.h b/digital/beacon/simu/formula.h
new file mode 100644
index 00000000..719ab8c0
--- /dev/null
+++ b/digital/beacon/simu/formula.h
@@ -0,0 +1,72 @@
+/* formula.h */
+/* Beacon triangulation algorithms. {{{
+ *
+ * Copyright (C) 2011 Florent Duchon
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+
+#ifndef _FORMULA_H
+#define _FORMULA_H
+
+/* Formula calibration */
+#define LARGEUR_TABLE 2000
+#define LONGUEUR_TABLE 3000
+#define LARGEUR_DEMI_TABLE LARGEUR_TABLE/2
+
+#define BLIND_ZONE_PRECISION 0.18 //10 degrees
+// #define BLIND_ZONE_PRECISION 0.18/2 //5 degrees
+#define BLIND_ZONE_BEACON1_ANGLE12_MAX 0 + BLIND_ZONE_PRECISION//10 degrees
+#define BLIND_ZONE_BEACON1_ANGLE13_MIN 1.25856505285 - BLIND_ZONE_PRECISION //72 degrees
+#define BLIND_ZONE_BEACON1_ANGLE13_MAX 1.25856505285 + BLIND_ZONE_PRECISION //72 degrees
+
+#define BLIND_ZONE_BEACON2_ANGLE12_MAX 0 + BLIND_ZONE_PRECISION //10 degrees
+#define BLIND_ZONE_BEACON2_ANGLE23_MIN 1.25856505285 - BLIND_ZONE_PRECISION//72 degrees
+#define BLIND_ZONE_BEACON2_ANGLE23_MAX 1.25856505285 + BLIND_ZONE_PRECISION //72 degrees
+
+#define BLIND_ZONE_BEACON3_ANGLE13_MIN 1.24765037811 - BLIND_ZONE_PRECISION //70 degrees
+#define BLIND_ZONE_BEACON3_ANGLE13_MAX 1.24765037811 + BLIND_ZONE_PRECISION//70 degrees
+#define BLIND_ZONE_BEACON3_ANGLE23_MIN 1.24765037811 - BLIND_ZONE_PRECISION//70 degrees
+#define BLIND_ZONE_BEACON3_ANGLE23_MAX 1.24765037811 + BLIND_ZONE_PRECISION//70 degrees
+
+
+/* Status return */
+typedef enum
+{
+ FORMULA_VALID_POSITION,
+ FORMULA_BLIND_ZONE_DETECTED,
+ FORMULA_OUT_OF_TABLE,
+ FORMULA_UNKNOWN_FORMULA
+} TFormulaStatus;
+
+
+/* This function computes an absolute position using 2 angles from 2 beacons */
+TFormulaStatus formula_compute_position(int formula, int current_beacon_ID, float latest_angle, float current_angle, coord_s * result);
+
+/* 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);
+
+/* This function computes coords using formula 4 mode ie for beacon 1 + 3*/
+TFormulaStatus formula4_compute_coord(coord_s * position, float angle_beacon1, float angle_beacon3);
+
+/* 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);
+
+#endif
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
diff --git a/digital/beacon/simu/position.h b/digital/beacon/simu/position.h
index de962e5a..9010e705 100644
--- a/digital/beacon/simu/position.h
+++ b/digital/beacon/simu/position.h
@@ -26,34 +26,51 @@
#ifndef _POSITION_H
#define _POSITION_H
-#define LARGEUR_TABLE 2000
-#define LONGUEUR_TABLE 3000
-#define LARGEUR_DEMI_TABLE LARGEUR_TABLE/2
-#define M_PI 3.14
+#define MAX_OBSTACLE 2
+#define MAX_BEACON 3
+#define MAX_TEMP_POSITION MAX_OBSTACLE * 2
+#define OBSTACLE_RADIUS 100
+#define IGNORE_ANGLE 1000
+/* Status returns */
+typedef enum{
+ POSITION_NO_ERROR,
+ POSITION_IGNORE_ANGLE
+} TPositionStatus;
+
+/* Structures definition */
+
+/* Beacon Structure */
typedef struct
{
- float angle[4];
- int last_updated_beacon;
- int before_last_updated_beacon;
-
-}beacon_value_s;
+ int angleNumber;
+ float angle[MAX_OBSTACLE+1];
+}beacon_s;
+/* Obstacle structure */
typedef struct
{
int x;
int y;
- int old_x;
- int old_y;
+ int trust
}opponent_s;
-
+
+/* Coordinates structure */
+typedef struct
+{
+ int x;
+ int y;
+}coord_s;
+
+/* Recovery Structure */
typedef struct
{
- int lost_beacon;
-}status_s;
+ int x;
+ int y;
+ int occurence;
+}recovery_s;
+/* This function is used to initialize all needed structures */
void init_struct(void);
-void update_position(int beacon_number,double angle);
-void distance_filter(void);
#endif \ No newline at end of file
diff --git a/digital/beacon/simu/recovery.c b/digital/beacon/simu/recovery.c
new file mode 100644
index 00000000..a090b32c
--- /dev/null
+++ b/digital/beacon/simu/recovery.c
@@ -0,0 +1,142 @@
+/* recovery.c */
+/* Beacon recovery mode. {{{
+ *
+ * Copyright (C) 2012 Florent Duchon
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+
+
+#include "position.h"
+#include "recovery.h"
+#include "trust.h"
+#include "debug.h"
+
+/* This function is used to calculate all obstacle positions from sractch */
+TRecoveryStatus recovery(coord_s * new_point,opponent_s opp[MAX_OBSTACLE])
+{
+ /* Declare variables */
+ int i = 0;
+ int j = 0;
+ int dx = 0;
+ int dy = 0;
+
+ static int counter = 0; /* Loop counter */
+ static int recovery_number = 0; /* Potential valid positions Number */
+ static int best_occurence = 0; /* Maximum occurence amoung Potential valid Position */
+ static recovery_s recovery_tab[MAX_POINT_RECOVERY]; /* Potention valid position Table containing (x;y) and occurence for all positions found */
+
+ int best_position = 0; /* Best Position variable found into recovery tab */
+
+
+ /* In recovery mode, trust must be lowest possible till real position is not found. */
+ for(j = 1 ; j <= MAX_OBSTACLE ; j++)
+ {
+ opp[j].trust=TRUST_MIN;
+ }
+
+ /* If we didn't reach the recovery threshold we continue to feed the recovery tab */
+ if(counter < MAX_POINT_RECOVERY)
+ {
+ /* Search into already known positions */
+ for(i = 0; i < recovery_number ; i ++)
+ {
+ dx = recovery_tab[i].x - new_point->x;
+ dy = recovery_tab[i].y - new_point->y;
+
+ if(dx * dx + dy * dy < OBSTACLE_RADIUS * OBSTACLE_RADIUS)
+ {
+ /* Seems be this position : update structure*/
+ recovery_tab[i].occurence++;
+ recovery_tab[i].x = new_point->x;
+ recovery_tab[i].y = new_point->y;
+ counter++;
+
+ /* Check if the new occurence is the best one amout all other positions */
+ if(recovery_tab[i].occurence > best_occurence)
+ {
+ best_occurence = recovery_tab[i].occurence;
+ /* !! Tricky operation !! : If counter reaches a defined threshold and if we just found the best occurence (ie best accuracy) we invalidate the current angle in order to ignore it next time. */
+ /* Doing that increase a lot the global computation accuracy by ignoring unwanted phantom position */
+ if(counter > RECOVERY_TRICKY_THRESHOLD)
+ {
+ return RECOVERY_IGNORE_ANGLE_NEXT_TIME;
+ }
+ }
+ /* Value found, counter and structures updated. Return NO_ERROR */
+ return RECOVERY_IN_PROGRESS;
+
+ }
+ }
+ /* Position was not found in the tab, we have to add it */
+ recovery_tab[recovery_number].x = new_point->x;
+ recovery_tab[recovery_number].y = new_point->y;
+ recovery_tab[recovery_number].occurence++;
+ recovery_number++;
+ counter++;
+ return RECOVERY_IN_PROGRESS;
+ }
+ else /* We have sufficient values to find absolute position of all obstacles */
+ {
+
+ for(j = 0 ; j < recovery_number ; j++)
+ {
+ DEBUG_RECOVERY("[%d] (%d ; %d)\n",recovery_tab[j].occurence,recovery_tab[j].x,recovery_tab[j].y);
+ }
+
+ for(i = 1 ; i <= MAX_OBSTACLE ; i++)
+ {
+ /* Search for the position with best occurence */
+ for(j = 0 ; j < recovery_number ; j++)
+ {
+ if(recovery_tab[j].occurence >= best_occurence)
+ {
+ best_position = j;
+ best_occurence = recovery_tab[j].occurence;
+ }
+ }
+
+ /* Update the obstacle structure with update position */
+ recovery_tab[best_position].occurence = 0;
+ opp[i].x = recovery_tab[best_position].x;
+ opp[i].y = recovery_tab[best_position].y;
+ opp[i].trust = TRUST_MAX;
+
+ /* Reset variable for next obstacle */
+ best_position = 0;
+ best_occurence = 0;
+ DEBUG_RECOVERY("Opponent %d ( %d ; %d ) trust = %d\n",i,opp[i].x,opp[i].y,opp[i].trust);
+ }
+
+ /* Reset couner variable for next recovery */
+ counter = 0;
+ recovery_number = 0;
+ best_occurence = 0;
+ for(j = 0 ; j < MAX_POINT_RECOVERY ; j++)
+ {
+ recovery_tab[j].occurence=0;
+ recovery_tab[j].x=0;
+ recovery_tab[j].y=0;
+ }
+ return RECOVERY_FINISHED;
+ }
+}
+
+
diff --git a/digital/beacon/simu/recovery.h b/digital/beacon/simu/recovery.h
new file mode 100644
index 00000000..08629e70
--- /dev/null
+++ b/digital/beacon/simu/recovery.h
@@ -0,0 +1,45 @@
+/* recovery.h */
+/* Beacon recovery mode. {{{
+ *
+ * Copyright (C) 2012 Florent Duchon
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+
+#ifndef _RECOVERY_H
+#define _RECOVERY_H
+
+#include "position.h"
+
+#define MAX_POINT_RECOVERY 500
+#define RECOVERY_TRICKY_THRESHOLD MAX_POINT_RECOVERY/3
+
+/* Status returns */
+typedef enum
+{
+ RECOVERY_IN_PROGRESS,
+ RECOVERY_FINISHED,
+ RECOVERY_IGNORE_ANGLE_NEXT_TIME
+} TRecoveryStatus;
+
+/* This function is used to calculate all obstacle positions from sractch */
+TRecoveryStatus recovery(coord_s * new_point,opponent_s result[MAX_OBSTACLE]);
+
+#endif
diff --git a/digital/beacon/simu/simulator.py b/digital/beacon/simu/simulator.py
index 4284b3d0..7e8b4e1c 100644
--- a/digital/beacon/simu/simulator.py
+++ b/digital/beacon/simu/simulator.py
@@ -32,9 +32,16 @@ from math import pi
import math
import random
import decimal
+import time
+
+import fcntl
+import os
+from subprocess import *
+
class Obstacle:
- def __init__ (self, pos, radius, factor):
+ def __init__ (self, id, pos, radius, factor):
+ self.id = id
self.pos = pos
self.radius = radius
self.factor = factor
@@ -50,7 +57,7 @@ class Beacon (Drawable):
self.pos = pos # position (x,y)
self.orientation = orientation # orientation in degrees
self.size = size # side size
- self.angle = 0 # obstacles angle
+ self.angle = {} # obstacles angle
self.mode = 0 # 0 = deactivated
# 1 = activated
def draw_beacon (self):
@@ -108,7 +115,13 @@ class Area (Drawable):
self.draw_line ((b.pos[0],b.pos[1]),(o.pos[0],o.pos[1]),fill='cyan', arrow = NONE)
def populate (self):
- self.obstacles.append (Obstacle ((500, 500), 200, 0))
+ self.obstacles.append (Obstacle (1,(random.randrange(100,2700),random.randrange(100,1700)), 200, 0))
+ self.obstacles.append (Obstacle (2,(random.randrange(100,2700), random.randrange(100,1700)), 200, 0))
+
+ #self.obstacles.append (Obstacle (1,(1666,1411), 200, 0))
+ #self.obstacles.append (Obstacle (2,(2604,344), 200, 0))
+ #self.obstacles.append (Obstacle ((500, 500), 200, 0))
+
self.beacons.append (Beacon (self, 1, (-40,2040), 270,80))
self.beacons.append (Beacon (self, 2, (-40,-40),0,80))
self.beacons.append (Beacon (self, 3, (3040,1000), 180,80))
@@ -146,7 +159,14 @@ class beacon_simu (Frame):
self.createWidgets (border_min, border_max)
self.robot_pos.set("Robot position = (0 , 0)")
self.phantom_pos.set("Last phantom position = ")
-
+ args = []
+ args[0:0] = [ './beacon.host' ]
+ self.p = Popen (args,shell=True, stdin = PIPE, stdout = PIPE)
+ self.counter = 0
+ self.valeur_ko = 0
+ self.valeur_ok = 0
+ self.total = 0
+
def createWidgets (self, border_min, border_max):
# Bottom Panel
self.bottomFrame = Frame (self)
@@ -194,9 +214,10 @@ class beacon_simu (Frame):
# Sixth subPanel for Exit button
self.subPanel6 = Frame (self.bottomFrame)
self.subPanel6.pack (side = 'right', fill = 'both')
- self.quitButton = Button (self.subPanel6, text = 'Quit', command = self.quit)
- self.quitButton.pack (side = 'right', fill = 'both')
-
+ self.quitButton = Button (self.subPanel6, text = 'Quit', command = self.exit)
+ self.quitButton.pack (side = 'right', fill = 'both')
+ self.recoveryButton = Button (self.subPanel6, text = 'Recovery', command = self.recovery)
+ self.recoveryButton.pack (side = 'right', fill = 'both')
self.areaview = AreaView (border_min, border_max, self)
self.areaview.pack (expand = True, fill = 'both')
self.areaview.bind ('<1>', self.click)
@@ -220,11 +241,14 @@ class beacon_simu (Frame):
def click (self, ev):
pos = self.areaview.screen_coord ((ev.x, ev.y))
-
# Update obstacles position
for o in self.areaview.area.obstacles:
if self.areaview.area.border_min[0] < pos[0] < self.areaview.area.border_max[0] and self.areaview.area.border_min[1] < pos[1] < self.areaview.area.border_max[1]:
- o.pos = pos
+ dx = o.pos[0] - pos[0]
+ dy = o.pos[1] - pos[1]
+ if dx * dx + dy * dy < 300*300:
+ print "Obstacle trouve"
+ o.pos = pos
self.robot_pos.set("Robot position = (%.0f , %.0f)" % pos)
# Check beacon mode
@@ -236,70 +260,115 @@ class beacon_simu (Frame):
# Update area
self.update ()
- def call_algorithm (self,num1,num2):
- args = [0,0,0,0,0,0,0,0]
- for b in self.areaview.area.beacons:
- if b.id is num1:
- args[1]=num1
- args[2]=b.angle
- if b.id is num2:
- args[3]=num2
- args[4]=b.angle
- args = [repr(a) for a in args]
- args[0:0] = [ './beacon.host' ]
- p = Popen (args, stdout = PIPE)
- output = p.communicate ()[0]
- del p
- output = output.split ('\n')
- return output
+ def call_algorithm (self,num,angle,angleID):
+ temp = []
+ self.p.stdin.write(str(num)+'\n')
+ self.p.stdin.flush()
+ self.p.stdin.write(str(angle)+'\n')
+ self.p.stdin.flush()
+ self.p.stdin.write(str(angleID)+'\n')
+ self.p.stdin.flush()
+ for o in self.areaview.area.obstacles:
+ x = self.p.stdout.readline().split('\n')
+ y= self.p.stdout.readline().split('\n')
+ trust = self.p.stdout.readline().split('\n')
+ temp.append([x,y,trust])
+ return temp
def rotate_beacons (self): # Simulate a rotation for a all beacons, ie set beacon.angles.
# Set the requested imprecision
imprecision = self.precisionScale.get () #1 to 3 degrees
imprecision = int (math.radians(imprecision)*1000)
imprecision = decimal.Decimal(random.randrange(-imprecision,imprecision))/1000
+ #imprecision = 0
# Compute angles for every beaconss
- for o in self.areaview.area.obstacles:
- for b in self.areaview.area.beacons:
+ for b in self.areaview.area.beacons:
+ for o in self.areaview.area.obstacles:
if b.id is 1:
- b.angle = math.atan(o.pos[0]/(2000-o.pos[1]))+float(imprecision)
+ b.angle[o.id] = math.atan(float(o.pos[0])/(float(2000)-float(o.pos[1])))+float(imprecision)
if b.id is 2:
- b.angle = math.atan(o.pos[0]/o.pos[1])+float(imprecision)
+ b.angle[o.id] = math.atan(float(o.pos[0])/float(o.pos[1]))+float(imprecision)
+ #print math.degrees(b.angle[o.id])
if b.id is 3:
- b.angle = math.atan((3000-o.pos[0])/(1000-o.pos[1]))+float(imprecision)
- if b.angle < 0:
- b.angle = pi - abs(b.angle)
+ b.angle[o.id] = math.atan((float(3000)-float(o.pos[0]))/(float(1000)-float(o.pos[1])))+float(imprecision)
+ if b.angle[o.id] < 0:
+ b.angle[o.id] = pi - abs(b.angle[o.id])
+
def manual_mode (self):
# Manual mode : warning : two beacons must already be activated
self.rotate_beacons ()
- temp = [0,0,0]
- i = 0
for b in self.areaview.area.beacons:
if b.mode is 1:
- temp[i] = b.id
- i=i+1
- phantom_pos = self.call_algorithm(temp[0],temp[1])
- self.areaview.add_phantom((int (phantom_pos[0]),int (phantom_pos[1])))
- self.phantom_pos.set("Last phantom position = (%.0f , %.0f)" %(float(phantom_pos[0]),float(phantom_pos[1])))
+ phantom_pos = self.call_algorithm(b.id,b.angle[1],1)
+ phantom_pos = self.call_algorithm(b.id,b.angle[2],2)
+ self.areaview.add_phantom((int (phantom_pos[0][0]),int (phantom_pos[0][1])))
+ self.areaview.add_phantom((int (phantom_pos[1][0]),int (phantom_pos[1][1])))
+ #self.phantom_pos.set("Last phantom position = (%.0f , %.0f)" %(float(phantom_pos[0]),float(phantom_pos[1])))
self.update ()
def automatic_mode (self):
# Automatic mode : all beacons are used
self.rotate_beacons ()
-
- # Randomly select two beacons and update obstacles position
- exclude = random.randrange(1,4)
- if exclude is 1:
- phantom_pos = self.call_algorithm(2,3)
- if exclude is 2:
- phantom_pos = self.call_algorithm(1,3)
- if exclude is 3:
- phantom_pos = self.call_algorithm(1,2)
-
- # Draw the computed position
- self.areaview.add_phantom((int (phantom_pos[0]),int (phantom_pos[1])))
- self.phantom_pos.set("Last phantom position = (%.0f , %.0f)" %(float(phantom_pos[0]),float(phantom_pos[1])))
+ select = random.randrange(1,4)
+ for b in self.areaview.area.beacons:
+ if b.id is select:
+ temp = 1
+ for o in self.areaview.area.obstacles:
+ phantom_pos = self.call_algorithm(b.id,b.angle[temp],temp)
+ for o in self.areaview.area.obstacles:
+ if phantom_pos[temp-1][2][0] is not '0':
+ self.areaview.add_phantom((int (phantom_pos[temp-1][0][0]),int (phantom_pos[temp-1][1][0])))
+ temp = temp + 1
+
+
+
+
+ def automatic_mode_old (self):
+ # Automatic mode : all beacons are used
+ self.rotate_beacons ()
+ select = random.randrange(1,4)
+ for b in self.areaview.area.beacons:
+ if b.id is select:
+ phantom_pos = self.call_algorithm(b.id,b.angle[1],1)
+ if phantom_pos[0][2][0] is not '0':
+ self.areaview.add_phantom((int (phantom_pos[0][0][0]),int (phantom_pos[0][1][0])))
+ if phantom_pos[1][2][0] is not '0':
+ self.areaview.add_phantom((int (phantom_pos[1][0][0]),int (phantom_pos[1][1][0])))
+ phantom_pos = self.call_algorithm(b.id,b.angle[2],2)
+ # Draw the computed position
+ if phantom_pos[0][2][0] is not '0':
+ self.areaview.add_phantom((int (phantom_pos[0][0][0]),int (phantom_pos[0][1][0])))
+ if phantom_pos[1][2][0] is not '0':
+ self.areaview.add_phantom((int (phantom_pos[1][0][0]),int (phantom_pos[1][1][0])))
+
+
+ def automatic_mode_for_recovery_test (self):
+ # Automatic mode : all beacons are used
+ self.rotate_beacons ()
+ trust = 0
+ select = random.randrange(1,4)
+ for b in self.areaview.area.beacons:
+ if b.id is select:
+ phantom_pos = self.call_algorithm(b.id,b.angle[1],1)
+ if phantom_pos[0][2][0] is not '0':
+ self.areaview.add_phantom((int (phantom_pos[0][0][0]),int (phantom_pos[0][1][0])))
+ trust += 1
+ if phantom_pos[1][2][0] is not '0':
+ self.areaview.add_phantom((int (phantom_pos[1][0][0]),int (phantom_pos[1][1][0])))
+ trust += 1
+ if trust != 0:
+ return trust
+ phantom_pos = self.call_algorithm(b.id,b.angle[2],2)
+ # Draw the computed position
+ if phantom_pos[0][2][0] is not '0':
+ print phantom_pos
+ self.areaview.add_phantom((int (phantom_pos[0][0][0]),int (phantom_pos[0][1][0])))
+ trust += 1
+ if phantom_pos[1][2][0] is not '0':
+ self.areaview.add_phantom((int (phantom_pos[1][0][0]),int (phantom_pos[1][1][0])))
+ trust += 1
+ return trust
def start (self) :
if self.mode.get() == "manual":
@@ -316,6 +385,51 @@ class beacon_simu (Frame):
else:
print "No mode selected"
return
+ def exit (self):
+ self.p.kill()
+ self.quit()
+
+ def recovery (self):
+ recovery = 0
+ self.clear_phantoms()
+ while recovery == 0:
+ recovery = self.automatic_mode_for_recovery_test()
+ #self.update()
+ #print self.areaview.area.phantoms
+ self.update()
+ for o in self.areaview.area.obstacles:
+ #print "obstacle"
+ #print "o.pos[0] = %d - self.areaview.area.phantoms[0][0] = %d" %(o.pos[0],self.areaview.area.phantoms[0][0])
+ dx1 = o.pos[0] - self.areaview.area.phantoms[0][0]
+ dy1 = o.pos[1] - self.areaview.area.phantoms[0][1]
+ dx2 = o.pos[0] - self.areaview.area.phantoms[1][0]
+ dy2 = o.pos[1] - self.areaview.area.phantoms[1][1]
+ total1 = dx1 * dx1 + dy1 * dy1
+ total2 = dx2 * dx2 + dy2 * dy2
+ x = random.randrange(100,2700)
+ y = random.randrange(100,1700)
+ o.pos = (x,y)
+ self.total += 1
+ if total1 < 90000 or total2 < 90000:
+ #print "1 trouve"
+ self.valeur_ok+=1
+ else:
+ #print "1 pas trouve"
+ self.valeur_ko+=1
+ print "#######################################"
+ print self.areaview.area.phantoms
+ print "OK = %d" %(self.valeur_ok)
+ print "KO = %d" %(self.valeur_ko)
+ print "Total = %d" %(self.total)
+ print "New position (%d,%d) (%d,%d)" %(self.areaview.area.obstacles[0].pos[0],self.areaview.area.obstacles[0].pos[1],self.areaview.area.obstacles[1].pos[0],self.areaview.area.obstacles[1].pos[1])
+ print "#######################################"
+ #time.sleep(10)
+ #self.clear_phantoms()
+ self.after (100,self.recovery)
+
+
+
+
if __name__ == '__main__':
app = beacon_simu ((0, 0), (3000, 2000))
diff --git a/digital/beacon/simu/trust.c b/digital/beacon/simu/trust.c
new file mode 100644
index 00000000..340a82bd
--- /dev/null
+++ b/digital/beacon/simu/trust.c
@@ -0,0 +1,73 @@
+/* trust.c */
+/* Beacon Trust control. {{{
+ *
+ * Copyright (C) 2011 Florent Duchon
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+
+#include "position.h"
+#include "trust.h"
+#include "debug.h"
+
+extern opponent_s opponent[MAX_OBSTACLE];
+
+
+/* This function returns the trust level */
+TTrustStatus trust_check_level(void)
+{
+ int i = 0;
+ for( i = 1; i <= MAX_OBSTACLE ; i++)
+ {
+ /* If at least one obstacle is under the trust threashold, we are not confident about its position */
+ DEBUG_TRUST("opponent[%d].trust == %d\n",i,opponent[i].trust);
+ if(opponent[i].trust < TRUST_THRESHOLD)
+ {
+ return TRUST_TOO_BAD;
+ }
+ }
+ return TRUST_LEVEL_OK;
+}
+
+
+/* This function increases the trust level for a specifical obstacle */
+TTrustStatus trust_increase(int number)
+{
+ if(opponent[number].trust < TRUST_MAX)
+ {
+ opponent[number].trust++;
+ }
+ return TRUST_LEVEL_OK;
+}
+
+/* This function decreases the trust level for all obstacles */
+TTrustStatus trust_decrease(void)
+{
+ int i = 0;
+ for( i = 1; i <= MAX_OBSTACLE ; i++)
+ {
+ if(opponent[i].trust > TRUST_MIN)
+ {
+ opponent[i].trust--;
+ }
+ }
+ return TRUST_LEVEL_OK;
+}
+
diff --git a/digital/beacon/simu/trust.h b/digital/beacon/simu/trust.h
new file mode 100644
index 00000000..8169660f
--- /dev/null
+++ b/digital/beacon/simu/trust.h
@@ -0,0 +1,50 @@
+/* trust.h */
+/* Beacon Trust control {{{
+ *
+ * Copyright (C) 2011 Florent Duchon
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+
+#ifndef _TRUST_H
+#define _TRUST_H
+
+#define TRUST_MIN 0
+#define TRUST_THRESHOLD 30
+#define TRUST_MAX 100
+
+/* Status returns */
+typedef enum
+{
+ TRUST_LEVEL_OK,
+ TRUST_TOO_BAD
+} TTrustStatus;
+
+/* This function returns the trust level */
+TTrustStatus trust_check_level(void);
+
+/* This function increases the trust level for a specifical obstacle */
+TTrustStatus trust_increase(int number);
+
+/* This function decreases the trust level for all obstacles */
+TTrustStatus trust_decrease(void);
+
+
+#endif \ No newline at end of file
diff --git a/digital/beacon/simu/update.c b/digital/beacon/simu/update.c
new file mode 100644
index 00000000..7a6805b0
--- /dev/null
+++ b/digital/beacon/simu/update.c
@@ -0,0 +1,63 @@
+/* update.c */
+/* Beacon udapte position mode. {{{
+ *
+ * Copyright (C) 2012 Florent Duchon
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+
+#include "position.h"
+#include "trust.h"
+#include "update.h"
+#include "debug.h"
+
+/* Globals Declaration */
+extern opponent_s opponent[MAX_OBSTACLE];
+
+/* This function checks is the given coord is a potential obstacle and updates the structure in consequence */
+TUpdateStatus update(coord_s * point)
+{
+ int j = 0;
+ int dx = 0;
+ int dy = 0;
+
+ /* Find if it's near a previous obstacle */
+ DEBUG_UPDATE("Check if ( %.4d ; %.4d ) exists\n",point->x,point->y);
+
+ for( j = 1; j <= MAX_OBSTACLE ; j++)
+ {
+ dx = opponent[j].x - point->x;
+ dy = opponent[j].y - point->y;
+ if (dx * dx + dy * dy < OBSTACLE_RADIUS * OBSTACLE_RADIUS)
+ {
+ DEBUG_UPDATE("Opponent found (%.4d ; %.4d)\n",opponent[j].x,opponent[j].y);
+ opponent[j].x = point->x;
+ opponent[j].y = point->y;
+ trust_increase(j);
+ return UPDATE_OBSTACLE_FOUND;
+ }
+ }
+
+ /* No obstacle found */
+ DEBUG_UPDATE("Opponent not found\n");
+ return UPDATE_OBSTACLE_NOT_FOUND;
+}
+
+
diff --git a/digital/beacon/simu/avrconfig.h b/digital/beacon/simu/update.h
index 8da6dbc6..72300d0c 100644
--- a/digital/beacon/simu/avrconfig.h
+++ b/digital/beacon/simu/update.h
@@ -1,9 +1,7 @@
-#ifndef avrconfig_h
-#define avrconfig_h
-/* avrconfig.h - Path module configuration template. */
-/* avr.path - Path finding module. {{{
+/* update.h */
+/* Beacon udapte position mode. {{{
*
- * Copyright (C) 2008 Nicolas Schodet
+ * Copyright (C) 2012 Florent Duchon
*
* APBTeam:
* Web: http://apbteam.org/
@@ -25,15 +23,18 @@
*
* }}} */
-/* path - Path finding module. */
-/** Report path found for debug. */
-#define AC_PATH_REPORT defined (HOST)
-/** Report function name. */
-#define AC_PATH_REPORT_CALLBACK path_report
-/** Number of possible obstacles. */
-#define AC_PATH_OBSTACLES_NB 10
-/** Number of points per obstacle. */
-#define AC_PATH_OBSTACLES_POINTS_NB 8
+#ifndef _UPDATE_H
+#define _UPDATE_H
+#include "position.h"
-#endif /* avrconfig_h */
+/* Status returns */
+typedef enum {
+ UPDATE_OBSTACLE_NOT_FOUND,
+ UPDATE_OBSTACLE_FOUND
+} TUpdateStatus;
+
+/* This function checks is the given coord is a potential obstacle and updates the structure in consequence */
+TUpdateStatus update(coord_s * point);
+
+#endif \ No newline at end of file