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....") chr = sys.stdin.read(1) g.log.write("Closing...\n") #print("Closing...")
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") stepper1.setOnAttachHandler(StepperAttached) stepper1.setOnDetachHandler(StepperDetached) stepper1.setOnErrorhandler(StepperError) stepper1.setOnCurrentChangeHandler(StepperCurrentChanged)
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)
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)
print ('Device error %i, %s' % (error.code, error.details)) exit(1) """ Step 3 - Detect when a device is attached """ try: # method has units of milliseconds, so wait for 5 seconds motor.waitForAttach(5000) encoder.waitForAttach(5000) torque_sensor.waitForAttach(5000) except PhidgetException as error: print ('Device Error %i: %s' % (error.code, error.details)) try: motor.closePhidget() encoder.closePhidget() torque_sensor.closePhidget() except PhidgetException as error: print ('Phidget Error %i: %s' % (error.code, error.details)) exit(1) exit(1) """ Step 4 - Main Control Program """ # Collect user inputs user_input_position_open = input('Enter the angle to rotate OPEN in degrees: ') user_input_speed_open = input('Enter the OPEN speed in degrees/second: ') user_input_position_close = input('Enter the angle to rotate CLOSE in degrees: ') user_input_speed_close = input('Enter the CLOSE speed in degrees/second: ')