MAC0318/ParticleFilter/python/robot.py

import numpy as np
import math
import bluetooth

class Robot( object ):
    """Move o robo distance centimetros"""
    def move(self, distance):
        raise NotImplementedError( "Should have implemented this" )

    """Roda o robo radians no sentido horario"""
    def rotate(self, radians):
        raise NotImplementedError( "Should have implemented this" )

    """Retornar verdadeiro ou falso, verdadeiro se leu branco."""
    def read(self):
        raise NotImplementedError( "Should have implemented this" )

# ------------------------------------------------------------------------
class VirtualRobot(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 move(self, distance):
        self.x = self.x + (math.cos(self.heading) * distance)
        self.y = self.y + (math.sin(self.heading) * distance)

    def rotate(self, radians):
        self.heading += float(radians)
        self.heading %= 2 * math.pi

    def read(self):
        return self.worldmap.white(self.x, self.y)

# ------------------------------------------------------------------------
class LejosRobot(Robot):
    def __init__(self, mac):
        self.blt = bluetooth.BluetoothSocket(bluetooth.RFCOMM)
        self.blt.connect((mac, 1))

    def move(self, 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):
        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):
        self.blt.send(bytes([0x3]))
        data = self.blt.recv(32)
        if data and int (data.decode("ascii")) < 40:
            return True
        else:
            return False