summaryrefslogtreecommitdiff
path: root/digital/beacon/simu
diff options
context:
space:
mode:
authorFlorent Duchon2012-03-13 22:50:23 +0100
committerFlorent Duchon2012-03-18 16:30:27 +0100
commit8ac7f52728a8ec2a09a6eeebc3cf5d4afff9b485 (patch)
tree0cdca38087cdbfae887e1c4148c4a065bcfae383 /digital/beacon/simu
parent1355d019cafedbb3d6b191156b80a857c6a618d1 (diff)
digital/beacon, digital/lol: beacon folders reorganisation
Diffstat (limited to 'digital/beacon/simu')
-rw-r--r--digital/beacon/simu/Makefile11
-rw-r--r--digital/beacon/simu/beacon.c76
-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.c155
-rw-r--r--digital/beacon/simu/position.h76
-rw-r--r--digital/beacon/simu/recovery.c142
-rw-r--r--digital/beacon/simu/recovery.h45
-rw-r--r--digital/beacon/simu/simulator.py436
-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.h40
14 files changed, 0 insertions, 1479 deletions
diff --git a/digital/beacon/simu/Makefile b/digital/beacon/simu/Makefile
deleted file mode 100644
index a9b28299..00000000
--- a/digital/beacon/simu/Makefile
+++ /dev/null
@@ -1,11 +0,0 @@
-BASE = ../../avr
-HOST_PROGS = beacon
-beacon_SOURCES = beacon.c position.c formula.c recovery.c update.c trust.c
-MODULES = math/fixed utils
-# atmega8, atmega8535, atmega128...
-AVR_MCU = atmega128
-# -O2 : speed
-# -Os : size
-OPTIMIZE = -O2 -lm
-
-include $(BASE)/make/Makefile.gen
diff --git a/digital/beacon/simu/beacon.c b/digital/beacon/simu/beacon.c
deleted file mode 100644
index 73fc1c8a..00000000
--- a/digital/beacon/simu/beacon.c
+++ /dev/null
@@ -1,76 +0,0 @@
-/* beacon.c */
-/* Beacon Simulator Interface. {{{
- *
- * 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 <stdio.h>
-#include <stdlib.h>
-#include <math.h>
-#include "position.h"
-
-/* Globals Declaration */
-extern opponent_s opponent[MAX_OBSTACLE];
-
-int main (int argc, char **argv)
-{
- char saisie[30];
- char ret = 0;
- int id = 0;
- float angle = 0;
- int angle_id = 0;
- int i = 0;
- /* Init global structures */
- init_struct();
-
- 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
deleted file mode 100644
index ab166fe9..00000000
--- a/digital/beacon/simu/debug.h
+++ /dev/null
@@ -1,74 +0,0 @@
-/* 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
deleted file mode 100644
index 8391b8f8..00000000
--- a/digital/beacon/simu/formula.c
+++ /dev/null
@@ -1,166 +0,0 @@
-/* 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
deleted file mode 100644
index 719ab8c0..00000000
--- a/digital/beacon/simu/formula.h
+++ /dev/null
@@ -1,72 +0,0 @@
-/* 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
deleted file mode 100644
index ca98b7b5..00000000
--- a/digital/beacon/simu/position.c
+++ /dev/null
@@ -1,155 +0,0 @@
-/* position.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 "position.h"
-#include "debug.h"
-#include "recovery.h"
-#include "update.h"
-#include "formula.h"
-#include "trust.h"
-
-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)
-{
- 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;
- }
-}
-
-int update_position(int beaconID, int angleID, double angle)
-{
- static int last_ID[2] = {0};
- int last_valid_id = 0;
- int which_formula = 0;
- 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++)
- {
- if(beaconID != last_ID[i])
- {
- last_valid_id = last_ID[i];
- }
- }
- which_formula = beaconID + last_valid_id;
-
-
- if(last_valid_id != 0)
- {
- 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++)
- {
- 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;
- }
- }
- }
- if(update_status == UPDATE_OBSTACLE_NOT_FOUND)
- {
- /* Obstacle not found */
- trust_decrease();
- }
- 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;
- }
- }
- }
- }
- }
- }
-
- /* 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
deleted file mode 100644
index 9010e705..00000000
--- a/digital/beacon/simu/position.h
+++ /dev/null
@@ -1,76 +0,0 @@
-/* position.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 _POSITION_H
-#define _POSITION_H
-
-#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
-{
- int angleNumber;
- float angle[MAX_OBSTACLE+1];
-}beacon_s;
-
-/* Obstacle structure */
-typedef struct
-{
- int x;
- int y;
- int trust
-}opponent_s;
-
-/* Coordinates structure */
-typedef struct
-{
- int x;
- int y;
-}coord_s;
-
-/* Recovery Structure */
-typedef struct
-{
- int x;
- int y;
- int occurence;
-}recovery_s;
-
-/* This function is used to initialize all needed structures */
-void init_struct(void);
-
-#endif \ No newline at end of file
diff --git a/digital/beacon/simu/recovery.c b/digital/beacon/simu/recovery.c
deleted file mode 100644
index a090b32c..00000000
--- a/digital/beacon/simu/recovery.c
+++ /dev/null
@@ -1,142 +0,0 @@
-/* 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
deleted file mode 100644
index 08629e70..00000000
--- a/digital/beacon/simu/recovery.h
+++ /dev/null
@@ -1,45 +0,0 @@
-/* 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
deleted file mode 100644
index 7e8b4e1c..00000000
--- a/digital/beacon/simu/simulator.py
+++ /dev/null
@@ -1,436 +0,0 @@
-# simu - Beacon simulation. {{{
-#
-# 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.
-#
-# }}}
-
-"""Graphic interface for beacon simulator."""
-
-import re
-from Tkinter import *
-from simu.inter.drawable import *
-from subprocess import Popen, PIPE
-from math import pi
-import math
-import random
-import decimal
-import time
-
-import fcntl
-import os
-from subprocess import *
-
-
-class Obstacle:
- def __init__ (self, id, pos, radius, factor):
- self.id = id
- self.pos = pos
- self.radius = radius
- self.factor = factor
-
- def move (self, pos):
- self.pos = pos
-
-
-class Beacon (Drawable):
- def __init__ (self,onto, num, pos, orientation, size):
- Drawable.__init__ (self, onto)
- self.id = num # ID
- self.pos = pos # position (x,y)
- self.orientation = orientation # orientation in degrees
- self.size = size # side size
- self.angle = {} # obstacles angle
- self.mode = 0 # 0 = deactivated
- # 1 = activated
- def draw_beacon (self):
- # Color depends of the beacon mode
- if self.mode is 1:
- color = 'black'
- else:
- color = 'red'
- # Draw it
- self.draw_rectangle ((self.pos[0]-self.size/2,self.pos[1]-self.size/2),(self.pos[0]+self.size/2,self.pos[1]+self.size/2), fill = color)
-
- def draw_axes (self):
- self.draw_line (self.pos,(self.pos[0]+(300)*math.cos(math.radians(self.orientation)),self.pos[1]+300*math.sin(math.radians(self.orientation))),fill='red', arrow = LAST)
- self.draw_line (self.pos,(self.pos[0]+(300)*math.cos(math.radians(self.orientation+90)),self.pos[1]+300*math.sin(math.radians(self.orientation+90))),fill='red', arrow = LAST)
-
- def toogle_mode (self):
- if self.mode is 0:
- self.mode = 1
- else:
- self.mode = 0
-
-
-class Area (Drawable):
- def __init__ (self, onto, border_min, border_max):
- Drawable.__init__ (self, onto)
- self.border_min = border_min
- self.border_max = border_max
- self.border = None
- self.phantoms_counter = 0
- self.obstacles = [ ]
- self.beacons = [ ]
- self.phantoms = [ ] # virtual obstacles computed by the beacon module.
-
- def draw (self):
- self.reset ()
- self.draw_rectangle (self.border_min, self.border_max, fill = 'grey')
- self.draw_rectangle ((self.border_min[0],self.border_max[1]-500),(self.border_min[0]+500,self.border_max[1]), fill = 'blue')
- self.draw_rectangle ((self.border_max[0]-500,self.border_max[1]-500), self.border_max, fill = 'red')
- self.draw_line ((0,0),(150,0),fill='black', arrow = LAST)
- self.draw_line ((0,0),(0,150),fill='black', arrow = LAST)
-
- for b in self.beacons:
- if b.pos is not None:
- b.draw_beacon ()
- for o in self.obstacles:
- if o.pos is not None:
- self.draw_circle (o.pos, o.radius,fill = o.factor and 'gray50' or 'gray25')
- for p in self.phantoms:
- if p is not None:
- self.draw_circle (p, 20,fill = 0 and 'gray50' or 'red')
-
- def show_angles (self):
- for b in self.beacons:
- for o in self.obstacles:
- 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 (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))
-
- def add_phantom (self,center):
- # Only take care of the 100 latest value. Delete previous one.
- if self.phantoms_counter is 100:
- del self.phantoms[0]
- else:
- self.phantoms_counter += 1
- self.phantoms.append(center)
-
-
-class AreaView (DrawableCanvas):
- def __init__ (self, border_min, border_max, master = None):
- self.border_min = border_min
- self.border_max = border_max
- width = border_max[0] - border_min[0]
- height = border_max[1] - border_min[0]
- DrawableCanvas.__init__ (self, width * 1.3, height * 1.3, -width / 2,-height / 2,master, borderwidth = 1, relief = 'sunken', background = 'white')
- self.area = Area (self, border_min, border_max)
- self.area.populate ()
-
- def draw (self):
- self.area.draw ()
-
- def add_phantom (self,center):
- self.area.add_phantom(center)
-
-
-class beacon_simu (Frame):
- def __init__ (self, border_min, border_max, master = None):
- Frame.__init__ (self, master)
- self.pack (expand = 1, fill = 'both')
- 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)
- self.bottomFrame.pack (side = 'bottom', fill = 'both')
-
- # First subPanel for display options
- self.subPanel1 = Frame (self.bottomFrame)
- self.subPanel1.pack (side = 'left', fill = 'both')
- self.display_axes = IntVar ()
- self.axesButton = Checkbutton (self.subPanel1,variable = self.display_axes, command = self.update,text = 'Display axes', indicatoron = True)
- self.axesButton.pack (anchor='w')
- self.display_angles = IntVar ()
- self.anglesButton = Checkbutton (self.subPanel1,variable = self.display_angles, command = self.update,text = 'Display angles', indicatoron = True)
- self.anglesButton.pack (anchor='w')
-
- # Second subPanel for simulator options
- self.subPanel2 = Frame (self.bottomFrame)
- self.subPanel2.pack (side = 'left', fill = 'both')
- self.mode = StringVar()
- self.manualRadioButton = Radiobutton (self.subPanel2, text = 'Manual', variable = self.mode, value='manual').pack(anchor='w')
- self.autoRadioButton = Radiobutton (self.subPanel2, text = 'Auto', variable = self.mode, value='auto').pack(anchor='w')
-
- # Third subPanel for simulator options
- self.subPanel3 = Frame (self.bottomFrame)
- self.subPanel3.pack (side = 'left', fill = 'both')
- self.precisionScale = Scale (self.subPanel3, label = 'Precision', orient = 'horizontal', from_ = 1, to = 3)
- self.precisionScale.pack ()
-
- # Fourth subPanel for buttons
- self.subPanel4 = Frame (self.bottomFrame)
- self.subPanel4.pack (side = 'left', fill = 'both')
- self.clearPhantomsButton = Button (self.subPanel4, text = 'Clear', command = self.clear_phantoms)
- self.clearPhantomsButton.pack (side = 'bottom')
- self.startButton = Button (self.subPanel4, text = 'Start', command = self.start)
- self.startButton.pack (side = 'top')
-
- # Fifth subPanel for Label
- self.subPanel5 = Frame (self.bottomFrame)
- self.subPanel5.pack (side = 'left', fill = 'both')
- self.robot_pos = StringVar()
- self.label = Label(self.subPanel5, textvariable=self.robot_pos).pack(anchor='w')
- self.phantom_pos = StringVar()
- self.label = Label(self.subPanel5, textvariable=self.phantom_pos).pack(anchor='w')
-
- # 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.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)
-
- def clear (self):
- self.areaview.area.draw ()
-
- def clear_phantoms (self):
- del self.areaview.area.phantoms[:]
- self.areaview.area.phantoms_counter = 0
- self.update()
-
- def update (self):
- self.areaview.area.update ()
- self.areaview.area.draw ()
- if self.display_angles.get() is 1:
- self.areaview.area.show_angles()
- if self.display_axes.get() is 1:
- for b in self.areaview.area.beacons:
- b.draw_axes()
-
- 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]:
- 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
- for b in self.areaview.area.beacons:
- dx = abs(b.pos[0] - pos[0])
- dy = abs(b.pos[1] - pos[1])
- if dx < b.size and dy < b.size:
- b.toogle_mode ()
- # Update area
- self.update ()
-
- 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 b in self.areaview.area.beacons:
- for o in self.areaview.area.obstacles:
- if b.id is 1:
- 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[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[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 ()
- for b in self.areaview.area.beacons:
- if b.mode is 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 ()
- 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":
- self.manual_mode()
- return
- if self.mode.get() == "auto":
- #for o in self.areaview.area.obstacles:
- #x = o.pos[0] + float (decimal.Decimal(random.randrange(-5,40)))
- #y = o.pos[1] + float (decimal.Decimal(random.randrange(-25,25)))
- #o.pos = (x,y)
- self.automatic_mode()
- self.after (30,self.start)
- self.update()
- 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))
- app.mainloop ()
diff --git a/digital/beacon/simu/trust.c b/digital/beacon/simu/trust.c
deleted file mode 100644
index 340a82bd..00000000
--- a/digital/beacon/simu/trust.c
+++ /dev/null
@@ -1,73 +0,0 @@
-/* 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
deleted file mode 100644
index 8169660f..00000000
--- a/digital/beacon/simu/trust.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/* 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
deleted file mode 100644
index 7a6805b0..00000000
--- a/digital/beacon/simu/update.c
+++ /dev/null
@@ -1,63 +0,0 @@
-/* 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/update.h b/digital/beacon/simu/update.h
deleted file mode 100644
index 72300d0c..00000000
--- a/digital/beacon/simu/update.h
+++ /dev/null
@@ -1,40 +0,0 @@
-/* update.h */
-/* 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.
- *
- * }}} */
-
-#ifndef _UPDATE_H
-#define _UPDATE_H
-
-#include "position.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