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/src/Makefile | 129 ++++++++++++ digital/beacon/src/debug.h | 74 +++++++ digital/beacon/src/formula.c | 166 +++++++++++++++ digital/beacon/src/formula.h | 72 +++++++ digital/beacon/src/main_simu.c | 77 +++++++ digital/beacon/src/position.c | 155 ++++++++++++++ digital/beacon/src/position.h | 76 +++++++ digital/beacon/src/recovery.c | 142 +++++++++++++ digital/beacon/src/recovery.h | 45 +++++ digital/beacon/src/simulator.py | 436 ++++++++++++++++++++++++++++++++++++++++ digital/beacon/src/trust.c | 73 +++++++ digital/beacon/src/trust.h | 50 +++++ digital/beacon/src/update.c | 63 ++++++ digital/beacon/src/update.h | 40 ++++ 14 files changed, 1598 insertions(+) create mode 100644 digital/beacon/src/Makefile create mode 100644 digital/beacon/src/debug.h create mode 100644 digital/beacon/src/formula.c create mode 100644 digital/beacon/src/formula.h create mode 100644 digital/beacon/src/main_simu.c create mode 100644 digital/beacon/src/position.c create mode 100644 digital/beacon/src/position.h create mode 100644 digital/beacon/src/recovery.c create mode 100644 digital/beacon/src/recovery.h create mode 100644 digital/beacon/src/simulator.py create mode 100644 digital/beacon/src/trust.c create mode 100644 digital/beacon/src/trust.h create mode 100644 digital/beacon/src/update.c create mode 100644 digital/beacon/src/update.h (limited to 'digital/beacon/src') diff --git a/digital/beacon/src/Makefile b/digital/beacon/src/Makefile new file mode 100644 index 00000000..571c85cd --- /dev/null +++ b/digital/beacon/src/Makefile @@ -0,0 +1,129 @@ +BITCLOUD_PATH = $(PWD)/Bitcloud_stack + +# PROJECT_NAME = MeshBean +# CONFIG_NAME = All_ZigBit_Atmega1281_Rf230_8Mhz_Gcc + +#PROJECT_NAME = RCB230 +#CONFIG_NAME = All_Rcb_Atmega1281_Rf230_8Mhz_Gcc +#CONFIG_NAME = Coordinator_Rcb_Atmega1281_Rf230_8Mhz_Gcc +#CONFIG_NAME = Router_Rcb_Atmega1281_Rf230_8Mhz_Gcc +#CONFIG_NAME = All_Sec_Rcb_Atmega1281_Rf230_8Mhz_Gcc +#CONFIG_NAME = Coordinator_Sec_Rcb_Atmega1281_Rf230_8Mhz_Gcc +#CONFIG_NAME = Router_Sec_Rcb_Atmega1281_Rf230_8Mhz_Gcc +#CONFIG_NAME = All_Rcb_Atmega1281_Rf230_8Mhz_Iar +#CONFIG_NAME = Coordinator_Rcb_Atmega1281_Rf230_8Mhz_Iar +#CONFIG_NAME = Router_Rcb_Atmega1281_Rf230_8Mhz_Iar +#CONFIG_NAME = All_Sec_Rcb_Atmega1281_Rf230_8Mhz_Iar +#CONFIG_NAME = Coordinator_Sec_Rcb_Atmega1281_Rf230_8Mhz_Iar +#CONFIG_NAME = Router_Sec_Rcb_Atmega1281_Rf230_8Mhz_Iar + +#PROJECT_NAME = RCB231 +#CONFIG_NAME = All_Rcb_Atmega1281_Rf231_8Mhz_Gcc +#CONFIG_NAME = Coordinator_Rcb_Atmega1281_Rf231_8Mhz_Gcc +#CONFIG_NAME = Router_Rcb_Atmega1281_Rf231_8Mhz_Gcc +#CONFIG_NAME = All_Sec_Rcb_Atmega1281_Rf231_8Mhz_Gcc +#CONFIG_NAME = Coordinator_Sec_Rcb_Atmega1281_Rf231_8Mhz_Gcc +#CONFIG_NAME = Router_Sec_Rcb_Atmega1281_Rf231_8Mhz_Gcc +#CONFIG_NAME = All_Rcb_Atmega1281_Rf231_8Mhz_Iar +#CONFIG_NAME = Coordinator_Rcb_Atmega1281_Rf231_8Mhz_Iar +#CONFIG_NAME = Router_Rcb_Atmega1281_Rf231_8Mhz_Iar +#CONFIG_NAME = All_Sec_Rcb_Atmega1281_Rf231_8Mhz_Iar +#CONFIG_NAME = Coordinator_Sec_Rcb_Atmega1281_Rf231_8Mhz_Iar +#CONFIG_NAME = Router_Sec_Rcb_Atmega1281_Rf231_8Mhz_Iar + +#PROJECT_NAME = MeshBean_900 +#CONFIG_NAME = All_ZigBit_Atmega1281_Rf212_8Mhz_Gcc +#CONFIG_NAME = Coordinator_ZigBit_Atmega1281_Rf212_8Mhz_Gcc +#CONFIG_NAME = Router_ZigBit_Atmega1281_Rf212_8Mhz_Gcc +#CONFIG_NAME = All_Sec_ZigBit_Atmega1281_Rf212_8Mhz_Gcc +#CONFIG_NAME = Coordinator_Sec_ZigBit_Atmega1281_Rf212_8Mhz_Gcc +#CONFIG_NAME = Router_Sec_ZigBit_Atmega1281_Rf212_8Mhz_Gcc +#CONFIG_NAME = All_ZigBit_Atmega1281_Rf212_8Mhz_Iar +#CONFIG_NAME = Coordinator_ZigBit_Atmega1281_Rf212_8Mhz_Iar +#CONFIG_NAME = Router_ZigBit_Atmega1281_Rf212_8Mhz_Iar +#CONFIG_NAME = All_Sec_ZigBit_Atmega1281_Rf212_8Mhz_Iar +#CONFIG_NAME = Coordinator_Sec_ZigBit_Atmega1281_Rf212_8Mhz_Iar +#CONFIG_NAME = Router_Sec_ZigBit_Atmega1281_Rf212_8Mhz_Iar + +#PROJECT_NAME = RCB212 +#CONFIG_NAME = All_Rcb_Atmega1281_Rf212_8Mhz_Gcc +#CONFIG_NAME = Coordinator_Rcb_Atmega1281_Rf212_8Mhz_Gcc +#CONFIG_NAME = Router_Rcb_Atmega1281_Rf212_8Mhz_Gcc +#CONFIG_NAME = All_Sec_Rcb_Atmega1281_Rf212_8Mhz_Gcc +#CONFIG_NAME = Coordinator_Sec_Rcb_Atmega1281_Rf212_8Mhz_Gcc +#CONFIG_NAME = Router_Sec_Rcb_Atmega1281_Rf212_8Mhz_Gcc +#CONFIG_NAME = All_Rcb_Atmega1281_Rf212_8Mhz_Iar +#CONFIG_NAME = Coordinator_Rcb_Atmega1281_Rf212_8Mhz_Iar +#CONFIG_NAME = Router_Rcb_Atmega1281_Rf212_8Mhz_Iar +#CONFIG_NAME = All_Sec_Rcb_Atmega1281_Rf212_8Mhz_Iar +#CONFIG_NAME = Coordinator_Sec_Rcb_Atmega1281_Rf212_8Mhz_Iar +#CONFIG_NAME = Router_Sec_Rcb_Atmega1281_Rf212_8Mhz_Iar + +PROJECT_NAME = MeshBean +CONFIG_NAME = All_ZigBit_Atmega1281_Rf230_8Mhz_Gcc +#CONFIG_NAME = Coordinator_ZigBit_Atmega1281_Rf230_8Mhz_Gcc +#CONFIG_NAME = Router_ZigBit_Atmega1281_Rf230_8Mhz_Gcc +#CONFIG_NAME = All_Sec_ZigBit_Atmega1281_Rf230_8Mhz_Gcc +#CONFIG_NAME = Coordinator_Sec_ZigBit_Atmega1281_Rf230_8Mhz_Gcc +#CONFIG_NAME = Router_Sec_ZigBit_Atmega1281_Rf230_8Mhz_Gcc +#CONFIG_NAME = All_ZigBit_Atmega1281_Rf230B_8Mhz_Gcc +#CONFIG_NAME = Coordinator_ZigBit_Atmega1281_Rf230B_8Mhz_Gcc +#CONFIG_NAME = Router_ZigBit_Atmega1281_Rf230B_8Mhz_Gcc +#CONFIG_NAME = All_Sec_ZigBit_Atmega1281_Rf230B_8Mhz_Gcc +#CONFIG_NAME = Coordinator_Sec_ZigBit_Atmega1281_Rf230B_8Mhz_Gcc +#CONFIG_NAME = Router_Sec_ZigBit_Atmega1281_Rf230B_8Mhz_Gcc +#CONFIG_NAME = All_ZigBit_Atmega1281_Rf230_8Mhz_Iar +#CONFIG_NAME = Coordinator_ZigBit_Atmega1281_Rf230_8Mhz_Iar +#CONFIG_NAME = Router_ZigBit_Atmega1281_Rf230_8Mhz_Iar +#CONFIG_NAME = All_Sec_ZigBit_Atmega1281_Rf230_8Mhz_Iar +#CONFIG_NAME = Coordinator_Sec_ZigBit_Atmega1281_Rf230_8Mhz_Iar +#CONFIG_NAME = Router_Sec_ZigBit_Atmega1281_Rf230_8Mhz_Iar +#CONFIG_NAME = All_ZigBit_Atmega1281_Rf230B_8Mhz_Iar +#CONFIG_NAME = Coordinator_ZigBit_Atmega1281_Rf230B_8Mhz_Iar +#CONFIG_NAME = Router_ZigBit_Atmega1281_Rf230B_8Mhz_Iar +#CONFIG_NAME = All_Sec_ZigBit_Atmega1281_Rf230B_8Mhz_Iar +#CONFIG_NAME = Coordinator_Sec_ZigBit_Atmega1281_Rf230B_8Mhz_Iar +#CONFIG_NAME = Router_Sec_ZigBit_Atmega1281_Rf230B_8Mhz_Iar + +#PROJECT_NAME = MeshBean_Amp +#CONFIG_NAME = All_ZigBit_Atmega1281_Rf230_8Mhz_Gcc +#CONFIG_NAME = Coordinator_ZigBit_Atmega1281_Rf230_8Mhz_Gcc +#CONFIG_NAME = Router_ZigBit_Atmega1281_Rf230_8Mhz_Gcc +#CONFIG_NAME = All_Sec_ZigBit_Atmega1281_Rf230_8Mhz_Gcc +#CONFIG_NAME = Coordinator_Sec_ZigBit_Atmega1281_Rf230_8Mhz_Gcc +#CONFIG_NAME = Router_Sec_ZigBit_Atmega1281_Rf230_8Mhz_Gcc +#CONFIG_NAME = All_ZigBit_Atmega1281_Rf230_8Mhz_Iar +#CONFIG_NAME = Coordinator_ZigBit_Atmega1281_Rf230_8Mhz_Iar +#CONFIG_NAME = Router_ZigBit_Atmega1281_Rf230_8Mhz_Iar +#CONFIG_NAME = All_Sec_ZigBit_Atmega1281_Rf230_8Mhz_Iar +#CONFIG_NAME = Coordinator_Sec_ZigBit_Atmega1281_Rf230_8Mhz_Iar +#CONFIG_NAME = Router_Sec_ZigBit_Atmega1281_Rf230_8Mhz_Iar + +BASE = ../../avr +HOST_PROGS = beacon +beacon_SOURCES = main_simu.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 -DSIMULATOR +include $(BASE)/make/Makefile.gen + +NUMBER:= 1 2 3 + +simu:host + python simulator.py + +avr: + for count in $(NUMBER); do\ + $(MAKE) -C makefiles/$(PROJECT_NAME) -f Makefile_$(CONFIG_NAME) clean BITCLOUD_PATH=$(BITCLOUD_PATH);\ + $(MAKE) -C makefiles/$(PROJECT_NAME) -f Makefile_$(CONFIG_NAME) all APP_NAME=Beacon_$$count BITCLOUD_PATH=$(BITCLOUD_PATH) LOL_NUMBER=$$count;\ + done;\ + +clean: + for count in $(NUMBER); do\ + $(MAKE) -C makefiles/$(PROJECT_NAME) -f Makefile_$(CONFIG_NAME) clean APP_NAME=Beacon_$$count BITCLOUD_PATH=$(BITCLOUD_PATH);\ + done;\ + + diff --git a/digital/beacon/src/debug.h b/digital/beacon/src/debug.h new file mode 100644 index 00000000..ab166fe9 --- /dev/null +++ b/digital/beacon/src/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/src/formula.c b/digital/beacon/src/formula.c new file mode 100644 index 00000000..8391b8f8 --- /dev/null +++ b/digital/beacon/src/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/src/formula.h b/digital/beacon/src/formula.h new file mode 100644 index 00000000..719ab8c0 --- /dev/null +++ b/digital/beacon/src/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/src/main_simu.c b/digital/beacon/src/main_simu.c new file mode 100644 index 00000000..b3191d90 --- /dev/null +++ b/digital/beacon/src/main_simu.c @@ -0,0 +1,77 @@ +/* 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. + * + * }}} */ +#ifdef SIMULATOR + +#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; +} +#endif //SIMULATOR diff --git a/digital/beacon/src/position.c b/digital/beacon/src/position.c new file mode 100644 index 00000000..ca98b7b5 --- /dev/null +++ b/digital/beacon/src/position.c @@ -0,0 +1,155 @@ +/* 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/src/position.h b/digital/beacon/src/position.h new file mode 100644 index 00000000..9010e705 --- /dev/null +++ b/digital/beacon/src/position.h @@ -0,0 +1,76 @@ +/* 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/src/recovery.c b/digital/beacon/src/recovery.c new file mode 100644 index 00000000..a090b32c --- /dev/null +++ b/digital/beacon/src/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/src/recovery.h b/digital/beacon/src/recovery.h new file mode 100644 index 00000000..08629e70 --- /dev/null +++ b/digital/beacon/src/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/src/simulator.py b/digital/beacon/src/simulator.py new file mode 100644 index 00000000..7e8b4e1c --- /dev/null +++ b/digital/beacon/src/simulator.py @@ -0,0 +1,436 @@ +# 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/src/trust.c b/digital/beacon/src/trust.c new file mode 100644 index 00000000..340a82bd --- /dev/null +++ b/digital/beacon/src/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/src/trust.h b/digital/beacon/src/trust.h new file mode 100644 index 00000000..8169660f --- /dev/null +++ b/digital/beacon/src/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/src/update.c b/digital/beacon/src/update.c new file mode 100644 index 00000000..7a6805b0 --- /dev/null +++ b/digital/beacon/src/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/src/update.h b/digital/beacon/src/update.h new file mode 100644 index 00000000..72300d0c --- /dev/null +++ b/digital/beacon/src/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