Example #1
0
class Robot(object):
  
  LEFT_WHEEL  = 0x02 # port C
  RIGHT_WHEEL = 0x00 # port A
  KICKER      = 0x01 # port B
    
  DEFAULT_POWER = 80
  TURN_POWER    = 0.8
  
  BUZZER = 769
  
  #NAME = "BrickAshley"
  NAME = "BrickAsh"
    
  def __init__(self, host=None):
    
    self.power = self.DEFAULT_POWER
    self.address = host   
    
    self.connect()
    
    self.leftWhell = Motor(self.brick, self.LEFT_WHEEL)
    self.rightWhell = Motor(self.brick, self.RIGHT_WHEEL)
    self.kicker = Motor(self.brick, self.KICKER)
    print "Set up Motors"
    
    try:
      self.kicker.turn(100, 100, brake=True)
    except Exception as error:
      print error
  
  def connect(self):
    print "Connecting ..."
    try:
      if self.address == None:
        self.brick = nxt.find_one_brick().connect()
      else:
        self.brick = BlueSock(self.address).connect()      
    except nxt.locator.BrickNotFoundError:
      raise RobotNotFoundError
    except BluetoothError as error:
      raise RobotConnectionError(error)
    
    self.__get_info()
    print "Conected to {name}".format(name=self.name)
  
  def disconnect(self):
    try:
      self.brick = None
      gc.collect()
    except:
      print "Unsafe disconect"
      
  def get_name(self):
    self.__get_info()
    return self.name
  
  def set_name(self, name):
    self.brick.set_brick_name(name)
    self.disconnect()
    self.connect()
    self.__get_info()
  
  def set_power(self, value):
    value=int(value)
    if value < -128 or value > 128:
      pass
      # TODO
    self.power = value
  
  def get_power(self):
    return self.power
  
  def __get_info(self):
    threading.Timer(30, self.__get_info).start()
    self.name, self.host, self.signal_strength, self.user_flash = self.brick.get_device_info()
    self.battery = self.brick.get_battery_level()
    print "Info: \n\tName: {name}" \
          "\n\tBT MAC: {host}\n\tBT signal: {signal}\n\t" \
          "Memory: {memory}\n\tBattery: {voltage}mV".format(name=self.name, host=self.host, \
          signal=self.signal_strength, memory=self.user_flash, voltage=self.battery)
  
  def up(self):
    print "go up"
    self.leftWhell.run(power=self.power)
    self.rightWhell.run(power=self.power)
  
  def down(self):
    print "go down"
    self.brick.play_tone_and_wait(self.BUZZER, 1000)
    self.leftWhell.run(power=-self.power)
    self.rightWhell.run(power=-self.power)
  
  def right(self, withBrake=False):
    print "go right"
    self.leftWhell.run(power=self.power*self.TURN_POWER)
    if withBrake:
      self.rightWhell.brake()
    else:
      self.rightWhell.run(power=-self.power*self.TURN_POWER)
  
  def left(self, withBrake=False):
    print "go left"
    if withBrake:
      self.leftWhell.brake()
    else:
      self.leftWhell.run(power=-self.power*self.TURN_POWER)
    self.rightWhell.run(power=self.power*self.TURN_POWER)
  
  def stop(self):
    print "go stop"
    self.leftWhell.brake()
    self.rightWhell.brake()
    self.kicker.brake()
  
  def buzz(self):
    print "buzz"
    self.brick.play_tone_and_wait(self.BUZZER, 1000)
  
  def kick(self):
    print "kick"
    self.kicker.turn(-127, 85, brake=True)
    sleep(1.5)
    self.kicker.turn(127, 90, brake=True)
Example #2
0
class Robot(object):

    LEFT_WHEEL = 0x02  # port C
    RIGHT_WHEEL = 0x00  # port A
    KICKER = 0x01  # port B

    DEFAULT_POWER = 80
    TURN_POWER = 0.8

    BUZZER_HZ = 769

    KICK_DISTANCE = 90

    STATE_DISCONNECTED = -1
    STATE_IDLE = 0
    STATE_UP = 1
    STATE_DOWN = 2
    STATE_RIGHT = 3
    STATE_LEFT = 4

    MAX_MOTOR_POWER = 127

    #NAME = "BrickAshley"
    NAME = "BrickAsh"

    def __init__(self, host=None):

        self.power = -1 * self.DEFAULT_POWER
        self.address = host
        self.state = self.STATE_DISCONNECTED

        self.log = logging.getLogger("Robot")

    def connect(self):
        self.log.info("Connecting ...")
        try:
            if self.address == None:
                self.brick = nxt.find_one_brick().connect()
            else:
                self.brick = BlueSock(self.address).connect()
        except nxt.locator.BrickNotFoundError:
            raise RobotNotFoundError
        except Exception as error:
            raise RobotConnectionError(error)

        self.leftWhell = Motor(self.brick, self.LEFT_WHEEL)
        self.rightWhell = Motor(self.brick, self.RIGHT_WHEEL)
        self.kicker = Motor(self.brick, self.KICKER)
        self.log.info("Set up Motors")

        try:
            #self.kicker.turn(100, 100, brake=True)

            self.log.debug(self.__read_motor_state(self.KICKER))

        except Exception as error:
            self.log.error("kicker reset error: " + str(error))

        self.state = self.STATE_IDLE

        self.__get_info()
        self.log.info("Conected to {name}".format(name=self.name))

        self.buzz()

    def disconnect(self):
        try:
            self.brick = None
            #self.get_info_thread.stop()
            gc.collect()
        except:
            self.log.warning("Unsafe disconect")

        self.state = self.STATE_DISCONNECTED

    def get_name(self):
        self.__get_info()
        return self.name

    def set_name(self, name):
        self.brick.set_brick_name(name)
        self.disconnect()
        self.connect()
        self.__get_info()

    def set_power(self, value):
        if value < -1 * self.MAX_MOTOR_POWER or value > self.MAX_MOTOR_POWER:
            raise ValueError("Power can only be +-127")
        else:
            self.power = value
            self.log.info("power set to: " + str(self.power))

    def get_power(self):
        return self.power

    def __get_info(self):
        #self.get_info_thread = threading.Timer(30, self.__get_info)
        #self.get_info_thread.start()
        self.name, self.host, self.signal_strength, self.user_flash = self.brick.get_device_info(
        )
        self.battery = self.brick.get_battery_level()
        self.log.info(
              "Info: \n\tName: {name}" \
              "\n\tBT MAC: {host}\n\tBT signal: {signal}\n\t" \
              "Memory: {memory}\n\tBattery: {voltage}mV".format(name=self.name, host=self.host, \
              signal=self.signal_strength, memory=self.user_flash, voltage=self.battery)
              )

    def up(self):
        self.log.debug("go up")
        if self.state != self.STATE_UP:
            self.state = self.STATE_UP
            self.leftWhell.run(power=self.power)
            self.rightWhell.run(power=self.power)

    def down(self):
        self.log.debug("go down")
        if self.state != self.STATE_DOWN:
            self.state = self.STATE_DOWN
            self.leftWhell.run(power=-1 * self.power)
            self.rightWhell.run(power=-1 * self.power)

    def right(self, withBrake=False):
        self.log.debug("go right")
        if self.state != self.STATE_RIGHT:
            self.state = self.STATE_RIGHT
            self.leftWhell.run(power=self.power * self.TURN_POWER)
            if withBrake:
                self.rightWhell.brake()
            else:
                self.rightWhell.run(power=-self.power * self.TURN_POWER)

    def left(self, withBrake=False):
        self.log.debug("go left")
        if self.state != self.STATE_LEFT:
            self.state = self.STATE_LEFT
            if withBrake:
                self.leftWhell.brake()
            else:
                self.leftWhell.run(power=-self.power * self.TURN_POWER)
            self.rightWhell.run(power=self.power * self.TURN_POWER)

    def stop(self):
        self.log.debug("go stop")
        self.state = self.STATE_IDLE
        self.leftWhell.brake()
        self.rightWhell.brake()

    def buzz(self):
        self.log.debug("buzz")
        self.brick.play_tone_and_wait(self.BUZZER_HZ, 1000)

    def kick(self):
        self.log.debug("kick")
        self.kicker.turn(-127, self.KICK_DISTANCE, brake=True)
        threading.Timer(1.5, self.__kick_reset).start()

    def __kick_reset(self):
        self.kicker.turn(127, self.KICK_DISTANCE, brake=True)

    #def __del__(self):
    #  self.log.debug("__del__")
    #  if self.brick != None:
    #    self.disconnect()

    def __read_motor_state(self, port):
        values = self.brick.get_output_state(port)
        self.log.debug("__read_motor_state: values='{0}'".format(values))
        #state, tacho = get_tacho_and_state(values)
        #self.log.debug("__read_motor_state: state='{0}', tacho='{1}'".format(state, tacho))
        left, kick, right = values[-3:]

        if port == self.KICKER:
            return kick
        elif port == self.LEFT_WHEEL:
            return left
        elif port == self.RIGHT_WHEEL:
            return left
        else:
            raise Exception("meh")

    def get_state(self):
        self.__read_motor_state(self.KICKER)
        self.__read_motor_state(self.LEFT_WHEEL)
        self.__read_motor_state(self.RIGHT_WHEEL)

    def kick_to(self, angle, kpower=127, withBrake=True):
        state, tacho = self.__read_motor_state(self.KICKER)
        if angle < tacho:
            self.kicker.turn(-kpower, tacho - angle, brake=withBrake)
        else:
            self.kicker.turn(+kpower, angle - tacho, brake=withBrake)
Example #3
0
class Robot(object):

    LEFT_WHEEL = 0x02  # port C
    RIGHT_WHEEL = 0x00  # port A
    KICKER = 0x01  # port B

    DEFAULT_POWER = 80
    TURN_POWER = 0.8

    BUZZER = 769

    #NAME = "BrickAshley"
    NAME = "BrickAsh"

    def __init__(self, host=None):

        self.power = self.DEFAULT_POWER
        self.address = host

        self.connect()

        self.leftWhell = Motor(self.brick, self.LEFT_WHEEL)
        self.rightWhell = Motor(self.brick, self.RIGHT_WHEEL)
        self.kicker = Motor(self.brick, self.KICKER)
        print "Set up Motors"

        try:
            self.kicker.turn(100, 100, brake=True)
        except Exception as error:
            print error

    def connect(self):
        print "Connecting ..."
        try:
            if self.address == None:
                self.brick = nxt.find_one_brick().connect()
            else:
                self.brick = BlueSock(self.address).connect()
        except nxt.locator.BrickNotFoundError:
            raise RobotNotFoundError
        except BluetoothError as error:
            raise RobotConnectionError(error)

        self.__get_info()
        print "Conected to {name}".format(name=self.name)

    def disconnect(self):
        try:
            self.brick = None
            gc.collect()
        except:
            print "Unsafe disconect"

    def get_name(self):
        self.__get_info()
        return self.name

    def set_name(self, name):
        self.brick.set_brick_name(name)
        self.disconnect()
        self.connect()
        self.__get_info()

    def set_power(self, value):
        value = int(value)
        if value < -128 or value > 128:
            pass
            # TODO
        self.power = value

    def get_power(self):
        return self.power

    def __get_info(self):
        threading.Timer(30, self.__get_info).start()
        self.name, self.host, self.signal_strength, self.user_flash = self.brick.get_device_info(
        )
        self.battery = self.brick.get_battery_level()
        print "Info: \n\tName: {name}" \
              "\n\tBT MAC: {host}\n\tBT signal: {signal}\n\t" \
              "Memory: {memory}\n\tBattery: {voltage}mV".format(name=self.name, host=self.host, \
              signal=self.signal_strength, memory=self.user_flash, voltage=self.battery)

    def up(self):
        print "go up"
        self.leftWhell.run(power=self.power)
        self.rightWhell.run(power=self.power)

    def down(self):
        print "go down"
        self.brick.play_tone_and_wait(self.BUZZER, 1000)
        self.leftWhell.run(power=-self.power)
        self.rightWhell.run(power=-self.power)

    def right(self, withBrake=False):
        print "go right"
        self.leftWhell.run(power=self.power * self.TURN_POWER)
        if withBrake:
            self.rightWhell.brake()
        else:
            self.rightWhell.run(power=-self.power * self.TURN_POWER)

    def left(self, withBrake=False):
        print "go left"
        if withBrake:
            self.leftWhell.brake()
        else:
            self.leftWhell.run(power=-self.power * self.TURN_POWER)
        self.rightWhell.run(power=self.power * self.TURN_POWER)

    def stop(self):
        print "go stop"
        self.leftWhell.brake()
        self.rightWhell.brake()
        self.kicker.brake()

    def buzz(self):
        print "buzz"
        self.brick.play_tone_and_wait(self.BUZZER, 1000)

    def kick(self):
        print "kick"
        self.kicker.turn(-127, 85, brake=True)
        sleep(1.5)
        self.kicker.turn(127, 90, brake=True)
Example #4
0
class Robot(object):
  
  LEFT_WHEEL  = 0x02 # port C
  RIGHT_WHEEL = 0x00 # port A
  KICKER      = 0x01 # port B
    
  DEFAULT_POWER = 80
  TURN_POWER    = 0.8
  
  BUZZER = 769
  
  #NAME = "BrickAshley"
  NAME = "BrickAsh"
    
  def __init__(self, host=None):
    
    self.power = self.DEFAULT_POWER
    self.address = host   
    
    self.log = logging.getLogger("Robot")
    
    self.connect()
    
    self.leftWhell = Motor(self.brick, self.LEFT_WHEEL)
    self.rightWhell = Motor(self.brick, self.RIGHT_WHEEL)
    self.kicker = Motor(self.brick, self.KICKER)
    self.log.info("Set up Motors")
    
    try:
      #self.kicker.turn(100, 100, brake=True)
      
      self.log.debug(self.__read_motor_state(self.KICKER))
      
    except Exception as error:
      self.log.error("kicker reset error: " + str(error))
  
  def connect(self):
    self.log.info("Connecting ...")
    try:
      if self.address == None:
        self.brick = nxt.find_one_brick().connect()
      else:
        self.brick = BlueSock(self.address).connect()      
    except nxt.locator.BrickNotFoundError:
      raise RobotNotFoundError
    except Exception as error:
      raise RobotConnectionError(error)
    
    self.__get_info()
    self.log.info("Conected to {name}".format(name=self.name))
  
  def disconnect(self):
    try:
      self.brick = None
      #self.get_info_thread.stop()
      gc.collect()
    except:
      self.log.warning("Unsafe disconect")
      pass
  
  def get_name(self):
    self.__get_info()
    return self.name
  
  def set_name(self, name):
    self.brick.set_brick_name(name)
    self.disconnect()
    self.connect()
    self.__get_info()
  
  def set_power(self, value):
    value=int(value)
    if value < -127 or value > 127:
      raise ValueError("Power can only be +-127")
    self.power = value
  
  def get_power(self):
    return self.power
  
  def __get_info(self):
    #self.get_info_thread = threading.Timer(30, self.__get_info)
    #self.get_info_thread.start()
    self.name, self.host, self.signal_strength, self.user_flash = self.brick.get_device_info()
    self.battery = self.brick.get_battery_level()
    self.log.info(
          "Info: \n\tName: {name}" \
          "\n\tBT MAC: {host}\n\tBT signal: {signal}\n\t" \
          "Memory: {memory}\n\tBattery: {voltage}mV".format(name=self.name, host=self.host, \
          signal=self.signal_strength, memory=self.user_flash, voltage=self.battery)
          )
  
  def up(self):
    self.log.debug("go up")
    self.leftWhell.run(power=self.power)
    self.rightWhell.run(power=self.power)
  
  def down(self):
    self.log.debug("go down")
    self.brick.play_tone_and_wait(self.BUZZER, 1000)
    self.leftWhell.run(power=-self.power)
    self.rightWhell.run(power=-self.power)
  
  def right(self, withBrake=False):
    self.log.debug("go right")
    self.leftWhell.run(power=self.power*self.TURN_POWER)
    if withBrake:
      self.rightWhell.brake()
    else:
      self.rightWhell.run(power=-self.power*self.TURN_POWER)
  
  def left(self, withBrake=False):
    self.log.debug("go left")
    if withBrake:
      self.leftWhell.brake()
    else:
      self.leftWhell.run(power=-self.power*self.TURN_POWER)
    self.rightWhell.run(power=self.power*self.TURN_POWER)
  
  def stop(self):
    self.log.debug("go stop")
    self.leftWhell.brake()
    self.rightWhell.brake()
    #self.kicker.brake()
  
  def buzz(self):
    self.log.debug("buzz")
    self.brick.play_tone_and_wait(self.BUZZER, 1000)
  
  def kick(self):
    self.log.debug("kick")
    self.kicker.turn(-127, 85, brake=True)
    threading.Timer(1.5, self.__kick_reset).start()
  
  def __kick_reset(self):
    self.kicker.turn(127, 90, brake=False)
  
  #def __del__(self):
  #  self.log.debug("__del__")
  #  if self.brick != None:
  #    self.disconnect()
  
  def __read_motor_state(self, port):
    values = self.brick.get_output_state(port)
    self.log.debug("__read_motor_state: values='{0}'".format(values))
    #state, tacho = get_tacho_and_state(values)
    #self.log.debug("__read_motor_state: state='{0}', tacho='{1}'".format(state, tacho))
    left, kick, right = values[-3:]
    
    if port == self.KICKER:
      return kick
    elif port == self.LEFT_WHEEL:
      return left
    elif port == self.RIGHT_WHEEL:
      return left
    else:
      raise Exception("meh")
  
  def get_state(self):
    self.__read_motor_state(self.KICKER)
    self.__read_motor_state(self.LEFT_WHEEL)
    self.__read_motor_state(self.RIGHT_WHEEL)
  
  def kick_to(self, angle, kpower=127, withBrake=True):
    state, tacho = self.__read_motor_state(self.KICKER)
    if angle < tacho:
      self.kicker.turn(-kpower, tacho-angle, brake=withBrake)
    else:
      self.kicker.turn(+kpower, angle-tacho, brake=withBrake)