def attach_stepper(self, id): try: ch = Stepper() ch.setOnAttachHandler(self.StepperAttached) ch.setOnDetachHandler(self.StepperDetached) ch.setOnErrorHandler(self.ErrorEvent) ch.setOnPositionChangeHandler(self.PositionChangeHandler) ch.setDeviceSerialNumber(id) print(id,": Waiting for the Phidget Stepper Object to be attached...") ch.openWaitForAttachment(5000) except PhidgetException as e: print(id, ": Phidget Exception %i: %s" % (e.code, e.details)) exit(1) print("Engaging the motor %d" % id) ch.setEngaged(1) print("Created motor %d" %id) print("Acceleration Range [%d,%d] : %d" % (ch.getMinAcceleration(), ch.getMaxAcceleration(), ch.getAcceleration())) print("Velocity Range [%d,%d] : %d" % (ch.getMinVelocityLimit(), ch.getMaxVelocityLimit(), ch.getVelocityLimit())) #print("Position Range [%d,%d] : %d" % (ch.getMinPosition(), ch.getMaxPosition(), ch.getPosition())) print("CurrentLimit Range [%d,%d] : %d" % (ch.getMinCurrentLimit(), ch.getMaxCurrentLimit(), ch.getCurrentLimit())) ch.setCurrentLimit(3) return ch
class SteeringController(): def __init__(self): # Set control mode # 0 = step mode (target position) # 1 = run mode (target velocity) self.control_mode = 0 #=========================# # Create ROS Node Objects #=========================# rospy.init_node("steering_controller") rospy.on_shutdown(self.close) # shuts down the Phidget properly if (self.control_mode == 0): rospy.Subscriber("~target_position", Float64, self.target_position_callback, queue_size=1) elif (self.control_mode == 1): rospy.Subscriber("~target_velocity", Float64, self.target_velocity_callback, queue_size=1) else: print( "Invalid control mode specified. Please set 'control_mode' to be 0 or 1" ) sys.exit(1) self.position_pub = rospy.Publisher("~current_position", Float64, queue_size=10) self.velocity_pub = rospy.Publisher("~current_velocity", Float64, queue_size=10) #================================# # Create phidget stepper channel #================================# try: self.ch = Stepper() except PhidgetException as e: sys.stderr.write("Runtime Error -> Creating Stepper") raise except RuntimeError as e: sys.stderr.write("Runtime Error -> Creating Stepper") raise self.ch.setDeviceSerialNumber(522972) self.ch.setChannel(0) # Set handlers, these are called when certain Phidget events happen print("\n--------------------------------------") print("Starting up Phidget controller") print("* Setting OnAttachHandler...") self.ch.setOnAttachHandler(self.onAttachHandler) print("* Setting OnDetachHandler...") self.ch.setOnDetachHandler(self.onDetachHandler) print("* Setting OnErrorHandler...") self.ch.setOnErrorHandler(self.onErrorHandler) print("* Setting OnPositionChangeHandler...") self.ch.setOnPositionChangeHandler(self.onPositionChangeHandler) print("* Setting OnVelocityChangeHandler...") self.ch.setOnVelocityChangeHandler(self.onVelocityChangeHandler) # Attach to Phidget print("* Opening and Waiting for Attachment...") try: self.ch.openWaitForAttachment(5000) except PhidgetException as e: PrintOpenErrorMessage(e, self.ch) raise EndProgramSignal("Program Terminated: Open Failed") # Set Rescale Factor # (pi rad / 180 deg) * (1.8deg/step * (1/16) step) / (Gear Ratio = 26 + (103/121)) self.ch.setRescaleFactor( (math.pi / 180.) * (1.8 / 16) / (26. + (103. / 121.))) # converts steps to radians # Set max velocity for position control if (self.control_mode == 0): speed = 60 # rpm speed = speed * (2 * math.pi) * (1. / 60.) # rad/s self.ch.setVelocityLimit(speed) # Set control mode (You must either uncomment the line below or do not set the control mode at all and leave let it be the default of 0, or "step" mode. Setting self.ch.setControlMode(ControlMode.CONTROL_MODE_STEP) does not work for some reason) if (self.control_mode == 1): self.ch.setControlMode(ControlMode.CONTROL_MODE_RUN) #===============# # Run main loop #===============# # self.mainLoop() rospy.spin() def onAttachHandler(self, channel): ph = channel try: # Get channel info channelClassName = ph.getChannelClassName() serialNumber = ph.getDeviceSerialNumber() channel_num = ph.getChannel() # DEBUG: print channel info print("\nAttaching Channel") print("* Channel Class: " + channelClassName + "\n* Serial Number: " + str(serialNumber) + "\n* Channel: " + str(channel_num) + "\n") # Set data time interval interval_time = 32 # ms (this will publish at roughly 30Hz) print("Setting DataInterval to %ims" % interval_time) try: ph.setDataInterval(interval_time) except PhidgetException as e: sys.stderr.write("Runtime Error -> Setting DataInterval\n") return # Engage stepper print("Engaging Stepper") try: ph.setEngaged(True) except PhidgetException as e: sys.stderr.write("Runtime Error -> Setting Engaged\n") return except PhidgetException as e: print("Error in attach event:") traceback.print_exc() return def onDetachHandler(self, channel): ph = channel try: # Get channel info channelClassName = ph.getChannelClassName() serialNumber = ph.getDeviceSerialNumber() channel_num = ph.getChannel() # DEBUG: print channel info print("\nDetaching Channel") print("\n\t-> Channel Class: " + channelClassName + "\n\t-> Serial Number: " + str(serialNumber) + "\n\t-> Channel: " + str(channel_num) + "\n") except PhidgetException as e: print("\nError in Detach Event:") traceback.print_exc() return def onErrorHandler(self, channel, errorCode, errorString): sys.stderr.write("[Phidget Error Event] -> " + errorString + " (" + str(errorCode) + ")\n") def onPositionChangeHandler(self, channel, position): self.position_pub.publish(Float64(position)) def onVelocityChangeHandler(self, channel, velocity): self.velocity_pub.publish(Float64(velocity)) def close(self): print("\n" + 30 * "-") print("Closing down Phidget controller") self.ch.setOnPositionChangeHandler(None) print("Cleaning up...") self.ch.close() print("Exiting...") return 0 def target_position_callback(self, msg): if (msg.data > self.ch.getMaxPosition()): targetPosition = self.ch.getMaxPosition() print( "Desired position greater than max, setting to max value of %.2f" % self.ch.getMaxPosition()) elif (msg.data < self.ch.getMinPosition()): targetPosition = self.ch.getMinPosition() print( "Desired position less than min, setting to min value of %.2f" % self.ch.getMinPosition()) else: targetPosition = msg.data try: self.ch.setTargetPosition(targetPosition) except PhidgetException as e: DisplayError(e) def target_velocity_callback(self, msg): if (msg.data > self.ch.getMaxVelocityLimit()): targetVelocity = self.ch.getMaxVelocityLimit() print( "Desired velocity greater than max, setting to max value of %.2f" % self.ch.getMaxVelocityLimit()) elif (msg.data < self.ch.getMinVelocityLimit()): targetVelocity = self.ch.getMinVelocityLimit() print( "Desired velocity less than min, setting to min value of %.2f" % self.ch.getMinVelocityLimit()) else: targetVelocity = msg.data try: self.ch.setVelocityLimit(targetVelocity) except PhidgetException as e: DisplayError(e)