def brake(self, port):
if self._bricks:
port = str(port)
port_up = port.upper()
if (port_up in NXT_MOTOR_PORTS):
port = NXT_MOTOR_PORTS[port_up]
try:
m = Motor(self._bricks[self.active_nxt], port)
m.brake()
except:
raise logoerror(ERROR_GENERIC)
else:
raise logoerror(ERROR_PORT_M % port)
else:
raise logoerror(ERROR_BRICK)
def brake(self, port):
if self._bricks:
port = str(port)
port_up = port.upper()
if (port_up in NXT_MOTOR_PORTS):
port = NXT_MOTOR_PORTS[port_up]
try:
m = Motor(self._bricks[self.active_nxt], port)
m.brake()
except:
raise logoerror(ERROR_GENERIC)
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)
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 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.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()
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, PORT_B, PORT_C
from nxt.sensor import Touch, PORT_4, PORT_3, PORT_2, Light, PORT_1, Ultrasonic
#from basicFunctions import step, calibrate
#light = Light(brick, PORT_1)
#turningMotor = Motor(brick, PORT_B)
walkingMotor = Motor(brick, PORT_C)
armMotor = Motor(brick, PORT_A)
walkingMotor.brake()
armMotor.brake()
def test(normalization, fileName):
with open(fileName, 'w') as outputFile:
outputFile.write('Initial value: %f' % normalization)
for i in range(1000):
diff = normalization - brick.get_battery_level()
outputFile.write('%f\n' % diff)
sleep(0.05)
def main():
repeat = ''
while repeat == '':
normalization = 0
for i in range(100):
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)
light.get_lightness()
if light.get_lightness() < threshold:
def motor_stop(letter):
m = Motor(b, MOTORS[letter.upper()])
m.brake()
return 'OK'
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.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)
