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
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)
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....") try:
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
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 lastActiveTime = time.time()
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)
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