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))
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))
class PhidgetNode(object):
"""Node for polling InterfaceKit, Encoder Board, and Wheatstone Bridge"""
def __init__(self):
"""Open InterfaceKit and Encoder devices and initialize ros node"""
rospy.init_node("phidgets_node")
#rospy.init_node("phidgets_node", log_level=rospy.DEBUG)
# Call base class initializer, which starts a ros node with a name and log_level
# It then opens and attaches Phidget device
self.interfaceKit = InterfaceKit()
self.encoder = Encoder()
# self.bridge = Bridge()
# initialize this to nan to indicate we haven't homed the location
self.sled_pos = float('nan') # position of sled in millimeters
self.encoder_rear_offset = 0
self.encoder_front_offset = 0
# Open the devices and wait for them to attach
self._attachPhidget(self.interfaceKit, "Interface Kit")
self._attachPhidget(self.encoder, "Encoder board")
# self._attachPhidget(self.bridge, "Wheatstone Bridge")
self.params = rospy.get_param("/phidgets")
self.sled_params = self.params["sled"]
self.sensor_pub = rospy.Publisher("/actuator_states/raw/",
ActuatorStates,
queue_size=10)
self.sensor_processed_pub = rospy.Publisher("/actuator_states/proc",
ActuatorStatesProcessed,
queue_size=10)
self.limit_sub = rospy.Subscriber("/pololu/limit_switch",
LimitSwitch,
self.limit_callback,
queue_size=10)
self.sled_is_homed = False
# enable both the sled encoders
self.encoder.setEnabled(self.sled_params["encoder_index"]["left"],
True)
self.encoder.setEnabled(self.sled_params["encoder_index"]["right"],
True)
# Display info of the devices
self._displayDeviceInfo(self.encoder)
self.displayInterfaceKitInfo()
# self.displayBridgeInfo()
def _attachPhidget(self, phidget, name):
"""Open and wait for the Phidget object to attach"""
try:
phidget.openPhidget()
rospy.loginfo("Waiting for %s to attach....", name)
phidget.waitForAttach(10000) # 10 s
except:
rospy.logerr("%s did not attach!", name)
def _displayDeviceInfo(self, phidget):
"""Display relevant info about device"""
rospy.logdebug("Attached: %s", phidget.isAttached())
rospy.logdebug("Type: %s", phidget.getDeviceName())
rospy.logdebug("Serial No.: %s", phidget.getSerialNum())
rospy.logdebug("Version: %s", phidget.getDeviceVersion())
def displayInterfaceKitInfo(self):
"""Display relevant info about interface kit"""
self._displayDeviceInfo(self.interfaceKit)
rospy.logdebug("Number of Digital Inputs: %i",
self.interfaceKit.getInputCount())
rospy.logdebug("Number of Digital Outputs: %i",
self.interfaceKit.getOutputCount())
rospy.logdebug("Number of Sensor Inputs: %i",
self.interfaceKit.getSensorCount())
rospy.logdebug("Min data rate: %i, Max data rate: %i",
self.interfaceKit.getDataRateMin(0),
self.interfaceKit.getDataRateMax(0))
def displayBridgeInfo(self):
"""Display relevant info about wheatstone bridge"""
self._displayDeviceInfo(self.bridge)
rospy.logdebug("Number of bridge inputs: %i",
self.bridge.getInputCount())
rospy.logdebug("Data Rate Max: %d", self.bridge.getDataRateMax())
rospy.logdebug("Data Rate Min: %d", self.bridge.getDataRateMin())
rospy.logdebug("Input Value Max: %d", self.bridge.getBridgeMax(0))
rospy.logdebug("Input Value Min: %d", self.bridge.getBridgeMin(0))
def limit_callback(self, limit_switch):
"""Callback for the current state of the switches. If this is the first
time they have been pressed, set flag to indicate the sled has been homed"""
if limit_switch.rear and not self.sled_is_homed:
self.sled_pos = self.sled_params["rear_pos"]
self.encoder_rear_offset = self.pollEncoders()
self.sled_is_homed = True
#elif limit_switch.front:
# self.sled_pos = self.sled_params["front_pos"]
# self.encoder_front_offset = self.pollEncoders()
# self.sled_is_homed = True
def pollEncoders(self):
"""Return averaged value of sled left and right encoder values"""
sled_left_ticks = self.encoder.getPosition(
self.sled_params["encoder_index"]["left"])
sled_right_ticks = self.encoder.getPosition(
self.sled_params["encoder_index"]["right"])
average = (-sled_left_ticks + sled_right_ticks) / 2
rospy.logdebug("left right tick average: %.2f", average)
return average
def pollLinearActuator(self, name):
"""Poll the sensor for its raw value. Then convert that value
to the actual position in millimeters. Return as a SensorValuePair"""
device = self.params[name]
index = device["sensor_index"]
min_pot_val = device["min"]
max_pot_val = device["max"]
physical_length = device["length"]
# Create objects to fill and later send over message
raw_val = self.interfaceKit.getSensorValue(index)
# Convert the potentiometer value to millimeter position
pos = (raw_val - min_pot_val) * (physical_length /
(max_pot_val - min_pot_val))
return pos
def sendProcessedMessage(self, actuator_states):
"""Process the interface sensor message to average and get the positions
of the motors"""
proc = ActuatorStatesProcessed()
proc.header = actuator_states.header
proc.arm = (actuator_states.arm_left + actuator_states.arm_right) / 2
proc.bucket = (actuator_states.bucket_left +
actuator_states.bucket_right) / 2
proc.sled = actuator_states.sled
self.sensor_processed_pub.publish(proc)
def pollSensors(self):
"""Poll and publish all the sensor values as ActuatorStates message"""
ticks_per_mm = self.sled_params["ticks_per_mm"]
actuator_states = ActuatorStates(
) # create object to store message components to send on topic
header = Header()
actuator_states.header.stamp = rospy.Time.now()
actuator_states.arm_left = self.pollLinearActuator("arm_left")
actuator_states.arm_right = self.pollLinearActuator("arm_right")
actuator_states.bucket_left = self.pollLinearActuator("bucket_left")
actuator_states.bucket_right = self.pollLinearActuator("bucket_right")
sled_left_ticks = self.encoder.getPosition(
self.sled_params["encoder_index"]["left"])
sled_right_ticks = self.encoder.getPosition(
self.sled_params["encoder_index"]["right"])
rospy.logdebug("left ticks = %d", sled_left_ticks)
rospy.logdebug("right ticks = %d", sled_right_ticks)
if self.sled_is_homed:
rospy.logdebug("left pos = %.2f", sled_left_ticks / ticks_per_mm)
rospy.logdebug("right pos = %.2f", sled_right_ticks / ticks_per_mm)
sled_pos = (self.pollEncoders() -
self.encoder_rear_offset) / ticks_per_mm
else:
sled_pos = float(
'nan'
) # set this so everybody knows we haven't homed the sled yet
actuator_states.sled = sled_pos
# TODO: add sled_pos to position when that is setup
# actuator_states.sled = self.interfaceKit.getSensorValue(self.params["sled"]["sensor_index"])
self.sensor_pub.publish(actuator_states)
self.sendProcessedMessage(actuator_states)
def run(self):
"""Run the main ros loop"""
rospy.loginfo("Starting phidgets node loop")
r_time = rospy.Rate(10) #10 Hz looping
while not rospy.is_shutdown():
self.pollSensors()
r_time.sleep()
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:
print "Aborted"
pass
endTime = time.clock()
motorControl.setVelocity(whichMotor, 0)
}
# 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
self.Initialize()
