Lejos

ParticleFilter/python/particle.py

@@ -6,7 +6,7 @@ import math
 from map import *
 import random
 import colorsys
-from robot import VirtualRobot
+from robot import VirtualRobot, LejosRobot
 
 # ------------------------------------------------------------------------
 class Particle:
@@ -38,7 +38,6 @@ class Particle:
     def read(self):
         return self.worldmap.white(self.x, self.y)
 
-
     def __repr__(self):
         return "(%f, %f)" % (self.x, self.y)
 
@@ -128,7 +127,7 @@ def main():
     height = 2383.94
     zoom = 2.5
     worldmap = Map(width, height, "../3.txt")
-    robot = VirtualRobot(worldmap)
+    robot = LejosRobot('00:16:53:18:2E:17')
 
     PARTICLE_COUNT = 2000
     particles = []
@@ -145,9 +144,12 @@ def main():
         rot = 0
         move = 50
 
-        # # ---------- Try to find current best estimate for display ----------
+        ## ---------- Estima posicao ----------
         m_x, m_y = compute_mean_point(particles)
-        # movimento
+
+        # Realiza o movimento do robo
+        # nesse exemplo estamos utilizando uma tecnica muito ruim.
+        # Ela esta baseada na posicao real, deveria ser na estimativa
         if robot.x < width*0.1 or width*0.9 < robot.x:
             rot = np.random.uniform(math.pi/2,math.pi/2)
             robot.rotate(rot)
@@ -156,12 +158,13 @@ def main():
             robot.rotate(rot)
         robot.move(move)
 
-        #leitura
+        # Realiza a leitura de um sensor
         read = robot.read()
 
+        # sabendo o movemento e a leitura, dedistribui as particulas
         particles = particle_filter(particles, read, move, rot, worldmap)
 
-        # ---------- display particles ----------
+        # exibe as particulas na tela
         for p in particles:
             pt = Point(p.x/zoom, p.y/zoom)
             h = p.heading/(math.pi*2)
@@ -172,21 +175,21 @@ def main():
             cir.setFill(rgb)
             cir.draw(win)
 
-
-        # ---------- show mean point ----------
+        # exibe a posicao estimada na tela
         pt = Point(m_x/zoom, m_y/zoom)
         cir = Circle(pt, 30/zoom)
         cir.setFill('red')
         cir.draw(win)
 
-        # ---------- display robot ----------
-        h = robot.heading/(math.pi*2)
-        rgb = colorsys.hsv_to_rgb(h, 0.8, 0.8)
-        rgb = color_rgb(colorFix(rgb[0]), colorFix(rgb[1]), colorFix(rgb[2]))
-        pt = Point(robot.x/zoom, robot.y/zoom)
-        cir = Circle(pt, 30/zoom)
-        cir.setFill(rgb)
-        cir.draw(win)
+        # mostra a posicao real
+        if type(robot) is VirtualRobot:
+            h = robot.heading/(math.pi*2)
+            rgb = colorsys.hsv_to_rgb(h, 0.8, 0.8)
+            rgb = color_rgb(colorFix(rgb[0]), colorFix(rgb[1]), colorFix(rgb[2]))
+            pt = Point(robot.x/zoom, robot.y/zoom)
+            cir = Circle(pt, 30/zoom)
+            cir.setFill(rgb)
+            cir.draw(win)
 
         win.update()
         win.delete('all')

ParticleFilter/python/robot.py

@@ -1,6 +1,7 @@
 
 import numpy as np
 import math
+import bluetooth
 
 class Robot( object ):
     """Move o robo distance centimetros"""
@@ -36,21 +37,26 @@ class VirtualRobot(Robot):
 
 # ------------------------------------------------------------------------
 class LejosRobot(Robot):
-    def __init__(self, worldmap):
-        self.x = np.random.rand()*worldmap.width
-        self.y = np.random.rand()*worldmap.height
-        self.heading = math.pi*2*np.random.rand()
-        self.worldmap = worldmap
+    def __init__(self, mac):
+        self.blt = bluetooth.BluetoothSocket(bluetooth.RFCOMM)
+        self.blt.connect((mac, 1))
 
     def move(self, distance):
-        self.x = self.x + (math.cos(self.heading) * distance)
-        self.y = self.y + (math.sin(self.heading) * distance)
+        d = int(d)
+        if d > 0 and d < 255:
+            self.blt.send(bytes([0x1, byte(d)]))
+            self.blt.recv(32)
 
     def rotate(self, radians):
-        self.heading += float(radians)
-        self.heading %= 2 * math.pi
+        r = int(radians*255/(math.pi*2))
+        if r > 0 and r < 255:
+            self.blt.send(bytes([0x2, byte(r)]))
+            self.blt.recv(32)
 
     def read(self):
-        return self.worldmap.white(self.x, self.y)
-
-
+        self.blt.send(bytes([0x3]))
+        data = self.blt.recv(32)
+        if data and int (data.decode("ascii")) < 40:
+            return True
+        else:
+            return False