def main():
b = find_one_brick()
eyes = Motor(b, PORT_A)
wheels = [Motor(b, PORT_B), Motor(b, PORT_C)]
ultrasonic = Ultrasonic(b, PORT_4)
phizic(eyes, wheels, ultrasonic)
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 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 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 __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 _idle_motors(self):
for b in self._bricks:
try:
Motor(b, PORT_A).idle()
Motor(b, PORT_B).idle()
Motor(b, PORT_C).idle()
except:
pass
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 __init__(self, world):
Robot.__init__(self, world, 2, Orientation.NORTH)
self.noise = 0
self.brick = find_one_brick()
self.motor1 = Motor(self.brick, PORT_B)
self.motor2 = Motor(self.brick, PORT_C)
self.ultrasonic = Ultrasonic(self.brick, PORT_4)
self.motor1.reset_position(False)
self.motor2.reset_position(False)
def ConnectToNxt():
brick = nxt.bluesock.BlueSock('00:16:53:04:17:F1').connect()
motor_right = Motor(brick, PORT_A)
motor_left = Motor(brick, PORT_C)
ultrasonic_front = Ultrasonic(brick, PORT_2)
ultrasonic_left = Ultrasonic(brick, PORT_1)
ultrasonic_right = Ultrasonic(brick, PORT_3)
return motor_right, motor_left, ultrasonic_front, ultrasonic_left, ultrasonic_right
def syncmotorsforever(self, power):
if self._bricks:
if not((power < -127) or (power > 127)):
try:
motorB = Motor(self._bricks[self.active_nxt], PORT_B)
motorC = Motor(self._bricks[self.active_nxt], PORT_C)
syncmotors = SynchronizedMotors(motorB, motorC, 0)
syncmotors.run(power)
except:
raise logoerror(ERROR_GENERIC)
else:
raise logoerror(ERROR_POWER)
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 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 __init__(self, brick="NXT"):
if isinstance(brick, basestring):
brick = find_one_brick(name=brick)
self.brick = brick
self.motor = True
self.sensor = True
try:
self.brick = brick
except:
print("No brick found")
try:
self.wheels = [Motor(brick, PORT_A), Motor(brick, PORT_B)]
except:
self.motor = False
print("No motors detected!")
def runMotorAngle(self, actuatorVal, actuatorMotorPorts='PORT_A', actuatorForward=True, angle=30, motorGearTeeth=1, actuatorGearTeeth=1, power=70, initial=False):
"""
Runs a motor such that an output reaches the desired angle
actuatorMotorPorts (str): The ports for the actuation motors (default='PORT_A')
actuatorForward (bool): Whether positive power applies forward movement (default=True)
angle (int): The desired angle of motion (default=30)
motorGearTeeth (int): The number of teeth on the motor geer [1 implies no geer] (default=1)
actuatorGearTeeth (int): The number of teeth on the actuator geer [1 implies no geer] (default=1)
power (int): The power you wish to apply to the motor (default=70)
"""
if not initial:
self.angle=()
ports = actuatorMotorPorts.split('.')
for port in ports:
self.angle+=Motor(self.nxt.brick,eval(port)),
gearRatio = float(actuatorGearTeeth/motorGearTeeth)
baseDegree=getUsefulTacho(self.angle[0])
if int(actuatorVal)==1:
if (actuatorForward and angle>0)or(not actuatorForward and angle<0):
print 'Turning Motor '+str(ports)+' '+str(angle)+' degrees at '+str(power)+' power'
curDegree=getUsefulTacho(self.angle[0])
controlledRun(self,self.angle,power,curDegree,(angle*gearRatio))
else:
print 'Turning Motor '+str(ports)+' '+str(angle)+' degrees at '+str(power)+' power'
curDegree=getUsefulTacho(self.angle[0])
controlledRun(self,self.angle,-power,curDegree,(angle*gearRatio))
sleep(1)
curDegree=getUsefulTacho(self.angle[0])
self.pose.updateSteerAngle(curDegree, baseDegree)
def runMotorDistance(self, actuatorVal, actuatorMotorPorts='PORT_B.PORT_C', distance=.1, power=100, initial=False):
"""
Runs a motor for a specific distance of travel
actuatorMotorPorts (str): The ports for the actuation motors (default='PORT_B.PORT_C')
distance (float): Distance the motor should run for (default=.1)
power (int): The power given to the motor (default=100)
"""
if not initial:
self.motors=()
ports = actuatorMotorPorts.split('.')
for port in ports:
self.motors+=Motor(self.nxt.brick,eval(port)),
neededDegree=distance/self.pose.wheelDiameter*180/pi/self.pose.gearRatio*2
baseDegree=getUsefulTacho(self.motors[0])
if int(actuatorVal)==1:
self.direction=self.pose.direction
if self.direction:
print 'Running motor '+str(ports)+' over '+str(distance)+' meters at '+str(power)+' power'
curDegree=getUsefulTacho(self.motors[0])
controlledRun(self,self.motors,power,curDegree,neededDegree)
else:
print 'Running motor '+str(ports)+' over '+str(distance)+' meters at '+str(power)+' power'
curDegree=getUsefulTacho(self.motors[0])
controlledRun(self,self.motors,-power,curDegree,-neededDegree)
sleep(1)
curDegree=getUsefulTacho(self.motors[0])
def runMotorTime(self, actuatorVal, actuatorMotorPorts='PORT_A', actuatorForward=True, actionTime=1.0, actionPower=100, initial=False):
"""
Runs a specified motor for power [-128 to 127] and time in seconds
actuatorMotorPorts (str): The motor ports that allow the robot to do an action (default='PORT_A')
actuatorForward (bool): Whether positive power applies forward motion (default=True)
actionTime (float): The number of seconds to run the action motor (default=1)
actionPower (int): The power to the actuator motor [0-100] (default=100)
"""
if not initial: #no initialization
self.action=()
ports = actuatorMotorPorts.split('.')
for port in ports:
self.action+=Motor(self.nxt.brick,eval(port)),
if int(actuatorVal) == 1: #a correction so that it doesn't run on startup (thanks Cameron!)
if actuatorForward: #if the user wants one direction
print 'Running motor '+str(ports)+' for '+str(actionTime)+' seconds at '+str(actionPower)+' power'
for motor in self.action:
motor.run(actionPower)
sleep(actionTime)
for motor in self.action:
motor.idle()
else: #if the users wants the other direction
print 'Running motor '+str(ports)+' for '+str(actionTime)+' seconds at '+str(actionPower)+' power'
for motor in self.action:
motor.run(-actionPower)
sleep(actionTime)
for motor in self.action:
motor.idle()
def turnOnMotor(self,
actuatorVal,
actuatorMotorPorts='PORT_A.PORT_B',
power=100,
initial=False):
"""
Turns on a given set of motors with specified power
actuatorMotorPorts (str): The ports for the actuation motors (default='PORT_A.PORT_B')
power (int): The power sent to the set of motors (default=100)
"""
if not initial:
self.actuating = True
self.on = ()
ports = actuatorMotorPorts.split('.')
for port in ports:
self.on += Motor(self.nxt.brick, eval(port)),
if int(actuatorVal) == 1:
self.direction = self.loco.leftForward
self.actuatorMotorOn = True
if self.direction:
print 'Turning ' + str(ports) + ' on with power ' + str(
power)
for motor in self.on:
motor.run(power)
else:
print 'Turning ' + str(ports) + ' on with power ' + str(
power)
for motor in self.on:
motor.run(-power)
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 syncmotors(self, power, turns):
if self._bricks:
if not((power < -127) or (power > 127)):
if turns < 0:
turns = abs(turns)
power = -1 * power
try:
motorB = Motor(self._bricks[self.active_nxt], PORT_B)
motorC = Motor(self._bricks[self.active_nxt], PORT_C)
syncmotors = SynchronizedMotors(motorB, motorC, 0)
syncmotors.turn(power, int(turns*360))
except:
raise logoerror(ERROR_GENERIC)
else:
raise logoerror(ERROR_POWER)
else:
raise logoerror(ERROR_BRICK)
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.arms = Motor(brick, PORT_C)
self.left = Motor(brick, PORT_A)
self.right = Motor(brick, PORT_B)
self.direction = HTCompass(brick, PORT_2)
self.ultrasonic = Ultrasonic(brick, PORT_4)
def motorreset(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)
t = m.get_tacho()
self._motor_pos[port_up][self.active_nxt] = t.tacho_count
m.idle()
except:
raise logoerror(ERROR_GENERIC)
else:
raise logoerror(ERROR_PORT_M % port)
else:
raise logoerror(ERROR_BRICK)
def motorposition(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)
t = m.get_tacho()
previous = self._motor_pos[port_up][self.active_nxt]
return (t.tacho_count - previous)
except:
raise logoerror(ERROR_GENERIC)
else:
raise logoerror(ERROR_PORT_M % port)
else:
raise logoerror(ERROR_BRICK)
def motorposition(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)
t = m.get_tacho()
previous = self._motor_pos[port_up][self.active_nxt]
return (t.tacho_count - previous)
except:
raise logoerror(ERROR_GENERIC)
else:
raise logoerror(ERROR_PORT_M % port)
else:
raise logoerror(ERROR_BRICK)
def motorreset(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)
t = m.get_tacho()
self._motor_pos[port_up][self.active_nxt] = t.tacho_count
m.idle()
except:
raise logoerror(ERROR_GENERIC)
else:
raise logoerror(ERROR_PORT_M % port)
else:
raise logoerror(ERROR_BRICK)
def startmotor(self, port, 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)):
try:
m = Motor(self._bricks[self.active_nxt], port)
m.weak_turn(power, 0)
except:
raise logoerror(ERROR_GENERIC)
else:
raise logoerror(ERROR_POWER)
else:
raise logoerror(ERROR_PORT_M % port)
else:
raise logoerror(ERROR_BRICK)
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, str):
brick = find_one_brick(name=brick)
self.brick = brick
self.arms = Motor(brick, PORT_A)
self.legs = [Motor(brick, PORT_B), Motor(brick, PORT_C)]
self.touch = Touch(brick, PORT_1)
self.sound = Sound(brick, PORT_2)
self.light = Light(brick, PORT_3)
self.ultrasonic = Ultrasonic(brick, PORT_4)
def startmotor(self, port, 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)):
try:
m = Motor(self._bricks[self.active_nxt], port)
m.weak_turn(power, 0)
except:
raise logoerror(ERROR_GENERIC)
else:
raise logoerror(ERROR_POWER)
else:
raise logoerror(ERROR_PORT_M % port)
else:
raise logoerror(ERROR_BRICK)
def __init__(self, proj, shared_data, leftDriveMotor='PORT_B', rightDriveMotor='PORT_C', steeringMotor='none', steeringGearRatio=1.0, leftForward=True, rightForward=True):
"""
Locomotion Command handler for NXT Mindstorms.
leftDriveMotor (str): The motor that drives the left side (default='PORT_B')
rightDriveMotor (str): The motor that drives the right side (default='PORT_C')
steeringMotor (str): The motor that controls steering, if applicable (default='none')
steeringGearRatio (float): The gear ratio on the steering control (default=1.0)
leftForward (bool): Whether forward direction is positive power for the left drive motor (default=True)
rightForward (bool): Whether forward direction is positive power for the right drive motor (default=True)
"""
self.nxt = shared_data['NXT_INIT_HANDLER'] # shared data is the nxt and its functions in this case
self.pose = proj.h_instance['pose'] # pose data is useful for travel
self.actuator = proj.h_instance['actuator']
# The following creates a tuple of the drive motors based on user input
# It also derives the left and right motors as well as a steering motor if used
# There are currently two modes of drive, differential and non-differential (car)
self.driveMotors=()
self.differentialDrive = False
self.leftForward = leftForward
self.rightForward = rightForward
if(leftDriveMotor=='PORT_A' or rightDriveMotor=='PORT_A'):
self.driveMotors+=Motor(self.nxt.brick, PORT_A),
if(leftDriveMotor=='PORT_A'):
self.left=Motor(self.nxt.brick, PORT_A)
else:
self.right=Motor(self.nxt.brick, PORT_A)
if(leftDriveMotor=='PORT_B' or rightDriveMotor=='PORT_B'):
self.driveMotors+=Motor(self.nxt.brick, PORT_B),
if(leftDriveMotor=='PORT_B'):
self.left=Motor(self.nxt.brick, PORT_B)
else:
self.right=Motor(self.nxt.brick, PORT_B)
if(leftDriveMotor=='PORT_C' or rightDriveMotor=='PORT_C'):
self.driveMotors+=Motor(self.nxt.brick, PORT_C),
if(leftDriveMotor=='PORT_C'):
self.left=Motor(self.nxt.brick, PORT_C)
else:
self.right=Motor(self.nxt.brick, PORT_C)
self.steerMotor=None
self.steeringRatio=1
if(steeringMotor=='none'):
self.differentialDrive=True
if(not self.differentialDrive):
if(steeringMotor=='PORT_A'): self.steerMotor = Motor(self.nxt.brick, PORT_A)
if(steeringMotor=='PORT_B'): self.steerMotor = Motor(self.nxt.brick, PORT_B)
if(steeringMotor=='PORT_C'): self.steerMotor = Motor(self.nxt.brick, PORT_C)
self.tacho = self.getUsefulTacho(self.steerMotor)
self.steeringRatio=steeringGearRatio # Global variable fro steering gear ratio
self.once=True # start dead reckoning path record only once
def run(self):
# Start a BT connection over to the NXT
print "Initializing Bluetooth"
try:
brick = nxt.bluesock.BlueSock(BRICK_ADDR).connect()
except:
print "Brick {0} not found. Trying search by name...".format(
BRICK_ADDR)
try:
brick = find_one_brick(name=BRICK_NAME)
except:
print "Brick {0} not found".format(BRICK_NAME)
return
bt_motors = (Motor(brick, PORT_A), Motor(brick,
PORT_B), Motor(brick, PORT_C))
while running:
# use shoulder buttons on the game pad to make the omnibot rotate around it's axis.
turnpower = 0
if gp_state['btn_r1']: turnpower = 60
if gp_state['btn_l1']: turnpower = -60
# read and scale joysticks
joy_x = scale(gp_state['move_x'], STICK_RANGE, (-100, 100))
joy_y = scale(gp_state['move_y'], STICK_RANGE, (-100, 100))
# convert joystick x and y to a direction and power (deviation from the centre)
joy_direction = math.atan2(joy_x, joy_y) # in radians
joy_power = (joy_x**2 + joy_y**2)**0.5 # pythagoras
i = 0
for motor in bt_motors:
# for each motor the angle has a different offset (0, 120 and 240 degrees)
angle = i * 2 * 3.1415 / 3 + joy_direction
# motor power calculation. A simple sin.
motorpower = math.sin(angle) * joy_power + turnpower
motorpower = round(clamp(motorpower, (-100, 100)))
motor.run(motorpower, regulated=True)
i += 1
# wait a bit before sending more commands. If we don't the BT buffer overflows.
btloop.throttle()
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 arcTurn(self,
actuatorVal,
actuatorMotorPorts='PORT_B.PORT_C',
power=100,
arcAngle=90,
arcRadius=.5,
initial=False):
"""
For a differential drive robot, run the drive motors such that it creates an arc
with radius arcRadius and through angle arcAngle. VERY INNACURATE
actuatorMotorPorts (str): The ports for the actuation motors (default='PORT_A.PORT_B')
power (int): The desired power to apply to the faster motor (default=100)
arcAngle (int): The angle that you want the robot to arc through in degrees (default=90)
arcRadius (float): The desired radius of the turning arc (default=.5)
"""
if not initial:
self.motors = ()
ports = actuatorMotorPorts.split('.')
for port in ports:
self.motors += Motor(self.nxt.brick, eval(port)),
distance = arcRadius * arcAngle * pi / 180
ri = arcRadius - self.pose.track / 2
ro = arcRadius + self.pose.track / 2
baseTravel = (getUsefulTacho(self.motors[0]) +
getUsefulTacho(self.motors[1])) / 2
curTravel = baseTravel
degrees = (distance / self.pose.wheelDiameter * 360 / pi /
self.pose.gearRatio)
if int(actuatorVal) == 1:
self.direction = self.pose.direction
print 'Arcing through ' + str(
arcAngle) + ' at a radius of ' + str(arcRadius) + 'm'
if self.direction:
while curTravel < baseTravel + abs(degrees):
if arcAngle < 0:
self.motors[0].run(power)
self.motors[1].run(power * ri / ro)
else:
self.motors[1].run(power)
self.motors[0].run(power * ri / ro)
curTravel = (getUsefulTacho(self.motors[0]) +
getUsefulTacho(self.motors[1])) / 2
else:
while curTravel > baseTravel - abs(degrees):
if arcAngle < 0:
self.motors[0].run(power)
self.motors[1].run(power * ri / ro)
else:
self.motors[1].run(power)
self.motors[0].run(power * ri / ro)
curTravel = (getUsefulTacho(self.motors[0]) +
getUsefulTacho(self.motors[1])) / 2
for motor in self.motors:
motor.idle()
class Reality(Output):
def __init__(self,kostkaid="00:16:53:07:F8:5B",homepos=(0,0),pos=(0,0)):
self.kostkaid = kostkaid
self.homepos=homepos
self.pos=pos
def __enter__(self):
self.brick = nxt.locator.find_one_brick(self.kostkaid)
self.motfile = Motor(self.brick,PORT_A)
self.motrank = Motor(self.brick,PORT_B)
self.motz = Motor(self.brick,PORT_C)
return self
def __exit__(self, exc_type, exc_val, exc_tb):
self.motx.idle()
self.moty.idle()
self.motz.idle()
print exc_type,exc_val,exc_tb
def goto(self,loc): print "goto ",loc
def lift(self,hchwyt,hlift): print "lifing from ",hchwyt," to ",hlift
def place(self,ileopuscic): print "opuszczanie o ",ileopuscic," , placing"
def home(self): print "----homing----" ; self.goto(self.HOMEpos)
def __init__(self, brick="NXT"):
r"""Creates a new Strider 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.back_leg = Motor(brick, PORT_B)
self.left_leg = Motor(brick, PORT_C)
self.right_leg = Motor(brick, PORT_A)
# self.touch = Touch(brick, PORT_1)
# self.sound = Sound(brick, PORT_2)
# self.light = Light(brick, PORT_3)
# self.ultrasonic = Ultrasonic(brick, PORT_4)
self.colour = Color20(brick, PORT_3)
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 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 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 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 initialize(self):
super(DrivarNxt, self).initialize()
self.block = nxt.locator.find_one_brick()
self.leftMotor = Motor(self.block, PORT_A)
self.rightMotor = Motor(self.block, PORT_C)
self.penMotor = Motor(self.block, PORT_B)
self.ultrasonicSensor = Ultrasonic(self.block, PORT_4)
self.lightSensor = Light(self.block, PORT_3)
self.initialized = True
def turnOffMotor(self, actuatorVal, actuatorMotorPorts='PORT_A.PORT_B', initial=False):
"""
Turns off a given set of motors
actuatorMotorPorts (str): The ports for the actuation motors (default='PORT_A.PORT_B')
"""
if not initial:
self.actuating=True
self.off=()
ports = actuatorMotorPorts.split('.')
self.actuatorMotorOn=False
for port in ports:
self.off+=Motor(self.nxt.brick,eval(port)),
if int(actuatorVal)==1:
for motor in self.off:
motor.idle()
def tachometer(self, motorPort='PORT_A', degree=0, operator='=', initial=False):
"""
Use the tachometer in the motors to determine True/False
motorPort (str): The port for the motor that you want to read (default=PORT_A)
degree (int): The value you are comparing the tachometer value to (default=0)
operator (str): How the value is being compared to the tachometer value ['<','>','=','!='] (default='=')
"""
if initial:
return False
else:
motor = Motor(self.nxt.brick,eval(motorPort))
data = getUsefulTacho(motor) #minus large number to positive large number
output = operation(data,operator,degree) #get bolean based on user input
if output:
print 'Tachometer value is '+operator+' '+str(degree)
return output
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 __init__(self, brick_name='NXT'):
sock = find_one_brick(name=brick_name)
brick = self.brick = sock.connect()
self.arm = Motor(brick, PORT_A)
self.legs = [Motor(brick, PORT_B), Motor(brick, PORT_C)]
#self.touch = Touch(brick, PORT_1)
#self.sound = Sound(brick, PORT_2)
#self.light = Light(brick, PORT_3)
#self.ultrasonic = Ultrasonic(brick, PORT_4)
self.busy = False
self.arg_d = {"forward": (self.forward, 2),
"backward": (self.backward, 2),
"left": (self.turn_left, 2),
"right": (self.turn_right, 2),
#"power": (self.add_arm_power, 1),
#"kick": (self.release_arm, 0),
"boot": (self.boot, 0),
"sing": (self.sing, 0),
"talk": (self.talk, 0)}
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)
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, PORT_B, PORT_C
from nxt.sensor import Touch, PORT_4, PORT_3, PORT_2, Light, PORT_1, Ultrasonic
light = Light(brick, PORT_4)
turningMotor = Motor(brick, PORT_B)
walkingMotor = Motor(brick, PORT_C)
armMotor = Motor(brick, PORT_A)
touch = Touch(brick, PORT_1)
ultrasonic = Ultrasonic(brick, PORT_2)
compass = Ultrasonic(brick, PORT_3)
# LINE FOLLOW VARIABLES
turningPower = 65 # 70, normalized, motor power used when turning in line follow
negInertiaPower = 70 # 65, normalized, motor power for negative inertia
findLineTimeOut = 0.5 # 0.5, time between switching motor to the opposite direction
negInertiaLengthOnWhite = 0.07 # 0.2, time before braking on negative inertia when originally on white
negInertiaLengthOnBlack = 0.07 # 0.05, time before braking on ngative inertia when originally on black (should be smaller than white to prevent overshooting the line)
# CALIBRATION VARIABLES
calTurningPower = 70 # 70, normalized, motor power used to turn when calibrate
class NXTLocomotionCommandHandler:
def __init__(self, proj, shared_data, leftDriveMotor='PORT_B', rightDriveMotor='PORT_C', steeringMotor='none', steeringGearRatio=1.0, leftForward=True, rightForward=True):
"""
Locomotion Command handler for NXT Mindstorms.
leftDriveMotor (str): The motor that drives the left side (default='PORT_B')
rightDriveMotor (str): The motor that drives the right side (default='PORT_C')
steeringMotor (str): The motor that controls steering, if applicable (default='none')
steeringGearRatio (float): The gear ratio on the steering control (default=1.0)
leftForward (bool): Whether forward direction is positive power for the left drive motor (default=True)
rightForward (bool): Whether forward direction is positive power for the right drive motor (default=True)
"""
self.nxt = shared_data['NXT_INIT_HANDLER'] # shared data is the nxt and its functions in this case
self.pose = proj.h_instance['pose'] # pose data is useful for travel
self.actuator = proj.h_instance['actuator']
# The following creates a tuple of the drive motors based on user input
# It also derives the left and right motors as well as a steering motor if used
# There are currently two modes of drive, differential and non-differential (car)
self.driveMotors=()
self.differentialDrive = False
self.leftForward = leftForward
self.rightForward = rightForward
if(leftDriveMotor=='PORT_A' or rightDriveMotor=='PORT_A'):
self.driveMotors+=Motor(self.nxt.brick, PORT_A),
if(leftDriveMotor=='PORT_A'):
self.left=Motor(self.nxt.brick, PORT_A)
else:
self.right=Motor(self.nxt.brick, PORT_A)
if(leftDriveMotor=='PORT_B' or rightDriveMotor=='PORT_B'):
self.driveMotors+=Motor(self.nxt.brick, PORT_B),
if(leftDriveMotor=='PORT_B'):
self.left=Motor(self.nxt.brick, PORT_B)
else:
self.right=Motor(self.nxt.brick, PORT_B)
if(leftDriveMotor=='PORT_C' or rightDriveMotor=='PORT_C'):
self.driveMotors+=Motor(self.nxt.brick, PORT_C),
if(leftDriveMotor=='PORT_C'):
self.left=Motor(self.nxt.brick, PORT_C)
else:
self.right=Motor(self.nxt.brick, PORT_C)
self.steerMotor=None
self.steeringRatio=1
if(steeringMotor=='none'):
self.differentialDrive=True
if(not self.differentialDrive):
if(steeringMotor=='PORT_A'): self.steerMotor = Motor(self.nxt.brick, PORT_A)
if(steeringMotor=='PORT_B'): self.steerMotor = Motor(self.nxt.brick, PORT_B)
if(steeringMotor=='PORT_C'): self.steerMotor = Motor(self.nxt.brick, PORT_C)
self.tacho = self.getUsefulTacho(self.steerMotor)
self.steeringRatio=steeringGearRatio # Global variable fro steering gear ratio
self.once=True # start dead reckoning path record only once
def sendCommand(self, cmd):
"""
Send movement command to the NXT
"""
# general power specifications
leftPow = BACK
if self.leftForward: leftPow = FORTH
rightPow = BACK
if self.rightForward: rightPow = FORTH
def forward(sec, power): #currently unused
'''allows for forward movement with power and for time'''
for motor in self.driveMotors:
motor.run(power)
sleep(sec)
for motor in self.driveMotors:
motor.idle()
def go(power):
'''turns the drive motors on with given power'''
for motor in self.driveMotors:
motor.run(power)
def carTurn(sec, direction): #currently unused
'''specifies time based steering'''
self.steerMotor.run(direction)
forward(sec,leftPow)
self.steerMotor.idle()
def left(leftPow, power):
'''used for differential drive on left turns'''
self.left.run(power)
self.right.run(leftPow)
def right(rightPow, power):
'''used for differential drive on right turns'''
self.left.run(rightPow)
self.right.run(power)
def idle():
'''sets all motors to idle state'''
for motor in self.driveMotors:
motor.idle()
if(not self.differentialDrive):
self.steerMotor.idle()
def goToDegree(degree):
"""
Takes a degree measurement (from the tachometer) and carefully aligns the steering motor to that degree
"""
curDegree = self.getUsefulTacho(self.steerMotor) #currently angled at this degree
baseDegree=curDegree
degreeRange = abs(curDegree-degree) #distance in degrees it has to run
leftPower = -75.0 #full power for steering
rightPower = 75.0
powerRange=rightPower-MIN #power range for steering, MIN is the minimum power for movement on steering
while(curDegree>degree+RANGE or curDegree<degree-RANGE): #checks to see if the current angle is close enough to the desired
while(curDegree>degree+RANGE):
if(abs(curDegree-degree)<30): leftPower=-MIN #small angle change necessitates small power
elif(abs(curDegree-degree)>degreeRange): leftPower = -75 #large angle change necessitates max power
else: leftPower = -(((abs(curDegree-degree)/degreeRange)*powerRange)+MIN) #As you get closer to the angle, decrease power to steering motor
self.steerMotor.run(leftPower)
lastDegree=curDegree
curDegree=self.getUsefulTacho(self.steerMotor) #get new current degree
if(lastDegree==curDegree): break #implies the motor is stuck
if(self.v==0): break #check for pause...
self.steerMotor.idle() #always idle motors before giving power incase opposite direction
curDegree=self.getUsefulTacho(self.steerMotor) #recheck current degre
while(curDegree<degree-RANGE): #Same as above
if(abs(curDegree-degree)<30): rightPower=MIN
elif(abs(curDegree-degree)>degreeRange): rightPower = 75
else: rightPower = (((abs(degree-curDegree)/degreeRange)*powerRange)+MIN)
self.steerMotor.run(rightPower)
lastDegree=curDegree
curDegree=self.getUsefulTacho(self.steerMotor)
if(lastDegree==curDegree): break
if(self.v==0): break # check for pause...
self.steerMotor.idle()
curDegree=self.getUsefulTacho(self.steerMotor)
if(self.v==0): break # check for pause...
self.steerMotor.idle()
try:
self.pose.updateSteerAngle(curDegree,baseDegree)
except:
pass
def difDrive():
"""
Methodology behind a differential drive. It uses facing direction and needed direction
"""
stopped=False
angle = self.angle*180/pi
# if angle > 0 or angle < 0:
#print 'angle='+str(angle)+' w='+str(self.angle)
leftPow = -80 #max powers
if self.leftForward: leftPow = 80
rightPow = -80
if self.rightForward: rightPow = 80
try:
if(self.v==0 and not self.actuator.actuatorMotorOn):
idle() #pause...
stopped=True
except:
if(self.v==0):
idle()
stopped=True
if stopped:
pass
elif angle>.5: #left turning arc
if(leftPow>0): arcPower=((leftPow-LOW)*(90-abs(angle))/90)+LOW # scaled power based on required omega
else: arcPower=((leftPow+LOW)*(90-abs(angle))/90)-LOW
left(leftPow,arcPower)
elif angle<-.5: #right tuning arc
if(rightPow>0): arcPower=((rightPow-LOW)*(90-abs(angle))/90)+LOW
else: arcPower=((rightPow+LOW)*(90-abs(angle))/90)-LOW
right(rightPow,arcPower)
else:
go(leftPow) #straight
stopped=False
def nonDifDrive():
"""
Methodology behind a car type drive. It checks current pose and uses given vx and vy to calculate whether it should be turning or not.
"""
pose = self.pose.getPose()
angle = pose[2]*180/pi+90 #Facing Direction
stopped=False
#Orient facing direction between -180 and 180
while(angle>180): angle-=360
while(angle<-180): angle+=360
phi = atan2(vy,vx)*180/pi #Direction to POI
try:
if(self.v==0 and not self.actuator.actuatorMotorOn): #pause command sent
idle()
stopped=True
except:
if(self.v==0):
idle()
stopped=True
if stopped:
pass
elif(phi+360-angle<angle-phi): #quadrant 3 angle and quadrant 2 phi
#left
degree=-MAX_ANGLE*self.steeringRatio
goToDegree(degree)
elif(angle+360-phi<phi-angle): #quadrant 2 angle and quadrant 3 phi
#right
degree=MAX_ANGLE*self.steeringRatio
goToDegree(degree)
elif(phi+RANGE<angle): #right turn to line up
#right
degree=MAX_ANGLE*self.steeringRatio
goToDegree(degree)
elif(phi-RANGE>angle): #left turn to line up
#left
degree=-MAX_ANGLE*self.steeringRatio
goToDegree(degree)
else: #general straight direction
#straight
degree=self.tacho
goToDegree(degree)
if(self.v!=0): #run drive motors
go(leftPow*.65)
stopped=False
pose = self.pose.getPose() #get pose from vicon
vx=cmd[0] #decompose velocity
vy=cmd[1]
if self.leftForward:
theta = pose[2]-(pi/2) #get facing angle (account for vicon axes)
else:
theta = pose[2]+(pi/2)
self.v = cos(theta)*vx+sin(theta)*vy #magnitude of v
if self.once:
try:
self.pose.setPose()
except:
print 'Not setting pose with dead reckoning'
pass
self.once=False
if(self.differentialDrive): #handles differential drive
#theta=theta-pi #orient angle for reverse direction of travel
self.angle = atan2(vy,vx) - theta
#print 'Vx: '+str(vx)+' Vy: '+str(vy)+' theta: '+str(theta)
while self.angle>pi: self.angle-=2*pi
while self.angle<-pi: self.angle+=2*pi
difDrive()
else: #handles car type drive
nonDifDrive()
def getUsefulTacho(self, motor):
"""Turns instance data from tachometer in to useful integer"""
# the tachometer data from the nxt is not useful in current form, this provides usability
tacho = tuple(int(n) for n in str(motor.get_tacho()).strip('()').split(','))
return tacho[0]
class Strider(object):
def __init__(self, brick="NXT"):
r"""Creates a new Strider 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.back_leg = Motor(brick, PORT_B)
self.left_leg = Motor(brick, PORT_C)
self.right_leg = Motor(brick, PORT_A)
# self.touch = Touch(brick, PORT_1)
# self.sound = Sound(brick, PORT_2)
# self.light = Light(brick, PORT_3)
# self.ultrasonic = Ultrasonic(brick, PORT_4)
self.colour = Color20(brick, PORT_3)
def walk(self, direction, time=0):
[back, left, right] = direction.get_directions()
self.back_leg.run(back * 80)
self.left_leg.run(left * 80)
self.right_leg.run(right * 80)
if time > 0:
sleep(time)
self.stop()
def show_colour(self, colour):
self.colour.set_light_color(colour)
def colour_display(self):
for colour in [Type.COLORRED, Type.COLORGREEN, Type.COLORBLUE]:
self.show_colour(colour)
sleep(2)
self.show_colour(Type.COLORNONE)
def stop(self):
self.back_leg.idle()
self.left_leg.idle()
self.right_leg.idle()
def fire_lasers(self):
for i in range(0, 5):
self.brick.play_sound_file(False, "! Laser.rso")
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)
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 AlphaRex(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, str):
brick = find_one_brick(name=brick)
self.brick = brick
self.arms = Motor(brick, PORT_A)
self.legs = [Motor(brick, PORT_B), Motor(brick, PORT_C)]
self.touch = Touch(brick, PORT_1)
self.sound = Sound(brick, PORT_2)
self.light = Light(brick, PORT_3)
self.ultrasonic = Ultrasonic(brick, PORT_4)
def echolocate(self):
r'''Reads the Ultrasonic sensor's output.
'''
return self.ultrasonic.get_sample()
def feel(self):
r'''Reads the Touch sensor's output.
'''
return self.touch.get_sample()
def hear(self):
r'''Reads the Sound sensor's output.
'''
return self.sound.get_sample()
def say(self, line, times=1):
r'''Plays a sound file named (line + '.rso'), which is expected to be
stored in the brick. The file is played (times) times.
line
The name of a sound file stored in the brick.
times
How many times the sound file will be played before this method
returns.
'''
for i in range(0, times):
self.brick.play_sound_file(False, line + '.rso')
sleep(1)
def see(self):
r'''Reads the Light sensor's output.
'''
return self.light.get_sample()
def walk(self, secs, power=FORTH):
r'''Simultaneously activates the leg motors, causing Alpha Rex to walk.
secs
How long the motors will rotate.
power
The strength effected by the motors. Positive values will cause
Alpha Rex to walk forward, while negative values will cause it
to walk backwards. If you are unsure about how much force to
apply, the special values FORTH and BACK provide reasonable
defaults. If omitted, FORTH is used.
'''
for motor in self.legs:
motor.run(power=power)
sleep(secs)
for motor in self.legs:
motor.idle()
def wave(self, secs, power=100):
r'''Make Alpha Rex move its arms.
secs
How long the arms' motor will rotate.
power
The strength effected by the motor. If omitted, (100) is used.
'''
self.arms.run(power=power)
sleep(secs)
self.arms.idle()
class NxtRobot(Robot):
FULL_ROTATE_FACTOR = 5.5
CELL_MOVE_FACTOR = 2.35
def __init__(self, world):
Robot.__init__(self, world, 2, Orientation.NORTH)
self.noise = 0
self.brick = find_one_brick()
self.motor1 = Motor(self.brick, PORT_B)
self.motor2 = Motor(self.brick, PORT_C)
self.ultrasonic = Ultrasonic(self.brick, PORT_4)
self.motor1.reset_position(False)
self.motor2.reset_position(False)
def move(self, direction):
if direction == Direction.FORWARD:
pass
elif direction == Direction.RIGHT:
self._rotate(-0.25)
pass
elif direction == Direction.BACK:
self._rotate(0.5)
pass
elif direction == Direction.LEFT:
self._rotate(0.25)
pass
m = self.ultrasonic.get_sample()
if m > 26:
t1 = threading.Thread(target=self.motor1.turn, args=(50, self.CELL_MOVE_FACTOR * 360))
t2 = threading.Thread(target=self.motor2.turn, args=(50, self.CELL_MOVE_FACTOR * 360))
t1.start()
t2.start()
t1.join()
t2.join()
def measure(self):
m = []
for i in range(4):
m.append(self.ultrasonic.get_sample())
self._rotate(0.25)
return m
def sync(self):
m = self.ultrasonic.get_sample()
while m == 255:
self._rotate(0.17)
m = self.ultrasonic.get_sample()
def _rotate(self, degree):
if degree == 0:
return
sign = 1
if degree < 0:
sign = -1
degree = abs(degree)
t1 = threading.Thread(target=self.motor1.turn, args=(sign * 50, self.FULL_ROTATE_FACTOR * 360 * degree))
t2 = threading.Thread(target=self.motor2.turn, args=(sign * -50, self.FULL_ROTATE_FACTOR * 360 * degree))
t1.start()
t2.start()
t1.join()
t2.join()
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
#!/usr/bin/env python
import nxt.bluesock
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)
class Mindstorm_Robot:
def __init__(self, brick_name='NXT'):
sock = find_one_brick(name=brick_name)
brick = self.brick = sock.connect()
self.arm = Motor(brick, PORT_A)
self.legs = [Motor(brick, PORT_B), Motor(brick, PORT_C)]
#self.touch = Touch(brick, PORT_1)
#self.sound = Sound(brick, PORT_2)
#self.light = Light(brick, PORT_3)
#self.ultrasonic = Ultrasonic(brick, PORT_4)
self.busy = False
self.arg_d = {"forward": (self.forward, 2),
"backward": (self.backward, 2),
"left": (self.turn_left, 2),
"right": (self.turn_right, 2),
#"power": (self.add_arm_power, 1),
#"kick": (self.release_arm, 0),
"boot": (self.boot, 0),
"sing": (self.sing, 0),
"talk": (self.talk, 0)}
def interpret(self, c):
self.interpret_exception(c)
#try: return self.interpret_exception(c)
#except Exception as e: print(e); return "FAIL"
def interpret_exception(self, c):
i = c.rest[0]
func, args = self.arg_d[i]
func(*map(int, c.rest[1:1+args]))
def stop(self):
self.arm.stop()
for m in self.legs:
m.stop()
def forward(self, X, P):
if not (1 <= X <= 20 and 5 <= P <= 100):
return "FAIL"
self.busy = True
for motor in self.legs:
motor.run(power=P)
time.sleep(X/5)
self.stop()
self.busy = False
return "SUCCESS"
def backward(self, X, P):
if not (1 <= X <= 20 and 5 <= P <= 100):
return "FAIL"
self.busy = True
for motor in self.legs:
motor.run(power=-P)
time.sleep(X/5)
self.stop()
self.busy = False
return "SUCCESS"
def turn_left(self, X, P):
if not (1 <= X <= 20 and 5 <= P <= 100):
return "FAIL"
self.busy = True
self.legs[0].run(power=P)
self.legs[1].run(power=-P)
time.sleep(X/5)
self.stop()
self.busy = False
return "SUCCESS"
def turn_right(self, X, P):
if not (1 <= X <= 20 and 5 <= P <= 100):
return "FAIL"
self.busy = True
self.legs[0].run(power=-P)
self.legs[1].run(power=P)
time.sleep(X/5)
self.stop()
self.busy = False
return "SUCCESS"
def add_arm_power(self, F):
return self.reel_arm(int(F/2), 20)
def reel_arm(self, X, P):
if not (1 <= X <= 20 and 5 <= P <= 100):
return "FAIL"
self.busy = True
self.arm.run(power=-P)
time.sleep(X/5)
self.stop()
self.busy = False
return "SUCCESS"
def boot(self):
self.add_arm_power(20)
def release_arm(self):
X = 5
P = 60
self.busy = True
self.arm.run(power=P)
time.sleep(X/5)
self.stop()
self.busy = False
return "SUCCESS"
def play(self, note, dur = 500):
if note:
self.brick.play_tone_and_wait(note, dur)
else:
time.sleep(0.5)
def sing(self):
C = 523
D = 587
E = 659
G = 784
R = None
#self.busy = True
for note in [E, D, C, D, E, E, E, R, \
D, D, D, R, \
E, G, G, R, E, D, C, D, E, E, E, E, D, D, E, D, C]:
self.play(note)
#self.busy = False
return "SUCCESS"
def talk(self):
C = 523
D = 587
E = 659
G = 784
notes = C,D,E,G
play_these = [choice(notes) for i in range(randint(2, 8))]
#self.busy = True
for p in play_these:
self.play(p, dur = randint(50, 800))
#self.busy = False
return "SUCCESS"
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)
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, PORT_B, PORT_C
from nxt.sensor import Touch, PORT_4, PORT_3, PORT_2, Light, PORT_1, Ultrasonic
turningMotor = Motor(brick, PORT_B)
walkingMotor = Motor(brick, PORT_C)
armMotor = Motor(brick, PORT_A)
legPosition = Touch(brick, PORT_3)
touch = Touch(brick, PORT_1)
while True:
print(touch.get_input_values().calibrated_value)
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.sensor import Light, Touch
from nxt.sensor import PORT_1, PORT_2, PORT_3, PORT_4
from nxt.motor import Motor, PORT_A, PORT_B, PORT_C
motorLeft = Motor(brick, PORT_B)
motorRight = Motor(brick, PORT_C)
light = Light(brick, PORT_3)
touch = Touch(brick, PORT_4)
def calibrate():
# turn on light sensor
light.set_illuminated(True)
# zeroes motor position
motorLeft.reset_position(False)
motorRight.reset_position(False)
# calibrates black value
black = light.get_lightness()
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()
def __enter__(self): self.brick = nxt.locator.find_one_brick(self.kostkaid) self.motfile = Motor(self.brick,PORT_A) self.motrank = Motor(self.brick,PORT_B) self.motz = Motor(self.brick,PORT_C) return self