summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorFlorent Duchon2011-12-26 23:48:32 +0100
committerFlorent Duchon2011-12-26 23:48:32 +0100
commit7872d2b5c319862f943d2c1340ecee1d7721d7f4 (patch)
treeaf3754adb98c81c7147ef95050a34b762e991e21
parenta0d7ee857c15e8ec8bd6c2df98048c2ee41a0e68 (diff)
First version of Beacons Simulator
-rw-r--r--digital/beacon/simu/Makefile12
-rw-r--r--digital/beacon/simu/avrconfig.h39
-rw-r--r--digital/beacon/simu/beacon.c106
-rw-r--r--digital/beacon/simu/position.c103
-rw-r--r--digital/beacon/simu/position.h59
-rw-r--r--digital/beacon/simu/simulator.py322
6 files changed, 641 insertions, 0 deletions
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 <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#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 <stdio.h>
+#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 ()