From 8ac7f52728a8ec2a09a6eeebc3cf5d4afff9b485 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Tue, 13 Mar 2012 22:50:23 +0100 Subject: digital/beacon, digital/lol: beacon folders reorganisation --- digital/beacon/simu/Makefile | 11 - digital/beacon/simu/beacon.c | 76 ------- digital/beacon/simu/debug.h | 74 ------- digital/beacon/simu/formula.c | 166 --------------- digital/beacon/simu/formula.h | 72 ------- digital/beacon/simu/position.c | 155 -------------- digital/beacon/simu/position.h | 76 ------- digital/beacon/simu/recovery.c | 142 ------------- digital/beacon/simu/recovery.h | 45 ---- digital/beacon/simu/simulator.py | 436 --------------------------------------- digital/beacon/simu/trust.c | 73 ------- digital/beacon/simu/trust.h | 50 ----- digital/beacon/simu/update.c | 63 ------ digital/beacon/simu/update.h | 40 ---- 14 files changed, 1479 deletions(-) delete mode 100644 digital/beacon/simu/Makefile delete mode 100644 digital/beacon/simu/beacon.c delete mode 100644 digital/beacon/simu/debug.h delete mode 100644 digital/beacon/simu/formula.c delete mode 100644 digital/beacon/simu/formula.h delete mode 100644 digital/beacon/simu/position.c delete mode 100644 digital/beacon/simu/position.h delete mode 100644 digital/beacon/simu/recovery.c delete mode 100644 digital/beacon/simu/recovery.h delete mode 100644 digital/beacon/simu/simulator.py delete mode 100644 digital/beacon/simu/trust.c delete mode 100644 digital/beacon/simu/trust.h delete mode 100644 digital/beacon/simu/update.c delete mode 100644 digital/beacon/simu/update.h (limited to 'digital/beacon/simu') 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 -#include -#include -#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 - -// #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 -#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 -- cgit v1.2.3