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
g = domeEncoder() #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:
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.setOnAttachHandler(interfaceKitAttachedEvent) 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:
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