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
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)) #print("Phidget Error %i: %s" % (e.code, e.details)) exit(1)
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)
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) print("Waiting for attach....")
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