g.log.write("Opening phidget object....\n")
#print("Opening phidget object....")
try:
encoder.openPhidget()
except PhidgetException as e:
g.log.write("Phidget Error %i: %s\n" % (e.code, e.details))
#print("Phidget Error %i: %s" % (e.code, e.details))
exit(1)
g.log.write("Waiting for attach....\n")
#print("Waiting for attach....")
try:
encoder.waitForAttach(10000)
except PhidgetException as e:
g.log.write("Phidget Error %i: %s\n" % (e.code, e.details))
#print("Phidget Error %i: %s" % (e.code, e.details))
try:
encoder.closePhidget()
except PhidgetException as e:
g.log.write("Phidget Error %i: %s\n" % (e.code, e.details))
#print("Phidget Error %i: %s" % (e.code, e.details))
exit(1)
exit(1)
else:
g.displayDeviceInfo()
g.log.write("Press Enter to quit....\n")
#print("Press Enter to quit....")
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))
except PhidgetException as e:
print("Phidget Error %i: %s" % (e.code, e.details))
exit(1)
print("Opening phidget object....")
try:
encoder.openPhidget()
except PhidgetException as e:
print("Phidget Error %i: %s" % (e.code, e.details))
exit(1)
print("Waiting for attach....")
try:
encoder.waitForAttach(10000)
except PhidgetException as e:
print("Phidget Error %i: %s" % (e.code, e.details))
try:
encoder.closePhidget()
except PhidgetException as e:
print("Phidget Error %i: %s" % (e.code, e.details))
exit(1)
exit(1)
else:
displayEncoderInfo()
try:
#logging example, uncomment to generate a log file
#stepper.enableLogging(PhidgetLogLevel.PHIDGET_LOG_VERBOSE, "phidgetlog.log")
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))
def initMotorAndEncoderBoards():
global motorControl, motorControlRight, rightWheels, phidget1065, encoders, leftEncoderPosition, rightEncoderPosition, motors_inverted, encoders_inverted
try:
motorControl = MotorControl()
except:
rospy.logerr("Unable to connect to Phidget HC Motor Control")
return
try:
motorControl.setOnAttachHandler(mcAttached)
motorControl.setOnErrorhandler(mcError)
motorControl.setOnVelocityChangeHandler(mcVelocityChanged)
motorControl.openPhidget()
#attach the board
motorControl.waitForAttach(10000)
"""use the function getMotorCount() to know how many motors the Phidget board can take
if the result is more than 1, we have a 1064 board and we take care of both motors with one motorControl variable. We need to handle the Phidget encoder board that is
separated from the phidget 1064.
if the result is equal to 1, we have two phidget 1065 boards. The one with serial number that is the lowest will be the left will, the other the right weel. The encoder has to be handled
in this file as it is part of the 1065 board.
"""
if motorControl.getMotorCount() == 1:
phidget1065 = True
rightWheels = 0
motorControlRight = MotorControl()
motorControlRight.setOnAttachHandler(mcAttached)
motorControlRight.setOnErrorhandler(mcError)
motorControlRight.setOnVelocityChangeHandler(mcVelocityChanged)
if motorControl.getSerialNum() > motorControlRight.getSerialNum():
""" As a rule, we need the serial number of the left board to be lower than for the right board. This is for consistancy for all the robots
"""
motorControlTemp = motorControl
motorControl = motorControlRight
motorControlRight = motorControlTemp
#Set up the encoders handler
motorControl.setOnPositionUpdateHandler(leftEncoderUpdated)
motorControlRight.setOnPositionUpdateHandler(rightEncoderUpdated)
#attach the board
motorControlRight.openPhidget()
motorControlRight.waitForAttach(10000)
# we have a motor controller board that control 2 motors but doesn't get any encoder input, so we need to initialize the encoder board.
else:
encoders = Encoder()
encoders.setOnPositionChangeHandler(encoderBoardPositionChange)
encoders.openPhidget()
encoders.waitForAttach(10000)
# some robots have the left and right encoders switched by mistake
if(motors_inverted or encoders_inverted):
leftEncoderPosition = 1;
rightEncoderPosition = 0;
encoders.setEnabled(leftEncoderPosition, True)
encoders.setEnabled(rightEncoderPosition, True)
except PhidgetException as e:
motorsError = 1
encodersError = 1
rospy.logerr("Unable to initialize the motors and encoders board: %i: %s", e.code, e.details)
return
except:
motorsError = 1
encodersError = 1
rospy.logerr("Unable to register the motors and encoders board")
return
if motorControl.isAttached():
rospy.loginfo("Device: %s, Serial: %d, Version: %d",motorControl.getDeviceName(),motorControl.getSerialNum(),motorControl.getDeviceVersion())
if phidget1065 == True:
if motorControlRight.isAttached():
rospy.loginfo("Device: %s, Serial: %d, Version: %d",motorControlRight.getDeviceName(),motorControlRight.getSerialNum(),motorControlRight.getDeviceVersion())
else:
rospy.loginfo("Device: %s, Serial: %d, Version: %d",encoders.getDeviceName(),encoders.getSerialNum(),encoders.getDeviceVersion())
if stop_when_obstacle:
timer = Timer(1.0, checkEncoders)
timer.start()
return
def initMotorAndEncoderBoards():
global motorControl, motorControlRight, rightWheels, phidget1065, encoders, leftEncoderPosition, rightEncoderPosition, motors_inverted, encoders_inverted
try:
motorControl = MotorControl()
except:
rospy.logerr("Unable to connect to Phidget HC Motor Control")
return
try:
motorControl.setOnAttachHandler(mcAttached)
motorControl.setOnErrorhandler(mcError)
motorControl.setOnVelocityChangeHandler(mcVelocityChanged)
motorControl.openPhidget()
#attach the board
motorControl.waitForAttach(10000)
"""use the function getMotorCount() to know how many motors the Phidget board can take
if the result is more than 1, we have a 1064 board and we take care of both motors with one motorControl variable. We need to handle the Phidget encoder board that is
separated from the phidget 1064.
if the result is equal to 1, we have two phidget 1065 boards. The one with serial number that is the lowest will be the left will, the other the right weel. The encoder has to be handled
in this file as it is part of the 1065 board.
"""
if motorControl.getMotorCount() == 1:
phidget1065 = True
rightWheels = 0
motorControlRight = MotorControl()
motorControlRight.setOnAttachHandler(mcAttached)
motorControlRight.setOnErrorhandler(mcError)
motorControlRight.setOnVelocityChangeHandler(mcVelocityChanged)
if motorControl.getSerialNum() > motorControlRight.getSerialNum():
""" As a rule, we need the serial number of the left board to be lower than for the right board. This is for consistancy for all the robots
"""
motorControlTemp = motorControl
motorControl = motorControlRight
motorControlRight = motorControlTemp
#Set up the encoders handler
motorControl.setOnPositionUpdateHandler(leftEncoderUpdated)
motorControlRight.setOnPositionUpdateHandler(rightEncoderUpdated)
#attach the board
motorControlRight.openPhidget()
motorControlRight.waitForAttach(10000)
# we have a motor controller board that control 2 motors but doesn't get any encoder input, so we need to initialize the encoder board.
else:
encoders = Encoder()
encoders.setOnPositionChangeHandler(encoderBoardPositionChange)
encoders.openPhidget()
encoders.waitForAttach(10000)
# some robots have the left and right encoders switched by mistake
if(motors_inverted or encoders_inverted):
leftEncoderPosition = 1;
rightEncoderPosition = 0;
encoders.setEnabled(leftEncoderPosition, True)
encoders.setEnabled(rightEncoderPosition, True)
except PhidgetException as e:
motorsError = 1
encodersError = 1
rospy.logerr("Unable to initialize the motors and encoders board: %i: %s", e.code, e.details)
return
except:
motorsError = 1
encodersError = 1
rospy.logerr("Unable to register the motors and encoders board")
return
if motorControl.isAttached():
rospy.loginfo("Device: %s, Serial: %d, Version: %d",motorControl.getDeviceName(),motorControl.getSerialNum(),motorControl.getDeviceVersion())
if phidget1065 == True:
if motorControlRight.isAttached():
rospy.loginfo("Device: %s, Serial: %d, Version: %d",motorControlRight.getDeviceName(),motorControlRight.getSerialNum(),motorControlRight.getDeviceVersion())
else:
rospy.loginfo("Device: %s, Serial: %d, Version: %d",encoders.getDeviceName(),encoders.getSerialNum(),encoders.getDeviceVersion())
if stop_when_obstacle:
timer = Timer(1.0, checkEncoders)
timer.start()
return
class PhidgetEncoder:
'''Monitor the pose of each encoders'''
def __init__(self):
self.period = rospy.get_param('~period', 0.02)
self.minPubPeriod = rospy.get_param('~min_pub_period', None)
if self.minPubPeriod < self.period * 2:
rospy.loginfo(
"Warning: you attempted to set the min_pub_period" +
" to a smaller value (%.3f)" % (self.minPubPeriod) +
" than twice the period (2 * %.3f)." % (self.period) +
" This is not allowed, hence I am setting the min_pub_period" +
" to %.3f." % (self.period * 2))
self.minPubPeriod = self.period * 2
# encoders are identified by their index (i.e. on which port they are
# plugged on the phidget)
self.left = 0
self.right = 1
self.countBufs = {self.left: CountBuffer(), self.right: CountBuffer()}
self.lastPub = None
self._mutex = threading.RLock()
self.initPhidget()
self.encoder.setEnabled(self.left, True)
self.encoder.setEnabled(self.right, True)
self.pub = rospy.Publisher('encoder_counts', EncoderCounts)
# diagnostics
self.diag_updater = DIAG.Updater()
self.diag_updater.setHardwareID('none')
f = {'min': 1.0 / self.minPubPeriod}
fs_params = DIAG.FrequencyStatusParam(f, 0.25, 1)
self.pub_diag = DIAG.HeaderlessTopicDiagnostic('encoder_counts',
self.diag_updater,
fs_params)
#TODO: add a diagnostic task to monitor the connection with the phidget
self.forcePub(None)
def initPhidget(self):
'''Connects to the phidget and init communication.'''
#Create an encoder object (from the Phidget library)
try:
self.encoder = Encoder()
except RuntimeError as e:
rospy.logerr("Runtime exception: %s. Exiting..." % e.details)
exit(1)
#Set the callback
try:
self.encoder.setOnPositionChangeHandler(self.encoderPositionChange)
except PhidgetException as e:
err(e)
rospy.loginfo("Opening phidget object....")
try:
self.encoder.openPhidget()
except PhidgetException as e:
err(e)
rospy.loginfo("Waiting for attach....")
try:
self.encoder.waitForAttach(10000)
except PhidgetException as e:
rospy.logerr("Phidget error %i: %s" % (e.code, e.details))
try:
self.encoder.closePhidget()
except PhidgetException as e:
err(e)
exit(1)
def closePhidget(self):
rospy.loginfo("Closing...")
try:
self.encoder.closePhidget()
except PhidgetException as e:
err(e)
def encoderPositionChange(self, e):
'''A callback function called whenever the position changed'''
#rospy.loginfo("Encoder %i: Encoder %i -- Change: %i -- Time: %i -- Position: %i"
#% ( e.device.getSerialNum(), e.index, e.positionChange, e.time,
#self.encoder.getPosition(e.index)) )
with self._mutex:
if e.index in self.countBufs.keys():
self.countBufs[e.index].add(e)
dts = [b.dt for b in self.countBufs.values()]
if min(dts) >= self.period:
#if self.countBufs[self.left].n != self.countBufs[self.right].n:
#rospy.loginfo("encoders: time criteria met, but not count criteria")
dt = sum(dts) / len(dts)
#rospy.loginfo("Publishing")
self.publish(dt)
def publish(self, dt):
encodersMsg = EncoderCounts()
self.lastPub = rospy.Time.now()
encodersMsg.stamp = self.lastPub # i.e. now()
encodersMsg.dt = dt
with self._mutex:
encodersMsg.d_left = self.countBufs[self.left].get_delta_rev()
encodersMsg.d_right = -self.countBufs[self.right].get_delta_rev()
self.countBufs[self.left].reset()
self.countBufs[self.right].reset()
self.pub.publish(encodersMsg)
self.pub_diag.tick()
self.diag_updater.update()
def forcePub(self, dummy):
''' We want to publish messages even if the vehicle is not moving. '''
if self.minPubPeriod:
if self.lastPub is None:
self.lastPub = rospy.Time.now()
else:
dt = (rospy.Time.now() - self.lastPub).to_sec()
if dt > self.minPubPeriod:
self.publish(dt)
self.timer = rospy.Timer(rospy.Duration(self.minPubPeriod),
self.forcePub, True)
def home(e):
#if the home switch is activated,
#e.setPosition(0,0)
if __name__ == '__main__':
#Main Program Code
#Create an accelerometer object
try:
encoder = Encoder()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
try:
#logging example, uncomment to generate a log file
#encoder.enableLogging(PhidgetLogLevel.PHIDGET_LOG_VERBOSE, "phidgetlog.log")
encoder.setOnAttachHandler(encoderAttached)
encoder.setOnDetachHandler(encoderDetached)
encoder.setOnErrorhandler(encoderError)
encoder.setOnInputChangeHandler(encoderInputChange)
encoder.setOnPositionChangeHandler(encoderPositionChange)
except PhidgetException as e:
print("Phidget Error %i: %s" % (e.code, e.details))
exit(1)
print("Opening phidget object....")
try:
encoder.openPhidget()
except PhidgetException as e:
print("Phidget Error %i: %s" % (e.code, e.details))
exit(1)
print("Waiting for attach....")
try:
encoder.waitForAttach(10000)
except PhidgetException as e:
print("Phidget Error %i: %s" % (e.code, e.details))
try:
encoder.closePhidget()
except PhidgetException as e:
print("Phidget Error %i: %s" % (e.code, e.details))
exit(1)
exit(1)
else:
displayDeviceInfo()
print("Press Enter to quit....")
chr = sys.stdin.read(1)
print("Closing...")
try:
encoder.closePhidget()
except PhidgetException as e:
print("Phidget Error %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Done.")
exit(0)
