def syncmotorsforever(self, power):
if self._bricks:
if not((power < -127) or (power > 127)):
try:
motorB = Motor(self._bricks[self.active_nxt], PORT_B)
motorC = Motor(self._bricks[self.active_nxt], PORT_C)
syncmotors = SynchronizedMotors(motorB, motorC, 0)
syncmotors.run(power)
except:
raise logoerror(ERROR_GENERIC)
else:
raise logoerror(ERROR_POWER)
else:
raise logoerror(ERROR_BRICK)
def syncmotorsforever(self, power):
if self._bricks:
if not((power < -127) or (power > 127)):
try:
motorB = Motor(self._bricks[self.active_nxt], PORT_B)
motorC = Motor(self._bricks[self.active_nxt], PORT_C)
syncmotors = SynchronizedMotors(motorB, motorC, 0)
syncmotors.run(power)
except:
raise logoerror(ERROR_GENERIC)
else:
raise logoerror(ERROR_POWER)
else:
raise logoerror(ERROR_BRICK)
def syncmotors(self, power, turns):
if self._bricks:
if not((power < -127) or (power > 127)):
if turns < 0:
turns = abs(turns)
power = -1 * power
try:
motorB = Motor(self._bricks[self.active_nxt], PORT_B)
motorC = Motor(self._bricks[self.active_nxt], PORT_C)
syncmotors = SynchronizedMotors(motorB, motorC, 0)
syncmotors.turn(power, int(turns*360))
except:
raise logoerror(ERROR_GENERIC)
else:
raise logoerror(ERROR_POWER)
else:
raise logoerror(ERROR_BRICK)
def syncmotors(self, power, turns):
if self._bricks:
if not((power < -127) or (power > 127)):
if turns < 0:
turns = abs(turns)
power = -1 * power
try:
motorB = Motor(self._bricks[self.active_nxt], PORT_B)
motorC = Motor(self._bricks[self.active_nxt], PORT_C)
syncmotors = SynchronizedMotors(motorB, motorC, 0)
syncmotors.turn(power, int(turns*360))
except:
raise logoerror(ERROR_GENERIC)
else:
raise logoerror(ERROR_POWER)
else:
raise logoerror(ERROR_BRICK)
def __init__(self):
self.brick = nxt.locator.find_one_brick()
self.left = Motor(self.brick, PORT_C)
self.right = Motor(self.brick, PORT_A)
self.tracks = SynchronizedMotors(self.left, self.right, 0)
self.scale_factor = 0.1 # A factor for reducing the size of input measurements
self.motorZero = [0,0] # An array for resetting the motor readings.
self.currentMotorPosition = [0,0]
self.previousMotorPosition = [0,0]
self.currentMotorVelocity = [0,0]
self.previousMotorVelocity = [0,0]
self.currentMotorAcceloration = [0,0]
self.currentSonarReading = 0
self.previousSonarReading = 0
self.currentSonarVelocity = 0
self.previousSonarVelocity = 0
self.currentSonarAcceloration = 0
class Robot(object):
def __init__(self):
self.brick = nxt.locator.find_one_brick()
self.left = Motor(self.brick, PORT_C)
self.right = Motor(self.brick, PORT_A)
self.tracks = SynchronizedMotors(self.left, self.right, 0)
self.scale_factor = 0.1 # A factor for reducing the size of input measurements
self.motorZero = [0,0] # An array for resetting the motor readings.
self.currentMotorPosition = [0,0]
self.previousMotorPosition = [0,0]
self.currentMotorVelocity = [0,0]
self.previousMotorVelocity = [0,0]
self.currentMotorAcceloration = [0,0]
self.currentSonarReading = 0
self.previousSonarReading = 0
self.currentSonarVelocity = 0
self.previousSonarVelocity = 0
self.currentSonarAcceloration = 0
def resetMotors(self):
self.motorZero = self.motorPosition()
""" Static Afferents """
# This function is rediculously over complicated! It stems from the fact that the NXT library is a mess and is therefore
# not my fault!
def motorPosition(self):
get_motor = str(self.tracks.get_tacho()) # Gets motor readings in the string form: 'tacho: ' + t1 + ' ' + t2
motor_strings = string.split(get_motor) # Splits string into array form ['tacho: ', 't1', 't2']
motor_reading = [] # new array
motor_reading.append(int( float(motor_strings[1])*self.scale_factor - self.motorZero[0] )) # Turns 't1' back into an integer and appends to new array
motor_reading.append(int( float(motor_strings[2])*self.scale_factor - self.motorZero[1] )) # Turns 't2' back into an integer and appends to new array
self.previousMotorPosition = self.currentMotorPosition # updating the motor record
self.currentMotorPosition = motor_reading
return motor_reading
def sonarReading(self):
x = Ultrasonic(self.brick, PORT_1).get_sample()
self.previousSonarReading = self.currentSonarReading
self.currentSonarReading = x
return x
def killSwitch(self):
touch = Touch(self.brick, PORT_4).get_sample()
return touch
""" Dynamic Afferents """
# These next two functions do EXACTLY the same thing, but have different names. I belive that this makes the code lower
# down, where they are called, more readable, so I've decided to keep them seperate.
def motorVelocity(self, time_interval): # Claculates change in position over time.
xdot = []
xdot.append(int( (self.currentMotorPosition[0]-self.previousMotorPosition[0]) /time_interval ))
xdot.append(int( (self.currentMotorPosition[1]-self.previousMotorPosition[1]) /time_interval ))
self.previousMotorVelocity = self.currentMotorVelocity
self.currentMotorVelocity = xdot
return xdot
def motorAcceloration(self, time_interval): # Claculates the change in velocity over time.
xdotdot = []
xdotdot.append(int( (self.currentMotorVelocity[0]-self.previousMotorVelocity[0]) / time_interval ))
xdotdot.append(int( (self.currentMotorVelocity[1]-self.previousMotorVelocity[1]) / time_interval ))
self.currentMotorAcceloration = xdotdot
return xdotdot
def sonarVelocity(self, time_interval): # Claculates change in position over time.
xdot = int( (self.currentSonarReading-self.previousSonarReading) /time_interval )
self.previousSonarVelocity = self.currentSonarVelocity
self.currentSonarVelocity = xdot
return xdot
def sonarAcceloration(self, time_interval): # Claculates the change in velocity over time.
xdotdot = int( (self.currentSonarVelocity-self.previousSonarVelocity) / time_interval )
self.currentSonarAcceloration = xdotdot
return xdotdot
""" Motor Efferents """
def move(self, power=75):
self.tracks.run(power) # Takes +ve and -ve values =)
def stop(self):
self.tracks.idle()
Added sonar readings with velocity and acceloration calculations for both. @author: benjamin """ from time import sleep from nxt.motor import Motor, PORT_A, PORT_C, SynchronizedMotors from nxt.sensor import Ultrasonic, PORT_1 import nxt.locator import string brick = nxt.locator.find_one_brick() left = Motor(brick, PORT_C) right = Motor(brick, PORT_A) tracks = SynchronizedMotors(left, right, 0) tacho_zero = [0,0] # An array for resetting the tacho readings. # This program uses the physics notiation of: # X for position # V for velocity, a.k.a: X-dot # A for acceleration, a.k.a: X-double-dot tachoX0 = [0,0] # Current tacho position in degrees tachoX1 = [0,0] # Previous tacho position tachoV0 = [0,0] # Current tacho velocity in dgrees per second tachoV1 = [0,0] # Tacho velocity at previous time step tachoA0 = [0,0] # Current tacho acceleration in dgrees per second per second sonarX0 = [0,0] # Current sonar reading provides distance in cm from objects sonarX1 = [0,0] # Previous sonar reading
def turn_right(self):
"""Turns the robot right."""
both = SynchronizedMotors(self.motors[1], self.motors[0], 20)
both.turn(-70, 250)
def move_forward(self):
"""Moves the robot forward."""
both = SynchronizedMotors(self.motors[0], self.motors[1], 0)
both.turn(-70, 2 * 250)
