class Car:
brick = None
left = None
right = None
def __init__(self):
brick = nxt.locator.find_one_brick()
self.brick = brick
self.left = Motor(brick, PORT_A)
self.right = Motor(brick, PORT_C)
self.light = Light(brick, PORT_4)
# self.ultrasonic = Ultrasonic(brick, PORT_4)
print "Connection established."
def turn(self, angle=100, speed=30):
if angle > 0:
self.left.turn(angle, speed)
elif angle < 0:
self.right.turn(-angle, speed)
def forward(self, distance=50, speed=30):
self.left.run(power=speed)
self.right.run(power=speed)
sleep(distance / speed)
self.left.idle()
self.right.idle()
def range(self):
return self.ultrasonic.get_sample()
def surface(self):
return self.light.get_sample()
class Robot:
def __init__(self):
print 'Searching for NXT bricks...'
self.robot = nxt.locator.find_one_brick()
print 'NXT brick found'
self.right_motor = Motor(self.robot, PORT_B)
self.left_motor = Motor(self.robot, PORT_C)
self.locator = Ultrasonic(self.robot, PORT_1)
self.haptic = Touch(self.robot, PORT_4)
def forward(self):
if(random.random() > .5):
self.right_motor.run(-STRAIGHT_POWER)
self.left_motor.run(-STRAIGHT_POWER)
else:
self.left_motor.run(-STRAIGHT_POWER)
self.right_motor.run(-STRAIGHT_POWER)
sleep(SECONDS)
if(random.random() > .5):
self.right_motor.idle()
self.left_motor.idle()
else:
self.left_motor.idle()
self.right_motor.idle()
def back(self):
self.right_motor.run(STRAIGHT_POWER)
self.left_motor.run(STRAIGHT_POWER)
sleep(SECONDS)
self.right_motor.idle()
self.left_motor.idle()
def right(self):
self.left_motor.turn(-TURN_POWER, ANGLE)
def left(self):
self.right_motor.turn(-TURN_POWER, ANGLE)
def distance(self):
return self.locator.get_sample()
def is_touching(self):
return self.haptic.get_sample()
def beep_ok(self):
self.robot.play_tone_and_wait(FREQ_C, DURATION)
self.robot.play_tone_and_wait(FREQ_D, DURATION)
self.robot.play_tone_and_wait(FREQ_E, DURATION)
def beep_not_ok(self):
self.robot.play_tone_and_wait(FREQ_E, DURATION)
self.robot.play_tone_and_wait(FREQ_D, DURATION)
self.robot.play_tone_and_wait(FREQ_C, DURATION)
def turnmotor(self, port, turns, power):
if self._bricks:
port = str(port)
port_up = port.upper()
if (port_up in NXT_MOTOR_PORTS):
port = NXT_MOTOR_PORTS[port_up]
if not((power < -127) or (power > 127)):
if turns < 0:
turns = abs(turns)
power = -1 * power
try:
m = Motor(self._bricks[self.active_nxt], port)
m.turn(power, int(turns*360), brake=True)
m.brake()
except:
raise logoerror(ERROR_GENERIC)
else:
raise logoerror(ERROR_POWER)
else:
raise logoerror(ERROR_PORT_M % port)
else:
raise logoerror(ERROR_BRICK)
def turnmotor(self, port, turns, power):
if self._bricks:
port = str(port)
port_up = port.upper()
if (port_up in NXT_MOTOR_PORTS):
port = NXT_MOTOR_PORTS[port_up]
if not((power < -127) or (power > 127)):
if turns < 0:
turns = abs(turns)
power = -1 * power
try:
m = Motor(self._bricks[self.active_nxt], port)
m.turn(power, int(turns*360), brake=True)
m.brake()
except:
raise logoerror(ERROR_GENERIC)
else:
raise logoerror(ERROR_POWER)
else:
raise logoerror(ERROR_PORT_M % port)
else:
raise logoerror(ERROR_BRICK)
class DrivarNxt(Drivar):
def __init__(self):
self.m_initialized = False
self.m_block = None
self.m_leftMotor = None
self.m_rightMotor = None
self.m_ultrasonicSensor = None
self.m_moving = False
def initialize(self):
super(DrivarNxt,self).initialize()
self.m_block = nxt.locator.find_one_brick()
self.m_leftMotor = Motor(self.m_block, PORT_A)
self.m_rightMotor = Motor(self.m_block, PORT_C)
self.m_ultrasonicSensor = Ultrasonic(self.m_block, PORT_4)
self.m_initialized = True
def move(self, direction=Drivar.DIR_FORWARD,durationInMs=1000, callback = None):
durationInMs = max(durationInMs,100)
_direct = direction
self.rotateWheels(direction = _direct)
time.sleep(durationInMs/1000)
self.stop()
if callback is not None:
callback()
def rotateWheels(self, wheelSet = Drivar.WHEELS_BOTH, direction = Drivar.DIR_FORWARD, speedLevel = Drivar.SPEED_FAST, callback = None):
power = self._getNxtSpeed(speedLevel)
# Correct the power (positive vs negative) depending on the direction
if(direction == Drivar.DIR_FORWARD):
if(power < 0):
power = power * -1
if(direction == Drivar.DIR_BACKWARD):
if(power > 0):
power = power * -1
# Get the wheels turning
if(wheelSet == Drivar.WHEELS_LEFT or wheelSet == Drivar.WHEELS_BOTH):
self.m_leftMotor.run(power)
if(wheelSet == Drivar.WHEELS_RIGHT or wheelSet == Drivar.WHEELS_BOTH):
self.m_rightMotor.run(power)
self.m_moving = True
if callback is not None:
callback()
def turn(self, direction = Drivar.DIR_LEFT, angle = 90):
left_power = -100
right_power = 100
if(direction == Drivar.DIR_RIGHT):
left_power *= -1
right_power *= -1
self.m_leftMotor.turn(left_power, angle)
self.m_rightMotor.turn(right_power, angle)
def stop(self, callback = None):
self.m_leftMotor.idle()
self.m_rightMotor.idle()
self.m_moving = False
if callback is not None:
callback()
'''
Return the distance to the nearest obstacle, in centimeters
'''
def getDistanceToObstacle(self):
return self.m_ultrasonicSensor.get_sample()
'''
Indicate with a boolean whether there is an obstacle within the given distance
'''
def isObstacleWithin(self, distance):
dist = self.m_ultrasonicSensor.get_sample()
if(dist <= distance):
return True
else:
return False
'''
Return the NXT speed equivalent for the given DRIVAR speed flag
'''
@staticmethod
def _getNxtSpeed(speed):
if(speed==Drivar.SPEED_SLOW):
return 70
elif(speed==Drivar.SPEED_MEDIUM):
return 100
elif(speed==Drivar.SPEED_FAST):
return 127
else :
return 100
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)
def step(forwardPower):
#print('stepping')
walkingMotor.run(power=forwardPower)
sleep(.1)
while True:
if legPosition.is_pressed() == True:
walkingMotor.run(power=0)
walkingMotor.brake()
return
def legsDown():
walkingMotor.run(120)
init = brick.get_battery_level()
print('starting')
while brick.get_battery_level() > init - 1000:
pass
walkingMotor.brake()
return
armMotor.brake()
repeat = ''
#armPower = int(raw_input('Input arm power: '))
'''
while repeat == '':
legsDown()
repeat = raw_input()
'''
armMotor.turn(-90, 90, brake=False)
import time
import ipdb
from nxt.motor import Motor, PORT_A, PORT_B
from nxt.sensor import Ultrasonic, Sound, PORT_2, PORT_4
robo = nxt.bluesock.BlueSock('00:16:53:08:51:40').connect()
direita = Motor(robo, PORT_A)
esquerda = Motor(robo, PORT_B)
direita.run(-82)
esquerda.run(-80)
time.sleep(20)
direita.brake()
esquerda.brake()
direita.turn(-40,500)
#ipdb.set_trace()
direita.run(-82)
esquerda.run(-80)
time.sleep(7)
direita.brake()
esquerda.brake()
direita.turn(-40,500)
direita.run(-82)
esquerda.run(-80)
time.sleep(6)
direita.brake()
esquerda.brake()
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)
def motor_turn(letter, deg, power):
m = Motor(b, MOTORS[letter.upper()])
m.turn(int(power), int(deg))
return 'OK'
class Robot(object):
r'''A high-level controller for the Alpha Rex model.
This class implements methods for the most obvious actions performable
by Alpha Rex, such as walk, wave its arms, and retrieve sensor samples.
Additionally, it also allows direct access to the robot's components
through public attributes.
'''
def __init__(self, brick='NXT'):
r'''Creates a new Alpha Rex controller.
brick
Either an nxt.brick.Brick object, or an NXT brick's name as a
string. If omitted, a Brick named 'NXT' is looked up.
'''
if isinstance(brick, basestring):
brick = find_one_brick(name=brick)
self.brick = brick
self.leftMotor = Motor(brick, PORT_B)
self.rightMotor = Motor(brick, PORT_C)
self.motors = [self.leftMotor, self.rightMotor]
self.touch = Touch(brick, PORT_3)
#self.sound = Sound(brick, PORT_2)
#self.light = Light(brick, PORT_3)
self.ultrasonic = Ultrasonic(brick, PORT_4)
def WarningNoise(self):
self.brick.play_tone_and_wait(FREQ_E, 100)
self.brick.play_tone_and_wait(FREQ_D, 100)
self.brick.play_tone_and_wait(FREQ_C, 100)
def OkNoise(self):
self.brick.play_tone_and_wait(FREQ_C, 100)
self.brick.play_tone_and_wait(FREQ_D, 100)
self.brick.play_tone_and_wait(FREQ_E, 100)
def StartWalk(self, power):
for motor in self.motors:
motor.run(power=power)
def StopWalk(self):
for motor in self.motors:
motor.idle()
def walk(self, secs, power=FORTH):
self.StartWalk(power)
sleep(secs)
self.StopWalk()
def IsButtonPressed(self):
r'''Reads the Touch sensor's output.
'''
return self.touch.get_sample()
def turn(self, direction, degrees):
if (direction == RobotMode.Left):
self.leftMotor.turn(-100, degrees)
self.rightMotor.turn(100, degrees)
else:
self.rightMotor.turn(-100, degrees)
self.leftMotor.turn(100, degrees)
def getDistanceFromObstacle(self):
return self.ultrasonic.get_sample()
class Robot:
def __init__(self, brick):
self.brick = brick
self.queue = Queue()
self.components = {}
self.sensors = {}
self.active = Component(self)
self.InitializeHardware()
self.InitializeComponents()
self.InitializeSensors()
self.active.Start()
self.queue_thread = Thread(target=self.QueueWorker)
self.queue_thread.start()
def GetBrick(self):
return self.brick
def InitializeHardware(self):
self.motor_grip = Motor(self.brick, nxt.PORT_C)
self.rotation = 50
self.motor_rotate = Motor(self.brick, nxt.PORT_B)
self.elevation = 0
self.motor_elevate = Motor(self.brick, nxt.PORT_A)
self.grip = False
def InitializeSensors(self):
self.sensors[ColorSensor.name] = ColorSensor(self, nxt.PORT_1)
def InitializeComponents(self):
self.components[Scanner.name] = Scanner
self.components[Collector.name] = Collector
def Connect(self):
# if something bad happens here, see
# http://code.google.com/p/nxt-python/wiki/Installation
#self.brick.connect()
for name in self.sensors:
self.sensors[name].Start()
def Interrupt(self, sensor, value):
self.queue.put((sensor, value))
def QueueWorker(self):
while True:
sensor, value = self.queue.get()
active_change = False
for name in self.components:
if (self.components[name].priority > self.active.priority):
if self.components[name].IsImportant(sensor, value):
self.active.Halt()
self.active = self.components[name](self)
active_change = True
if active_change:
self.active.Start()
#if self.active.HasStopped():
# self.active = Scanner(self)
# self.active.Start()
self.queue.task_done()
def Grip(self):
#if not self.grip:
self.motor_grip.turn(GRIP_POWER, GRIP_ANGLE)
self.grip = True
def Release(self):
#if self.grip:
self.motor_grip.turn(-GRIP_POWER, GRIP_ANGLE)
self.grip = False
def StopMotors(self):
self.motor_rotate.idle()
self.motor_elevate.idle()
def RotateTo(self, n):
diff = self.rotation - n
if (diff > 0):
turnPercent = diff / 100.0 * ROTATE_ANGLE
Thread(target=self.motor_elevate.turn, args=(-ELEVATION_ANGLE_ROTATION_POWER, diff / 100.0 * ELEVATION_ANGLE_ROTATION)).start()
self.motor_rotate.turn(ROTATE_POWER, turnPercent)
elif (diff < 0):
turnPercent = -diff / 100.0 * ROTATE_ANGLE
Thread(target=self.motor_elevate.turn, args=(ELEVATION_ANGLE_ROTATION_POWER, -diff / 100.0 * ELEVATION_ANGLE_ROTATION)).start()
self.motor_rotate.turn(-ROTATE_POWER, turnPercent)
self.rotation = n
def Rotate(self, add):
if (add + self.rotation > ROTATE_ANGLE or
add + self.rotation < 0):
div, mod = divmod(add + self.rotation, 100)
self.RotateTo(abs(div))
def ElevateTo(self, n):
diff = self.elevation - n
if (diff > 0):
self.motor_elevate.turn(ELEVATION_POWER, diff / 100.0 * ELEVATION_ANGLE)
elif (diff < 0):
self.motor_elevate.turn(-ELEVATION_POWER, -diff / 100.0 * ELEVATION_ANGLE)
self.elevation = n
self.motor_elevate.idle()
def Elevate(self, add):
if add + self.rotation > ELEVATE_ANGLE:
div, mod = divmod(add + self.elevation, 100)
self.ElevateTo(ELEVATE_ANGLE)
elif add + self.elevation < 0:
self.ElevateTo(0)
def GetSensor(self, name):
return self.sensors[name]
import nxt
import nxtConnect # has to be in search path
import time
brickName = "Team60"
useUSB = False
if useUSB:
brick = nxt.find_one_brick(name=brickName,
strict=True,
method=nxt.locator.Method(usb=True,
bluetooth=True))
else:
# the bluetooth function of the nxt library works too, but "wastes"
# time searching for devices.
brick = nxtConnect.btConnect(brickName)
print(brick.get_device_info()) # check what brick you connected to
from time import sleep
from nxt.motor import Motor, PORT_A
armMotor = Motor(brick, PORT_A)
armMotor.turn(-80, 120, brake=True, timeout=3, emulate=True)
class DrivarNxt(Drivar):
def __init__(self):
self.m_initialized = False
self.m_block = None
self.m_leftMotor = None
self.m_rightMotor = None
self.m_penMotor = None
self.m_ultrasonicSensor = None
self.m_lightSensor = None
self.m_moving = False
def initialize(self):
super(DrivarNxt, self).initialize()
self.m_block = nxt.locator.find_one_brick()
self.m_leftMotor = Motor(self.m_block, PORT_A)
self.m_rightMotor = Motor(self.m_block, PORT_C)
self.m_penMotor = Motor(self.m_block, PORT_B)
self.m_ultrasonicSensor = Ultrasonic(self.m_block, PORT_4)
self.m_lightSensor = Light(self.m_block, PORT_3)
self.m_initialized = True
def move(self,
direction=Drivar.DIR_FORWARD,
durationInMs=1000,
callback=None):
durationInMs = max(durationInMs, 100)
_direct = direction
self.rotateWheels(direction=_direct)
time.sleep(durationInMs / 1000)
self.stop()
if callback is not None:
callback()
def rotateWheels(self,
wheelSet=Drivar.WHEELS_BOTH,
direction=Drivar.DIR_FORWARD,
speedLevel=Drivar.SPEED_FAST,
callback=None):
power = self._getNxtSpeed(speedLevel)
# Correct the power (positive vs negative) depending on the direction
if (direction == Drivar.DIR_FORWARD):
if (power < 0):
power = power * -1
if (direction == Drivar.DIR_BACKWARD):
if (power > 0):
power = power * -1
# Get the wheels turning
if (wheelSet == Drivar.WHEELS_LEFT or wheelSet == Drivar.WHEELS_BOTH):
self.m_leftMotor.run(power)
if (wheelSet == Drivar.WHEELS_RIGHT or wheelSet == Drivar.WHEELS_BOTH):
self.m_rightMotor.run(power)
self.m_moving = True
if callback is not None:
callback()
def turn(self, direction=Drivar.DIR_LEFT, angle=90, callback=None):
left_power = -100
right_power = 100
if (direction == Drivar.DIR_RIGHT):
left_power *= -1
right_power *= -1
self.m_leftMotor.turn(left_power, angle)
self.m_rightMotor.turn(right_power, angle)
def stop(self, callback=None):
self.m_leftMotor.idle()
self.m_rightMotor.idle()
self.m_moving = False
if callback is not None:
callback()
'''
Return the distance to the nearest obstacle, in centimeters
'''
def getDistanceToObstacle(self):
return self.m_ultrasonicSensor.get_sample()
'''
Indicate with a boolean whether there is an obstacle within the given distance
'''
def isObstacleWithin(self, distance):
dist = self.m_ultrasonicSensor.get_sample()
if (dist <= distance):
return True
else:
return False
def rotatePen(self, angle):
power = 70
if angle < 0:
angle = -1 * angle
power = -70
self.m_penMotor.turn(power, angle)
def getReflectivityMeasurement(self):
self.m_lightSensor.set_illuminated(True)
return self.m_lightSensor.get_sample()
def wait(self, milliseconds):
time.sleep(milliseconds / 1000)
'''
Return the NXT speed equivalent for the given DRIVAR speed flag
'''
@staticmethod
def _getNxtSpeed(speed):
if (speed == Drivar.SPEED_SLOW):
return 70
elif (speed == Drivar.SPEED_MEDIUM):
return 100
elif (speed == Drivar.SPEED_FAST):
return 127
else:
return 100
print(brick.get_device_info()) # check what brick you connected to
from time import sleep
from nxt.sensor import Light
from nxt.sensor.hitechnic import Gyro
from nxt.sensor import PORT_1, PORT_2
from nxt.motor import Motor, PORT_A, PORT_B, PORT_C
motor = Motor(brick, PORT_C)
light = Light(brick, PORT_2)
motor.reset_position(False)
print(motor.get_tacho())
black = light.get_lightness()
print("black = ", black)
motor.turn(30, 50, brake=True, timeout=1.5, emulate=True)
sleep(0.05)
print(motor.get_tacho())
motor.brake()
white = light.get_lightness()
print("white = ", white)
motor.turn(-30, 50, brake=True, timeout=1.5, emulate=True)
threshold = (black + white) / 2
print(motor.get_tacho())
print("Threshold = ", threshold)
while light.get_lightness() < 1020:
light.get_lightness()
if light.get_lightness() > threshold:
light.get_lightness()
motor.run(power=10)
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)
