def main(robot):
# Définition des moteurs / capteurs
m_left = Motor(robot, PORT_B)
m_right = Motor(robot, PORT_A)
touch_right = Touch(robot, PORT_2)
touch_left = Touch(robot, PORT_1)
ultrason = Ultrasonic(robot, PORT_3)
DEFAULT_POWER = 80
DISTANCE_LIMIT = 20
TURN_TIME = 1.5
# Début
while True:
m_left.run(power=DEFAULT_POWER)
m_right.run(power=DEFAULT_POWER)
while not touch_right.is_pressed() and not touch_left.is_pressed() and ultrason.get_distance() > DISTANCE_LIMIT:
sleep(0.01)
print("Aieee")
if touch_right.is_pressed():
m_left.run(power=DEFAULT_POWER)
m_left.run(power=-DEFAULT_POWER)
else:
m_left.run(power=-DEFAULT_POWER)
m_left.run(power=DEFAULT_POWER)
sleep(TURN_TIME)
class Car:
brick = None
left = None
right = None
def __init__(self):
brick = nxt.locator.find_one_brick()
self.brick = brick
self.left = Motor(brick, PORT_A)
self.right = Motor(brick, PORT_C)
self.light = Light(brick, PORT_4)
# self.ultrasonic = Ultrasonic(brick, PORT_4)
print "Connection established."
def turn(self, angle=100, speed=30):
if angle > 0:
self.left.turn(angle, speed)
elif angle < 0:
self.right.turn(-angle, speed)
def forward(self, distance=50, speed=30):
self.left.run(power=speed)
self.right.run(power=speed)
sleep(distance / speed)
self.left.idle()
self.right.idle()
def range(self):
return self.ultrasonic.get_sample()
def surface(self):
return self.light.get_sample()
class 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")
from time import sleep
# see files in library ( /usr/local/lib/python2.7/dist-packages/nxt )
# for a more comprehensive list of ports / commands available
from nxt.motor import Motor, PORT_A, PORT_B, PORT_C
from nxt.sensor import Light, Sound, Touch, Ultrasonic
from nxt.sensor import PORT_1, PORT_2, PORT_3, PORT_4
# use try with finally to stop motors at end, even if program
# encountered an (programming) error.
try:
touchSensor = Touch(brick, PORT_1) # plug touch sensor into Port 1
motor = Motor(brick, PORT_A) # plug motor into Port A
# Note: |Power| <50 might not be strong enough to turn motor /
# overcome the internal friction
motor.run(power = 70) # go forward
sleep(2.5) # let NXT do its thing for 2.5 seconds
motor.run(power = -70) # go backward
sleep(2)
# will read when this line of code is reached, so KEEP sensor
# pressed till then
print("Current touch sensor state: {}".format(
touchSensor.get_sample()))
finally:
Motor(brick, PORT_A).idle() # otherwise motor keeps running
print("Terminating Program")
brick.sock.close()
#!/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 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 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"
import nxt
import nxtConnect # has to be in search path
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
light = Light(brick, PORT_1)
turningMotor = Motor(brick, PORT_B)
walkingMotor = Motor(brick, PORT_C)
legPosition = Touch(brick, PORT_3)
#ultrasonic = Sonar(brick, PORT_2)
walkingMotor.run(power=120)
class Robot(object):
LEFT_WHEEL = 0x02 # port C
RIGHT_WHEEL = 0x00 # port A
KICKER = 0x01 # port B
DEFAULT_POWER = 80
TURN_POWER = 0.8
BUZZER_HZ = 769
KICK_DISTANCE = 90
STATE_DISCONNECTED = -1
STATE_IDLE = 0
STATE_UP = 1
STATE_DOWN = 2
STATE_RIGHT = 3
STATE_LEFT = 4
MAX_MOTOR_POWER = 127
#NAME = "BrickAshley"
NAME = "BrickAsh"
def __init__(self, host=None):
self.power = -1 * self.DEFAULT_POWER
self.address = host
self.state = self.STATE_DISCONNECTED
self.log = logging.getLogger("Robot")
def connect(self):
self.log.info("Connecting ...")
try:
if self.address == None:
self.brick = nxt.find_one_brick().connect()
else:
self.brick = BlueSock(self.address).connect()
except nxt.locator.BrickNotFoundError:
raise RobotNotFoundError
except Exception as error:
raise RobotConnectionError(error)
self.leftWhell = Motor(self.brick, self.LEFT_WHEEL)
self.rightWhell = Motor(self.brick, self.RIGHT_WHEEL)
self.kicker = Motor(self.brick, self.KICKER)
self.log.info("Set up Motors")
try:
#self.kicker.turn(100, 100, brake=True)
self.log.debug(self.__read_motor_state(self.KICKER))
except Exception as error:
self.log.error("kicker reset error: " + str(error))
self.state = self.STATE_IDLE
self.__get_info()
self.log.info("Conected to {name}".format(name=self.name))
self.buzz()
def disconnect(self):
try:
self.brick = None
#self.get_info_thread.stop()
gc.collect()
except:
self.log.warning("Unsafe disconect")
self.state = self.STATE_DISCONNECTED
def get_name(self):
self.__get_info()
return self.name
def set_name(self, name):
self.brick.set_brick_name(name)
self.disconnect()
self.connect()
self.__get_info()
def set_power(self, value):
if value < -1 * self.MAX_MOTOR_POWER or value > self.MAX_MOTOR_POWER:
raise ValueError("Power can only be +-127")
else:
self.power = value
self.log.info("power set to: " + str(self.power))
def get_power(self):
return self.power
def __get_info(self):
#self.get_info_thread = threading.Timer(30, self.__get_info)
#self.get_info_thread.start()
self.name, self.host, self.signal_strength, self.user_flash = self.brick.get_device_info(
)
self.battery = self.brick.get_battery_level()
self.log.info(
"Info: \n\tName: {name}" \
"\n\tBT MAC: {host}\n\tBT signal: {signal}\n\t" \
"Memory: {memory}\n\tBattery: {voltage}mV".format(name=self.name, host=self.host, \
signal=self.signal_strength, memory=self.user_flash, voltage=self.battery)
)
def up(self):
self.log.debug("go up")
if self.state != self.STATE_UP:
self.state = self.STATE_UP
self.leftWhell.run(power=self.power)
self.rightWhell.run(power=self.power)
def down(self):
self.log.debug("go down")
if self.state != self.STATE_DOWN:
self.state = self.STATE_DOWN
self.leftWhell.run(power=-1 * self.power)
self.rightWhell.run(power=-1 * self.power)
def right(self, withBrake=False):
self.log.debug("go right")
if self.state != self.STATE_RIGHT:
self.state = self.STATE_RIGHT
self.leftWhell.run(power=self.power * self.TURN_POWER)
if withBrake:
self.rightWhell.brake()
else:
self.rightWhell.run(power=-self.power * self.TURN_POWER)
def left(self, withBrake=False):
self.log.debug("go left")
if self.state != self.STATE_LEFT:
self.state = self.STATE_LEFT
if withBrake:
self.leftWhell.brake()
else:
self.leftWhell.run(power=-self.power * self.TURN_POWER)
self.rightWhell.run(power=self.power * self.TURN_POWER)
def stop(self):
self.log.debug("go stop")
self.state = self.STATE_IDLE
self.leftWhell.brake()
self.rightWhell.brake()
def buzz(self):
self.log.debug("buzz")
self.brick.play_tone_and_wait(self.BUZZER_HZ, 1000)
def kick(self):
self.log.debug("kick")
self.kicker.turn(-127, self.KICK_DISTANCE, brake=True)
threading.Timer(1.5, self.__kick_reset).start()
def __kick_reset(self):
self.kicker.turn(127, self.KICK_DISTANCE, brake=True)
#def __del__(self):
# self.log.debug("__del__")
# if self.brick != None:
# self.disconnect()
def __read_motor_state(self, port):
values = self.brick.get_output_state(port)
self.log.debug("__read_motor_state: values='{0}'".format(values))
#state, tacho = get_tacho_and_state(values)
#self.log.debug("__read_motor_state: state='{0}', tacho='{1}'".format(state, tacho))
left, kick, right = values[-3:]
if port == self.KICKER:
return kick
elif port == self.LEFT_WHEEL:
return left
elif port == self.RIGHT_WHEEL:
return left
else:
raise Exception("meh")
def get_state(self):
self.__read_motor_state(self.KICKER)
self.__read_motor_state(self.LEFT_WHEEL)
self.__read_motor_state(self.RIGHT_WHEEL)
def kick_to(self, angle, kpower=127, withBrake=True):
state, tacho = self.__read_motor_state(self.KICKER)
if angle < tacho:
self.kicker.turn(-kpower, tacho - angle, brake=withBrake)
else:
self.kicker.turn(+kpower, angle - tacho, brake=withBrake)
class Robot(object):
LEFT_WHEEL = 0x02 # port C
RIGHT_WHEEL = 0x00 # port A
KICKER = 0x01 # port B
DEFAULT_POWER = 80
TURN_POWER = 0.8
BUZZER = 769
#NAME = "BrickAshley"
NAME = "BrickAsh"
def __init__(self, host=None):
self.power = self.DEFAULT_POWER
self.address = host
self.log = logging.getLogger("Robot")
self.connect()
self.leftWhell = Motor(self.brick, self.LEFT_WHEEL)
self.rightWhell = Motor(self.brick, self.RIGHT_WHEEL)
self.kicker = Motor(self.brick, self.KICKER)
self.log.info("Set up Motors")
try:
#self.kicker.turn(100, 100, brake=True)
self.log.debug(self.__read_motor_state(self.KICKER))
except Exception as error:
self.log.error("kicker reset error: " + str(error))
def connect(self):
self.log.info("Connecting ...")
try:
if self.address == None:
self.brick = nxt.find_one_brick().connect()
else:
self.brick = BlueSock(self.address).connect()
except nxt.locator.BrickNotFoundError:
raise RobotNotFoundError
except Exception as error:
raise RobotConnectionError(error)
self.__get_info()
self.log.info("Conected to {name}".format(name=self.name))
def disconnect(self):
try:
self.brick = None
#self.get_info_thread.stop()
gc.collect()
except:
self.log.warning("Unsafe disconect")
pass
def get_name(self):
self.__get_info()
return self.name
def set_name(self, name):
self.brick.set_brick_name(name)
self.disconnect()
self.connect()
self.__get_info()
def set_power(self, value):
value=int(value)
if value < -127 or value > 127:
raise ValueError("Power can only be +-127")
self.power = value
def get_power(self):
return self.power
def __get_info(self):
#self.get_info_thread = threading.Timer(30, self.__get_info)
#self.get_info_thread.start()
self.name, self.host, self.signal_strength, self.user_flash = self.brick.get_device_info()
self.battery = self.brick.get_battery_level()
self.log.info(
"Info: \n\tName: {name}" \
"\n\tBT MAC: {host}\n\tBT signal: {signal}\n\t" \
"Memory: {memory}\n\tBattery: {voltage}mV".format(name=self.name, host=self.host, \
signal=self.signal_strength, memory=self.user_flash, voltage=self.battery)
)
def up(self):
self.log.debug("go up")
self.leftWhell.run(power=self.power)
self.rightWhell.run(power=self.power)
def down(self):
self.log.debug("go down")
self.brick.play_tone_and_wait(self.BUZZER, 1000)
self.leftWhell.run(power=-self.power)
self.rightWhell.run(power=-self.power)
def right(self, withBrake=False):
self.log.debug("go right")
self.leftWhell.run(power=self.power*self.TURN_POWER)
if withBrake:
self.rightWhell.brake()
else:
self.rightWhell.run(power=-self.power*self.TURN_POWER)
def left(self, withBrake=False):
self.log.debug("go left")
if withBrake:
self.leftWhell.brake()
else:
self.leftWhell.run(power=-self.power*self.TURN_POWER)
self.rightWhell.run(power=self.power*self.TURN_POWER)
def stop(self):
self.log.debug("go stop")
self.leftWhell.brake()
self.rightWhell.brake()
#self.kicker.brake()
def buzz(self):
self.log.debug("buzz")
self.brick.play_tone_and_wait(self.BUZZER, 1000)
def kick(self):
self.log.debug("kick")
self.kicker.turn(-127, 85, brake=True)
threading.Timer(1.5, self.__kick_reset).start()
def __kick_reset(self):
self.kicker.turn(127, 90, brake=False)
#def __del__(self):
# self.log.debug("__del__")
# if self.brick != None:
# self.disconnect()
def __read_motor_state(self, port):
values = self.brick.get_output_state(port)
self.log.debug("__read_motor_state: values='{0}'".format(values))
#state, tacho = get_tacho_and_state(values)
#self.log.debug("__read_motor_state: state='{0}', tacho='{1}'".format(state, tacho))
left, kick, right = values[-3:]
if port == self.KICKER:
return kick
elif port == self.LEFT_WHEEL:
return left
elif port == self.RIGHT_WHEEL:
return left
else:
raise Exception("meh")
def get_state(self):
self.__read_motor_state(self.KICKER)
self.__read_motor_state(self.LEFT_WHEEL)
self.__read_motor_state(self.RIGHT_WHEEL)
def kick_to(self, angle, kpower=127, withBrake=True):
state, tacho = self.__read_motor_state(self.KICKER)
if angle < tacho:
self.kicker.turn(-kpower, tacho-angle, brake=withBrake)
else:
self.kicker.turn(+kpower, angle-tacho, brake=withBrake)
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
#######################################################################
## Then, you can specify what you want the NXT to do
#######################################################################
from time import sleep
from nxt.motor import Motor, PORT_A, PORT_B, PORT_C
from nxt.sensor import Touch, PORT_4
motorLeft = Motor(brick, PORT_B)
motorRight = Motor(brick, PORT_C)
touch = Touch(brick, PORT_4)
motorLeft.run(power = -70)
motorRight.run(power = -120)
#######################################################################
## Then, you can specify what you want the NXT to do
#######################################################################
from time import sleep
from nxt.motor import Motor, PORT_A, PORT_B, PORT_C
from nxt.sensor import Touch, PORT_4, PORT_3
turningMotor = Motor(brick, PORT_B)
walkingMotor = Motor(brick, PORT_C)
turnerSwitch = Touch(brick, PORT_4)
legPosition = Touch(brick, PORT_3)
while True:
if turnerSwitch.is_pressed() == False:
turningMotor.run(power=0)
a = 0
walkingMotor.run(power=120)
else:
if legPosition.is_pressed() == False:
walkingMotor.run(power=100)
else:
a = 1
if a == 1:
walkingMotor.run(power=0)
walkingMotor.brake()
turningMotor.run(power=120)
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 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 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()
class Robot(object):
LEFT_WHEEL = 0x02 # port C
RIGHT_WHEEL = 0x00 # port A
KICKER = 0x01 # port B
DEFAULT_POWER = 80
TURN_POWER = 0.8
BUZZER = 769
#NAME = "BrickAshley"
NAME = "BrickAsh"
def __init__(self, host=None):
self.power = self.DEFAULT_POWER
self.address = host
self.connect()
self.leftWhell = Motor(self.brick, self.LEFT_WHEEL)
self.rightWhell = Motor(self.brick, self.RIGHT_WHEEL)
self.kicker = Motor(self.brick, self.KICKER)
print "Set up Motors"
try:
self.kicker.turn(100, 100, brake=True)
except Exception as error:
print error
def connect(self):
print "Connecting ..."
try:
if self.address == None:
self.brick = nxt.find_one_brick().connect()
else:
self.brick = BlueSock(self.address).connect()
except nxt.locator.BrickNotFoundError:
raise RobotNotFoundError
except BluetoothError as error:
raise RobotConnectionError(error)
self.__get_info()
print "Conected to {name}".format(name=self.name)
def disconnect(self):
try:
self.brick = None
gc.collect()
except:
print "Unsafe disconect"
def get_name(self):
self.__get_info()
return self.name
def set_name(self, name):
self.brick.set_brick_name(name)
self.disconnect()
self.connect()
self.__get_info()
def set_power(self, value):
value = int(value)
if value < -128 or value > 128:
pass
# TODO
self.power = value
def get_power(self):
return self.power
def __get_info(self):
threading.Timer(30, self.__get_info).start()
self.name, self.host, self.signal_strength, self.user_flash = self.brick.get_device_info(
)
self.battery = self.brick.get_battery_level()
print "Info: \n\tName: {name}" \
"\n\tBT MAC: {host}\n\tBT signal: {signal}\n\t" \
"Memory: {memory}\n\tBattery: {voltage}mV".format(name=self.name, host=self.host, \
signal=self.signal_strength, memory=self.user_flash, voltage=self.battery)
def up(self):
print "go up"
self.leftWhell.run(power=self.power)
self.rightWhell.run(power=self.power)
def down(self):
print "go down"
self.brick.play_tone_and_wait(self.BUZZER, 1000)
self.leftWhell.run(power=-self.power)
self.rightWhell.run(power=-self.power)
def right(self, withBrake=False):
print "go right"
self.leftWhell.run(power=self.power * self.TURN_POWER)
if withBrake:
self.rightWhell.brake()
else:
self.rightWhell.run(power=-self.power * self.TURN_POWER)
def left(self, withBrake=False):
print "go left"
if withBrake:
self.leftWhell.brake()
else:
self.leftWhell.run(power=-self.power * self.TURN_POWER)
self.rightWhell.run(power=self.power * self.TURN_POWER)
def stop(self):
print "go stop"
self.leftWhell.brake()
self.rightWhell.brake()
self.kicker.brake()
def buzz(self):
print "buzz"
self.brick.play_tone_and_wait(self.BUZZER, 1000)
def kick(self):
print "kick"
self.kicker.turn(-127, 85, brake=True)
sleep(1.5)
self.kicker.turn(127, 90, brake=True)
class Robot:
def __init__(self):
print 'Searching for NXT bricks...'
self.robot = nxt.locator.find_one_brick()
print 'NXT brick found'
self.right_motor = Motor(self.robot, PORT_B)
self.left_motor = Motor(self.robot, PORT_C)
self.locator = Ultrasonic(self.robot, PORT_1)
self.haptic = Touch(self.robot, PORT_4)
def forward(self):
if(random.random() > .5):
self.right_motor.run(-STRAIGHT_POWER)
self.left_motor.run(-STRAIGHT_POWER)
else:
self.left_motor.run(-STRAIGHT_POWER)
self.right_motor.run(-STRAIGHT_POWER)
sleep(SECONDS)
if(random.random() > .5):
self.right_motor.idle()
self.left_motor.idle()
else:
self.left_motor.idle()
self.right_motor.idle()
def back(self):
self.right_motor.run(STRAIGHT_POWER)
self.left_motor.run(STRAIGHT_POWER)
sleep(SECONDS)
self.right_motor.idle()
self.left_motor.idle()
def right(self):
self.left_motor.turn(-TURN_POWER, ANGLE)
def left(self):
self.right_motor.turn(-TURN_POWER, ANGLE)
def distance(self):
return self.locator.get_sample()
def is_touching(self):
return self.haptic.get_sample()
def beep_ok(self):
self.robot.play_tone_and_wait(FREQ_C, DURATION)
self.robot.play_tone_and_wait(FREQ_D, DURATION)
self.robot.play_tone_and_wait(FREQ_E, DURATION)
def beep_not_ok(self):
self.robot.play_tone_and_wait(FREQ_E, DURATION)
self.robot.play_tone_and_wait(FREQ_D, DURATION)
self.robot.play_tone_and_wait(FREQ_C, DURATION)
def main(robot):
# Définition des moteurs / capteurs
m_left = Motor(robot, PORT_B)
m_right = Motor(robot, PORT_A)
touch_right = Touch(robot, PORT_2)
touch_left = Touch(robot, PORT_1)
ultrason = Ultrasonic(robot, PORT_3)
DEFAULT_POWER = 80
DISTANCE_LIMIT = 20
TURN_TIME = 1.5
# Début
while True:
m_left.run(power=DEFAULT_POWER)
m_right.run(power=DEFAULT_POWER)
while not touch_right.is_pressed() and not touch_left.is_pressed(
) and ultrason.get_distance() > DISTANCE_LIMIT:
sleep(0.01)
print("Aieee")
if touch_right.is_pressed():
m_left.run(power=DEFAULT_POWER)
m_left.run(power=-DEFAULT_POWER)
else:
m_left.run(power=-DEFAULT_POWER)
m_left.run(power=DEFAULT_POWER)
sleep(TURN_TIME)
def motor_start(letter, power):
m = Motor(b, MOTORS[letter.upper()])
m.run(power=int(power))
return 'OK'
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()
motor.turn(30, 50, brake=True, timeout=1.5, emulate=True)
sleep(0.05)
print(motor.get_tacho())
motor.brake()
white = light.get_lightness()
print("white = ", white)
motor.turn(-30, 50, brake=True, timeout=1.5, emulate=True)
threshold = (black + white) / 2
print(motor.get_tacho())
print("Threshold = ", threshold)
while light.get_lightness() < 1020:
light.get_lightness()
if light.get_lightness() > threshold:
light.get_lightness()
motor.run(power=10)
light.get_lightness()
if light.get_lightness() < threshold:
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)
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)
pygame.joystick.init()
pygame.display.init()
jcount = pygame.joystick.get_count()
if jcount == 0 : raise 'No joystick detected'
for i in range(jcount) :
js = pygame.joystick.Joystick(i)
js.init()
while True:
ev = pygame.event.wait()
if ev.type == pygame.JOYAXISMOTION and ev.joy == 0 :
if ev.axis == 0 :
yaw.run(ev.value * 10)
elif ev.axis == 2 :
pitch.run(ev.value * -5)
elif ev.type == pygame.JOYBUTTONDOWN :
pass
elif ev.type == pygame.JOYBUTTONUP :
if ev.button == 4 :
yaw.stop()
yaw.run(0)
pitch.stop()
pitch.run(0)
sock.close()
sys.exit(0)