From 7872d2b5c319862f943d2c1340ecee1d7721d7f4 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Mon, 26 Dec 2011 23:48:32 +0100 Subject: First version of Beacons Simulator --- digital/beacon/simu/Makefile | 12 ++ digital/beacon/simu/avrconfig.h | 39 +++++ digital/beacon/simu/beacon.c | 106 +++++++++++++ digital/beacon/simu/position.c | 103 +++++++++++++ digital/beacon/simu/position.h | 59 +++++++ digital/beacon/simu/simulator.py | 322 +++++++++++++++++++++++++++++++++++++++ 6 files changed, 641 insertions(+) create mode 100644 digital/beacon/simu/Makefile create mode 100644 digital/beacon/simu/avrconfig.h create mode 100644 digital/beacon/simu/beacon.c create mode 100644 digital/beacon/simu/position.c create mode 100644 digital/beacon/simu/position.h create mode 100644 digital/beacon/simu/simulator.py diff --git a/digital/beacon/simu/Makefile b/digital/beacon/simu/Makefile new file mode 100644 index 00000000..132f0b2e --- /dev/null +++ b/digital/beacon/simu/Makefile @@ -0,0 +1,12 @@ +BASE = ../../avr +HOST_PROGS = beacon +beacon_SOURCES = beacon.c position.c +MODULES = math/fixed utils +CONFIGFILE = avrconfig.h +# atmega8, atmega8535, atmega128... +AVR_MCU = atmega128 +# -O2 : speed +# -Os : size +OPTIMIZE = -O2 -lm + +include $(BASE)/make/Makefile.gen diff --git a/digital/beacon/simu/avrconfig.h b/digital/beacon/simu/avrconfig.h new file mode 100644 index 00000000..8da6dbc6 --- /dev/null +++ b/digital/beacon/simu/avrconfig.h @@ -0,0 +1,39 @@ +#ifndef avrconfig_h +#define avrconfig_h +/* avrconfig.h - Path module configuration template. */ +/* avr.path - Path finding module. {{{ + * + * Copyright (C) 2008 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/* path - Path finding module. */ +/** Report path found for debug. */ +#define AC_PATH_REPORT defined (HOST) +/** Report function name. */ +#define AC_PATH_REPORT_CALLBACK path_report +/** Number of possible obstacles. */ +#define AC_PATH_OBSTACLES_NB 10 +/** Number of points per obstacle. */ +#define AC_PATH_OBSTACLES_POINTS_NB 8 + + +#endif /* avrconfig_h */ diff --git a/digital/beacon/simu/beacon.c b/digital/beacon/simu/beacon.c new file mode 100644 index 00000000..5ea8a263 --- /dev/null +++ b/digital/beacon/simu/beacon.c @@ -0,0 +1,106 @@ +/* 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" +#include "common.h" + +/* Globals Declaration */ +beacon_value_s beacon; +opponent_s opponent; + +void syntax (void) +{ + fprintf (stderr, + "beacon sources_number source1 angle1 ... sourceX angleX\n" + "example: beacon 2,1,3.2,3,0.82\n" + ); + exit (1); +} + +void read_tab (const char *s, double *tab, int n) +{ + assert (n > 0); + while (n) + { + char *sp; + double v; + v = strtod (s, &sp); + if (sp == s) + syntax (); + if ((n > 1 && *sp != ',') + || (n == 1 && *sp != '\0')) + syntax (); + s = sp + 1; + *tab++ = v; + n--; + } +} + +int main (int argc, char **argv) +{ + if (argc < 8 || argc > 10) + syntax (); + + double input[7] = {0}; + /* Init global structures */ + init_struct(); + + + /* Read Beacon num */ + read_tab (argv[2], input, 1); + + /* Read Beacon angle */ + read_tab (argv[3], input+1, 1); + + /* Read Beacon num */ + read_tab (argv[4], input+2, 1); + + /* Read Beacon angle */ + read_tab (argv[5], input+3, 1); + + /* Read Activate filter */ + read_tab (argv[6],input+4, 1); + + /* Read last x */ + read_tab (argv[7],input+5, 1); + + /* Read last y */ + read_tab (argv[8],input+6, 1); + +// printf("Balise %d --- Angle %f\n",(int)input[0],input[1]); +// printf("Balise %d --- Angle %f\n",(int)input[2],input[3]); + + /* Compute position */ + update_position(input[0],input[1]); + update_position(input[2],input[3]); + + /* Return position to the simulator */ + printf("%d\n",(int)opponent.x); + printf("%d\n",(int)opponent.y); + + return 0; +} diff --git a/digital/beacon/simu/position.c b/digital/beacon/simu/position.c new file mode 100644 index 00000000..5bc4a8f8 --- /dev/null +++ b/digital/beacon/simu/position.c @@ -0,0 +1,103 @@ +/* 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 +#include "position.h" + +extern beacon_value_s beacon; +extern opponent_s opponent; + +void init_struct(void) +{ + beacon.angle[0]=0; + beacon.angle[1]=0; + beacon.angle[2]=0; + beacon.angle[3]=0; + beacon.last_updated_beacon = 0; + beacon.before_last_updated_beacon = 0; + opponent.x = 0; + opponent.y = 0; +} + +float degree_to_radian(int value) +{ + float temp = 0; + temp = (value * 2 * M_PI) / 360; + return temp; +} + +void update_position(int beacon_number,double angle) +{ + int which_formula = 0; + if(beacon_number == beacon.last_updated_beacon) + { + beacon.last_updated_beacon = beacon_number; + } + else + { + beacon.before_last_updated_beacon = beacon.last_updated_beacon; + beacon.last_updated_beacon = beacon_number; + } + which_formula = beacon.before_last_updated_beacon + beacon.last_updated_beacon; +// printf("[position.c] => Update_position beacon_number = %d angle = %f\n",(int)beacon_number,(double)angle); + beacon.angle[beacon_number] = angle; + + switch(which_formula) + { + case 3: +// printf("[position.c] => Formula 3\r\n"); +// printf("[position.c] => angle[1] = %f angle[2] = %f\n",beacon.angle[1],beacon.angle[2]); + opponent.x = LARGEUR_TABLE * tan(beacon.angle[2]) * tan(beacon.angle[1]) / (tan(beacon.angle[2])+tan(beacon.angle[1])); + opponent.y = LARGEUR_TABLE * tan(beacon.angle[1]) / (tan(beacon.angle[2])+tan(beacon.angle[1])); + break; + case 4: +// printf("[position.c] => Formula 4\r\n"); + if(beacon.angle[3] > M_PI/2) + { + opponent.y = (LARGEUR_DEMI_TABLE*tan(M_PI - beacon.angle[3]) - LARGEUR_TABLE*tan(beacon.angle[1]) + LONGUEUR_TABLE) / (tan(M_PI - beacon.angle[3]) - tan(beacon.angle[1])); + } + else + { + opponent.y = (LARGEUR_DEMI_TABLE*tan(beacon.angle[3]) + LARGEUR_TABLE*tan(beacon.angle[1])-LONGUEUR_TABLE) / (tan(beacon.angle[1]) + tan(beacon.angle[3])); + } + opponent.x = (LARGEUR_TABLE - opponent.y)*tan(beacon.angle[1]); + break; + case 5: +// printf("[position.c] => formula 5\r\n"); + if(beacon.angle[3] > M_PI/2) + { + opponent.y = (LONGUEUR_TABLE + LARGEUR_DEMI_TABLE * tan(M_PI - beacon.angle[3])) / (tan(beacon.angle[2]) + tan(M_PI - beacon.angle[3])); + } + else + { + opponent.y = (LARGEUR_DEMI_TABLE*tan(beacon.angle[3]) - LONGUEUR_TABLE) / (tan(beacon.angle[3]) - tan(beacon.angle[2])); + } + opponent.x = tan(beacon.angle[2]) * opponent.y; + break; + default: +// printf("[position.c] => Unknown Formula\r\n"); + break; + } +} \ No newline at end of file diff --git a/digital/beacon/simu/position.h b/digital/beacon/simu/position.h new file mode 100644 index 00000000..de962e5a --- /dev/null +++ b/digital/beacon/simu/position.h @@ -0,0 +1,59 @@ +/* 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 LARGEUR_TABLE 2000 +#define LONGUEUR_TABLE 3000 +#define LARGEUR_DEMI_TABLE LARGEUR_TABLE/2 +#define M_PI 3.14 + +typedef struct +{ + float angle[4]; + int last_updated_beacon; + int before_last_updated_beacon; + +}beacon_value_s; + +typedef struct +{ + int x; + int y; + int old_x; + int old_y; +}opponent_s; + +typedef struct +{ + int lost_beacon; +}status_s; + +void init_struct(void); +void update_position(int beacon_number,double angle); +void distance_filter(void); + +#endif \ No newline at end of file diff --git a/digital/beacon/simu/simulator.py b/digital/beacon/simu/simulator.py new file mode 100644 index 00000000..4284b3d0 --- /dev/null +++ b/digital/beacon/simu/simulator.py @@ -0,0 +1,322 @@ +# 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 + +class Obstacle: + def __init__ (self, pos, radius, factor): + 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 = 0 # 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 ((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 = ") + + 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.quit) + self.quitButton.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]: + 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,num1,num2): + args = [0,0,0,0,0,0,0,0] + for b in self.areaview.area.beacons: + if b.id is num1: + args[1]=num1 + args[2]=b.angle + if b.id is num2: + args[3]=num2 + args[4]=b.angle + args = [repr(a) for a in args] + args[0:0] = [ './beacon.host' ] + p = Popen (args, stdout = PIPE) + output = p.communicate ()[0] + del p + output = output.split ('\n') + return output + + def 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 + # Compute angles for every beaconss + for o in self.areaview.area.obstacles: + for b in self.areaview.area.beacons: + if b.id is 1: + b.angle = math.atan(o.pos[0]/(2000-o.pos[1]))+float(imprecision) + if b.id is 2: + b.angle = math.atan(o.pos[0]/o.pos[1])+float(imprecision) + if b.id is 3: + b.angle = math.atan((3000-o.pos[0])/(1000-o.pos[1]))+float(imprecision) + if b.angle < 0: + b.angle = pi - abs(b.angle) + + def manual_mode (self): + # Manual mode : warning : two beacons must already be activated + self.rotate_beacons () + temp = [0,0,0] + i = 0 + for b in self.areaview.area.beacons: + if b.mode is 1: + temp[i] = b.id + i=i+1 + phantom_pos = self.call_algorithm(temp[0],temp[1]) + self.areaview.add_phantom((int (phantom_pos[0]),int (phantom_pos[1]))) + self.phantom_pos.set("Last phantom position = (%.0f , %.0f)" %(float(phantom_pos[0]),float(phantom_pos[1]))) + self.update () + + def automatic_mode (self): + # Automatic mode : all beacons are used + self.rotate_beacons () + + # Randomly select two beacons and update obstacles position + exclude = random.randrange(1,4) + if exclude is 1: + phantom_pos = self.call_algorithm(2,3) + if exclude is 2: + phantom_pos = self.call_algorithm(1,3) + if exclude is 3: + phantom_pos = self.call_algorithm(1,2) + + # Draw the computed position + self.areaview.add_phantom((int (phantom_pos[0]),int (phantom_pos[1]))) + self.phantom_pos.set("Last phantom position = (%.0f , %.0f)" %(float(phantom_pos[0]),float(phantom_pos[1]))) + + 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 + +if __name__ == '__main__': + app = beacon_simu ((0, 0), (3000, 2000)) + app.mainloop () -- cgit v1.2.3