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 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 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)
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)
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 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 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
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 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 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 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()
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:
light.get_lightness()
motor.run(power=-10)
light.get_lightness()
print("light = ", light.get_lightness())
angle = motor.get_tacho()
print(angle)
motor.idle()
'''
lightSensor = Light(brick, PORT_1)
gyro = Gyro(brick, PORT_2)
sum = 0
gyro.calibrate()
updatePeriod = .01
while True:
dps = gyro.get_rotation_speed()
# integrate
sum += dps * updatePeriod
sleep(updatePeriod)
print(sum)
def motor_idle(letter):
m = Motor(b, MOTORS[letter.upper()])
m.idle()
return 'OK'
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)
class Seng(object):
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 echolocate(self):
r'''Reads the Ultrasonic sensor's output.
'''
return self.ultrasonic.get_sample()
def compass(self):
return self.direction.get_sample()
def walk_seng(self, secs, power):
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.
'''
self.left.run(power=power)
self.right.run(power=power)
sleep(secs)
self.left.idle()
self.right.idle()
def walk_seng_start(self, power):
self.left.run(power=power)
self.right.run(power=power)
def walk_seng_stop(self):
self.left.idle()
self.right.idle()
def turn_seng(self, secs, power):
self.left.run(power=power)
self.right.run(power=-power)
sleep(secs)
self.left.idle()
self.right.idle()
def turn_seng_start(self, power):
self.left.run(power=power)
self.right.run(power=-power)
def turn_seng_stopp(self):
self.left.idle()
self.right.idle()
def legPosition():
touchState = touch.get_input_values().calibrated_value
if touchState == 829 or touchState == 810:
return True
else:
return False
def step(forwardPower = 120):
walkingMotor.run(forwardPower)
sleep(.2) # give it time to move off touch sensor
while not legPosition():
pass
walkingMotor.run(0)
walkingMotor.brake()
return
outFile = open('compassValueswithMagnet.txt', 'w')
try:
while True:
step()
compassVal = compass.get_distance()
outFile.write('%f\n' % compassVal)
print(compassVal)
except:
outFile.close()
walkingMotor.idle()
outName = 'FinalCal_' + binType + '.txt'
outfile = open(outName, 'w')
for i in range(25):
timeDiff = binPickup()
outfile.write('\n%f' % timeDiff)
print(timeDiff)
sleep(.5)
binDropOff()
raw_input()
outfile.close()
try:
main()
except KeyboardInterrupt:
armMotor.idle()
walkingMotor.idle()
turningMotor.idle()
'''
def binIDTest():
battLevel = brick.get_battery_level()
print(battLevel)
binType = raw_input('Input bin type: ')
fileName = 'BinID_Dec4_normalized' + binType + '.txt'
outputFile = open(fileName, 'w')
repeat = ''
for i in range(25):
binTime = binPickup()
sleep(.3)
binDropOff()
print(binTime)
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]
