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: ')
