class PhidgetEncoders:
def __init__(
self,
leftEncoder,
rightEncoder,
leftSignAdjust,
rightSignAdjust,
rollover
):
self.leftEncoderId = leftEncoder
self.rightEncoderId = rightEncoder
self.leftSignAdjust = leftSignAdjust
self.rightSignAdjust = rightSignAdjust
self.rollover = rollover
self.leftEncoder = Int16(0)
self.rightEncoder = Int16(0)
self.encoder = Encoder()
self.encoder.setOnAttachHandler(self.encoderAttached)
self.encoder.setOnDetachHandler(self.encoderDetached)
self.encoder.setOnErrorhandler(self.encoderError)
self.encoder.setOnInputChangeHandler(self.encoderInputChanged)
self.encoder.setOnPositionChangeHandler(self.encoderPositionChange)
try:
rospy.logdebug('openPhidget()')
self.encoder.openPhidget()
except PhidgetException, e:
rospy.logerror("openPhidget() failed")
rospy.logerror("code: %d" % e.code)
rospy.logerror("message", e.message)
raise
try:
rospy.logdebug('waitForAttach()')
self.encoder.waitForAttach(10000)
except PhidgetException, e:
rospy.logerror("waitForAttach() failed")
rospy.logerror("code: %d" % e.code)
rospy.logerror("message", e.message)
raise
#Create an accelerometer object
try:
encoder = Encoder()
except RuntimeError as e:
g.log.write("Runtime Exception: %s\n" % e.details)
g.log.write("Exiting....\n")
#print("Runtime Exception: %s" % e.details)
#print("Exiting....")
exit(1)
#Main Program Code
try:
#logging example, uncomment to generate a log file
#encoder.enableLogging(PhidgetLogLevel.PHIDGET_LOG_VERBOSE, "phidgetlog.log")
encoder.setOnAttachHandler(g.encoderAttached)
encoder.setOnDetachHandler(g.encoderDetached)
encoder.setOnErrorhandler(g.encoderError)
encoder.setOnInputChangeHandler(g.encoderInputChange)
encoder.setOnPositionChangeHandler(g.encoderPositionChange)
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("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))
def AttachEncoder(databasepath, serialNumber):
def onAttachHandler(event):
logString = "Encoder Attached " + str(event.device.getSerialNum())
#print(logString)
DisplayAttachedDeviceInfo(event.device)
def onDetachHandler(event):
logString = "Encoder Detached " + str(event.device.getSerialNum())
#print(logString)
DisplayDetachedDeviceInfo(event.device)
event.device.closePhidget()
def onErrorHandler(event):
logString = "Encoder Error " + str(event.device.getSerialNum()) + ", Error: " + event.description
print(logString)
DisplayErrorDeviceInfo(event.device)
def onServerConnectHandler(event):
logString = "Encoder Server Connect " + str(event.device.getSerialNum())
#print(logString)
def onServerDisconnectHandler(event):
logString = "Encoder Server Disconnect " + str(event.device.getSerialNum())
#print(logString)
def inputChangeHandler(event):
logString = "Encoder Input Changed"
#print(logString)
try:
conn = sqlite3.connect(databasepath)
conn.execute("INSERT INTO ENCODER_INPUTCHANGE VALUES(NULL, DateTime('now'), ?, ?, ?)",
(event.device.getSerialNum(), event.index, int(event.state)))
conn.commit()
conn.close()
except sqlite3.Error as e:
print "An error occurred:", e.args[0]
def positionChangeHandler(event):
logString = "Encoder Position Changed"
#print(logString)
try:
conn = sqlite3.connect(databasepath)
conn.execute("INSERT INTO ENCODER_POSITIONCHANGE VALUES(NULL, DateTime('now'), ?, ?, ?)",
(event.device.getSerialNum(), event.index, event.position))
conn.commit()
conn.close()
except sqlite3.Error as e:
print "An error occurred:", e.args[0]
try:
p = Encoder()
p.setOnAttachHandler(onAttachHandler)
p.setOnDetachHandler(onDetachHandler)
p.setOnErrorhandler(onErrorHandler)
p.setOnServerConnectHandler(onServerConnectHandler)
p.setOnServerDisconnectHandler(onServerDisconnectHandler)
p.setOnInputChangeHandler(inputChangeHandler)
p.setOnPositionChangeHandler(positionChangeHandler)
p.openPhidget(serialNumber)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting...")
exit(1)
#Create a stepper object
try:
stepper2 = Stepper()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
#Main Program Code
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)
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))
interfaceKit.setOnDetachHandler(interfaceKitDetachedEvent)
interfaceKit.setOnInputChangeHandler(interfaceKitInputChanged)
interfaceKit.openPhidget()
# Attach InterfaceKit phidget
try:
interfaceKit.waitForAttach(1000)
except PhidgetException as e:
print(
"Cannot connect to InterfaceKit (for buttons). Please ensure all devices are plugged in."
)
# Encoder object
encoder = Encoder()
encoder.setOnAttachHandler(encoderAttachedEvent)
encoder.setOnDetachHandler(encoderDetachedEvent)
encoder.setOnPositionChangeHandler(encoderPositionChanged)
encoder.openPhidget()
# Attach encoder object
try:
encoder.waitForAttach(1000)
except PhidgetException as e:
print(
"Cannot connect to Encoder. Please ensure all devices are plugged in.")
#=======================================================================================
# Main loop
try:
# Reset activity timer
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)
def AttachEncoder(databasepath, serialNumber):
def onAttachHandler(event):
logString = "Encoder Attached " + str(event.device.getSerialNum())
#print(logString)
DisplayAttachedDeviceInfo(event.device)
def onDetachHandler(event):
logString = "Encoder Detached " + str(event.device.getSerialNum())
#print(logString)
DisplayDetachedDeviceInfo(event.device)
event.device.closePhidget()
def onErrorHandler(event):
logString = "Encoder Error " + str(
event.device.getSerialNum()) + ", Error: " + event.description
print(logString)
DisplayErrorDeviceInfo(event.device)
def onServerConnectHandler(event):
logString = "Encoder Server Connect " + str(
event.device.getSerialNum())
#print(logString)
def onServerDisconnectHandler(event):
logString = "Encoder Server Disconnect " + str(
event.device.getSerialNum())
#print(logString)
def inputChangeHandler(event):
logString = "Encoder Input Changed"
#print(logString)
try:
conn = sqlite3.connect(databasepath)
conn.execute(
"INSERT INTO ENCODER_INPUTCHANGE VALUES(NULL, DateTime('now'), ?, ?, ?)",
(event.device.getSerialNum(), event.index, int(event.state)))
conn.commit()
conn.close()
except sqlite3.Error as e:
print "An error occurred:", e.args[0]
def positionChangeHandler(event):
logString = "Encoder Position Changed"
#print(logString)
try:
conn = sqlite3.connect(databasepath)
conn.execute(
"INSERT INTO ENCODER_POSITIONCHANGE VALUES(NULL, DateTime('now'), ?, ?, ?)",
(event.device.getSerialNum(), event.index, event.position))
conn.commit()
conn.close()
except sqlite3.Error as e:
print "An error occurred:", e.args[0]
try:
p = Encoder()
p.setOnAttachHandler(onAttachHandler)
p.setOnDetachHandler(onDetachHandler)
p.setOnErrorhandler(onErrorHandler)
p.setOnServerConnectHandler(onServerConnectHandler)
p.setOnServerDisconnectHandler(onServerDisconnectHandler)
p.setOnInputChangeHandler(inputChangeHandler)
p.setOnPositionChangeHandler(positionChangeHandler)
p.openPhidget(serialNumber)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting...")
exit(1)
class PhidgetEncoders:
def __init__(self):
rospy.loginfo("Initializing PhidgetEncoders")
self.leftFrontEncoder = 1
self.rightFrontEncoder = 0
self.leftRearEncoder = 3
self.rightRearEncoder = 2
self.leftSignAdjust = 1 # forward is positive
self.rightSignAdjust = -1 # forward is negative
self.roverHasntMoved = True
self.useCalculatedCovariances = False
self.driveWheelRadius = 0.054
self.wheelSeparation = 0.277
self.pulsesPerRevolution = 4331 # wheel revolution
self.wheelsConstant = 2 * pi * self.driveWheelRadius / self.wheelSeparation
self.pulsesConstant = (pi / self.pulsesPerRevolution) * self.driveWheelRadius
#
# the initialCovariance is used before the rover has moved,
# because it's position and orientation are known perfectly.
# once it has moved, the defaultCovariance is used, until
# enough data points have been received to estimate a reasonable
# covariance matrix.
#
self.initialCovariance = [0.0001, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0001, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0001, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0001, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0001, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0001]
#
# the rover is not able to move along the Z axis, so the Z value
# of zero will always be "perfect". the rover is also incapable
# of rotating about either the X or the Y axis, so those
# zeroes will also always be "perfect".
#
self.defaultCovariance = [1000.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 1000.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0001, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0001, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0001, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 1000.0]
#
# the rover begins with the origins and axes of the odom and
# base_link frames aligned.
#
self.previousX = 0
self.previousY = 0
self.previousLeftFrontPosition = 0
self.previousLeftRearPosition = 0
self.previousRightFrontPosition = 0
self.previousRightRearPosition = 0
#
# the rover begins oriented along the positive X of both the
# odom and base_link frames.
#
self.previousTheta = 0
#
# publish the Odometry message to describe the current position
# and orientation of the origin of the base_link frame.
#
self.encoder = Encoder()
self.odometryMessage = Odometry()
self.odometryMessage.header.frame_id = 'base_link'
self.odometryMessage.child_frame_id = 'base_link'
if self.useCalculatedCovariances:
self.poseCovariance, self.poseSampleList, self.numberOfPoseSamples = calcCovariance(
None,
[0.0,
0.0,
0.0,
0.0,
0.0,
0.0],
0,
100
)
self.twistCovariance, self.twistSampleList, self.numberOfTwistSamples = calcCovariance(
None,
[0.0,
0.0,
0.0,
0.0,
0.0,
0.0],
0,
100
)
#
# the rover is incapable of translating along the Z axis,
# so it will always be zero
#
self.odometryMessage.pose.pose.position.z = 0
self.odometryMessage.pose.covariance = self.initialCovariance
self.odometryMessage.twist.covariance = self.initialCovariance
#
# robot_pose_ekf subscribes (via remapping) to wheel_odom,
# to get 2D position and orientation data. the z, roll and pitch
# are ignored by robot_pose_ekf.
#
self.encoderPublisher = rospy.Publisher('wheel_odom', Odometry)
self.encoder.setOnAttachHandler(self.encoderAttached)
self.encoder.setOnDetachHandler(self.encoderDetached)
self.encoder.setOnErrorhandler(self.encoderError)
self.encoder.setOnInputChangeHandler(self.encoderInputChanged)
self.encoder.setOnPositionChangeHandler(self.encoderPositionChange)
try:
rospy.logdebug('openPhidget()')
self.encoder.openPhidget()
except PhidgetException, e:
rospy.logerror("openPhidget() failed")
rospy.logerror("code: %d" % e.code)
rospy.logerror("message", e.message)
raise
try:
rospy.logdebug('waitForAttach()')
self.encoder.waitForAttach(10000)
except PhidgetException, e:
rospy.logerror("waitForAttach() failed")
rospy.logerror("code: %d" % e.code)
rospy.logerror("message", e.message)
raise
