From 1355d019cafedbb3d6b191156b80a857c6a618d1 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Sun, 4 Mar 2012 01:32:10 +0100 Subject: digital/beacon/simu: beacon simulator v2 -> Multi Obstacles management --- digital/beacon/simu/Makefile | 3 +- digital/beacon/simu/avrconfig.h | 39 ------- digital/beacon/simu/beacon.c | 108 +++++++------------ digital/beacon/simu/debug.h | 74 +++++++++++++ digital/beacon/simu/formula.c | 166 +++++++++++++++++++++++++++++ digital/beacon/simu/formula.h | 72 +++++++++++++ digital/beacon/simu/position.c | 168 +++++++++++++++++++----------- digital/beacon/simu/position.h | 49 ++++++--- digital/beacon/simu/recovery.c | 142 +++++++++++++++++++++++++ digital/beacon/simu/recovery.h | 45 ++++++++ digital/beacon/simu/simulator.py | 218 +++++++++++++++++++++++++++++---------- digital/beacon/simu/trust.c | 73 +++++++++++++ digital/beacon/simu/trust.h | 50 +++++++++ digital/beacon/simu/update.c | 63 +++++++++++ digital/beacon/simu/update.h | 40 +++++++ 15 files changed, 1074 insertions(+), 236 deletions(-) delete mode 100644 digital/beacon/simu/avrconfig.h create mode 100644 digital/beacon/simu/debug.h create mode 100644 digital/beacon/simu/formula.c create mode 100644 digital/beacon/simu/formula.h create mode 100644 digital/beacon/simu/recovery.c create mode 100644 digital/beacon/simu/recovery.h create mode 100644 digital/beacon/simu/trust.c create mode 100644 digital/beacon/simu/trust.h create mode 100644 digital/beacon/simu/update.c create mode 100644 digital/beacon/simu/update.h (limited to 'digital/beacon') 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/avrconfig.h b/digital/beacon/simu/avrconfig.h deleted file mode 100644 index 8da6dbc6..00000000 --- a/digital/beacon/simu/avrconfig.h +++ /dev/null @@ -1,39 +0,0 @@ -#ifndef avrconfig_h -#define avrconfig_h -/* avrconfig.h - Path module configuration template. */ -/* avr.path - Path finding module. {{{ - * - * Copyright (C) 2008 Nicolas Schodet - * - * 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. - * - * }}} */ - -/* 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 - - -#endif /* avrconfig_h */ 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 #include #include #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 + +// #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 +#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 #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/update.h b/digital/beacon/simu/update.h new file mode 100644 index 00000000..72300d0c --- /dev/null +++ b/digital/beacon/simu/update.h @@ -0,0 +1,40 @@ +/* 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 -- cgit v1.2.3