class Encoders:
default_unit = 'rad/s'
def __init__(self, countsPerRevolution, units=default_unit):
self.countsPerRevolution = float(countsPerRevolution)
print 'self.countsPerRevolution : {0}'.format(self.countsPerRevolution)
self.max_counts = 2**30 #set max counts below max integer regular int size
self.unitConversionMultiplier = None
self.__setVelocityUnits(units)
#set encoder direction defaults
#may be modified to modify interpreted direction of encoder to match intended ESC input
self.encoder_direction = {0: 1, 1: 1, 2: 1}
#Create an encoder object
try:
self.encoder = Encoder()
except RuntimeError as e:
raise RuntimeError("Runtime Exception: {0:s}".format(e.details))
#Set event handler behavior
try:
self.encoder.setOnAttachHandler(self.__encoderAttached)
self.encoder.setOnDetachHandler(self.__encoderDetached)
self.encoder.setOnErrorhandler(self.__encoderError)
except PhidgetException as e:
raise RuntimeError("Phidget Error {0}: {1}".format(
e.code, e.details))
try:
self.encoder.openPhidget()
self.encoder.waitForAttach(10000)
except PhidgetException as e:
raise RuntimeError(
"Phidget Error {0}: {1}.\nFailed 'openPhidget()'.".format(
e.code, e.details))
time.sleep(
2
) #pause here to avoid starting motors before encoders fully initialized
#set all encoder positions to 0
for i in xrange(3):
self.__resetCounts(i)
self.time_init = time.time()
self.prevCountArray = [self.time_init, 0, 0, 0]
#Event Handler Callback Functions
def __encoderAttached(self, e):
attached = e.device
print "Encoder {0} Attached!".format(attached.getSerialNum())
#self.__displayDeviceInfo()
def __encoderDetached(self, e):
detached = e.device
raise RuntimeError("Encoder {0} Detached!".format(
detached.getSerialNum()))
def __encoderError(self, e):
try:
source = e.device
raise RuntimeError("Encoder {0}: Phidget Error {1}: {2}".format(
source.getSerialNum(), e.eCode, e.description))
except PhidgetException as e:
raise RuntimeError("Phidget Exception {0}: {1}".format(
e.code, e.details))
#External Methods
def resetCounter(self, index):
self.encoder.setPostion(index)
def getVelocities(self):
#return instantaneous velocities for each encoder
print '\n'
print 'self.unitConversionMultiplier : {0}'.format(
self.unitConversionMultiplier)
count_array = self.returnCountArray()
print 'count_array : {0}'.format(count_array)
print 'prevCountArray : {0}'.format(self.prevCountArray)
diff_array = [
float(j - self.prevCountArray[i])
for i, j in enumerate(count_array)
]
print 'diff_array : {0}'.format(diff_array)
self.prevCountArray = [i for i in count_array]
velocities = [count_array[0]
] + self.__returnVelocitiesFromCounts(diff_array)
print 'velocities : {0}'.format(velocities)
#check if any encoders need to be reset to avoid exceeding max integer value
#if so, reset them
encoders_to_reset = [
i for i, x in enumerate(count_array[1:])
if abs(x) > self.max_counts
]
for i in encoders_to_reset:
print 'Reset counts on index {0} from {1} to 0 to avoid int overflow.'.format(
i, )
self.__resetCounts(i)
return velocities
def reverseDirection(self, index):
#set interpreted spin direction of an encoder channel
self.encoder_direction[index] *= -1
return None
def returnCountArray(self):
#return counts array:
#[time of measurement, encoder 0 count, encoder 1 count, encoder 2 count]
counts_array = []
counts_array = [time.time()] + [
self.encoder.getPosition(i) * self.encoder_direction[i]
for i in xrange(3)
]
return counts_array
#Internal Methods
def __returnVelocitiesFromCounts(self, diffCountsArray):
#convert counts to velocity in selected units
return [
i / diffCountsArray[0] / self.countsPerRevolution *
self.unitConversionMultiplier for i in diffCountsArray[1:]
]
def __returnEncoderTime(self, time_init):
return time.time() - self.time_init
def __displayDeviceInfo():
#Information Display Function
print(
"|------------|----------------------------------|--------------|------------|"
)
print(
"|- Attached -|- Type -|- Serial No. -|- Version -|"
)
print(
"|------------|----------------------------------|--------------|------------|"
)
print("|- {0:8} -|- {1:30s} -|- {2:10d} -|- {3:8d} -|".format(
encoder.isAttached(), encoder.getDeviceName(),
encoder.getSerialNum(), encoder.getDeviceVersion()))
print(
"|------------|----------------------------------|--------------|------------|"
)
def __resetCounts(self, index):
#reset encoder channel position to zero
self.encoder.setPosition(index, 0)
def __setVelocityUnits(self, units):
#toggle output between rad/s, Hz, rpm
#multipliers for converting from Hz
#self.unitConversion is used in __returnVelocitiesFromCounts to get to desired units
unitsConversion = {'rad/s': (2.0 * pi), 'Hz': 1, 'rpm': 60.0}
if units in unitsConversion:
self.unitConversionMultiplier = unitsConversion[units]
print 'Units set to {0}.'.format(units)
else:
self.unitConversionMultiplier = unitsConversion[default_unit]
print('Requested units ({0}) not available. Using {1} instead.'.
format(units, default_unit))
try:
motorControl.waitForAttach(10000)
except PhidgetException, e:
print "waitForAttach() failed"
print "code: %d" % e.code
print "message", e.message
raise
if motorControl.isAttached():
print "motor control attached"
else:
print "motor attach Failed"
encoder.setPosition(whichEncoder, 0)
encoder.setEnabled(whichEncoder, True)
averageAcceleration = (motorControl.getAccelerationMax(whichMotor) - motorControl.getAccelerationMin(whichMotor)) * 0.9
print "averageAcceleration", averageAcceleration
motorControl.setAcceleration(whichMotor, averageAcceleration)
position = encoder.getPosition(whichEncoder)
print "Starting position", position
startTime = time.clock()
motorControl.setVelocity(whichMotor, velocity)
try:
while position < pulsesWanted:
position = encoder.getPosition(whichEncoder) * encoderSign
print "Position", position, "Current", motorControl.getCurrent(whichMotor), "\r",
except:
class PhidgetsController(kp.Module):
def configure(self):
from Phidgets.Devices.Stepper import Stepper
from Phidgets.Devices.Encoder import Encoder
self.current_limit = self.get("current_limit") or 2.5
self.motor_id = self.get("motor_id") or 0
self.stepper = Stepper()
self.encoder = Encoder()
self.setup(self.motor_id)
def setup(self, motor_id):
self.stepper.openPhidget()
self.stepper.waitForAttach(10000)
self.encoder.openPhidget()
self.encoder.waitForAttach(10000)
self.stepper.setVelocityLimit(motor_id, 1000)
self.stepper.setAcceleration(motor_id, 5000)
self.stepper.setCurrentLimit(motor_id, 2.5)
self.e = 13250.0
self.s = 70500.0
self._stepper_dest = 0
self._encoder_dest = 0
self.reset_positions()
def drive_to_angle(self, ang, motor_id=0, relative=False):
stepper_dest = self._stepper_dest = self.raw_stepper_position(ang)
self._encoder_dest = self.raw_encoder_position(ang)
if relative:
self.reset_positions()
self.wake_up()
time.sleep(0.1)
self.stepper_target_pos = stepper_dest
self.wait_for_stepper()
self.log_offset()
while abs(self.offset) > 1:
self.log_offset()
stepper_offset = round(self.offset / self.e * self.s)
log.debug("Correcting stepper by {0}".format(stepper_offset))
log.debug("Stepper target pos: {0}".format(
self.stepper_target_pos))
log.debug("Stepper pos: {0}".format(self.stepper_pos))
self.stepper_target_pos = self.stepper_pos + stepper_offset
self.wait_for_stepper()
self.log_positions()
self.stand_by()
def wait_for_stepper(self):
while self.stepper_pos != self._stepper_dest:
time.sleep(0.1)
self.log_positions()
def log_offset(self):
log.debug("Difference (encoder): {0}".format(self.offset))
@property
def offset(self):
return self._encoder_dest - self.encoder_pos
@property
def stepper_target_pos(self):
return self.stepper.getTargetPosition(self.motor_id)
@stepper_target_pos.setter
def stepper_target_pos(self, val):
self._stepper_dest = int(val)
self.stepper.setTargetPosition(self.motor_id, int(val))
@property
def stepper_pos(self):
return self.stepper.getCurrentPosition(self.motor_id)
@stepper_pos.setter
def stepper_pos(self, val):
self.stepper.setCurrentPosition(self.motor_id, int(val))
@property
def encoder_pos(self):
return self.encoder.getPosition(self.motor_id)
def raw_stepper_position(self, angle):
return round(angle * self.s / 360)
def raw_encoder_position(self, angle):
return round(angle * self.e / 360)
def wake_up(self, motor_id=0):
self.stepper.setEngaged(motor_id, 1)
def stand_by(self, motor_id=0):
self.stepper.setEngaged(motor_id, 0)
def reset_positions(self, motor_id=0):
self.stepper.setCurrentPosition(motor_id, 0)
self.encoder.setPosition(motor_id, 0)
def log_positions(self, motor_id=0):
log.info("Stepper position: {0}\nEncoder position:{1}".format(
self.stepper_pos / self.s * 360, self.encoder_pos / self.e * 360))
'Open': [int(user_input_speed_open), int(user_input_position_open)],
'Close': [int(user_input_speed_close), int(user_input_position_close)]
}
# Convert speed: deg/sec -> RPM -> % duty cycle. Motor rotation: 33 RPM at 100% voltage (24 VDC)
# Convert position to counts for the encoder. Encoder has 300 counts/revolution = 0.833 counts/degree with a 76 reduction ratio
PWM_duty_cycle = []
commanded_position = []
# each list ordered [open, close]
for key, value in inputs.items():
PWM_duty_cycle.append((value[0] * 0.16666) / 33)
commanded_position.append(value[1] * (0.83333/76)
# Set encoder & calculate position difference [open, close]
encoder.setPosition(0, 0)
position_difference = []
position_difference[:] = [i - encoder.getPosition(0) for i in commanded_position]
# Create simple PID controller
# Reference: http://examples.oreilly.com/9780596809577/CH09/PID.py
class PID:
"""
Simple PID control from "Real-World Instrumentation with Python"
by J. M. Hughes, published by O'Reilly Media, December 2010"
"""
def __init__(self):
# initialze gains
self.Kp = 0
self.Kd = 0
self.Ki = 0
