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)
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)
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)
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)