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)
def sort(pos=5000): try: """ * Allocate a new Phidget Channel object """ ch = Stepper() """ * Set matching parameters to specify which channel to open """ #You may remove this line and hard-code the addressing parameters to fit your application channelInfo = AskForDeviceParameters(ch) ch.setDeviceSerialNumber(channelInfo.deviceSerialNumber) ch.setHubPort(channelInfo.hubPort) ch.setIsHubPortDevice(channelInfo.isHubPortDevice) ch.setChannel(channelInfo.channel) if (channelInfo.netInfo.isRemote): ch.setIsRemote(channelInfo.netInfo.isRemote) if (channelInfo.netInfo.serverDiscovery): try: Net.enableServerDiscovery( PhidgetServerType.PHIDGETSERVER_DEVICEREMOTE) except PhidgetException as e: PrintEnableServerDiscoveryErrorMessage(e) raise EndProgramSignal( "Program Terminated: EnableServerDiscovery Failed") else: Net.addServer("Server", channelInfo.netInfo.hostname, channelInfo.netInfo.port, channelInfo.netInfo.password, 0) """ * Add event handlers before calling open so that no events are missed. """ print("\n--------------------------------------") #print("\nSetting OnAttachHandler...") ch.setOnAttachHandler(onAttachHandler) #print("Setting OnDetachHandler...") ch.setOnDetachHandler(onDetachHandler) #print("Setting OnErrorHandler...") ch.setOnErrorHandler(onErrorHandler) #print("\nSetting OnPositionChangeHandler...") ch.setOnPositionChangeHandler(onPositionChangeHandler) """ * Open the channel with a timeout """ #print("\nOpening and Waiting for Attachment...") try: ch.openWaitForAttachment(5000) except PhidgetException as e: PrintOpenErrorMessage(e, ch) raise EndProgramSignal("Program Terminated: Open Failed") """ * Sorting process, set position for the stepper to move """ end = False buf = pos ch.setVelocityLimit(50000) ch.setAcceleration(200000) while (end != True): if (buf == 0): end = True break targetPosition = buf if (targetPosition > ch.getMaxPosition() or targetPosition < ch.getMinPosition()): print("TargetPosition must be between %.2f and %.2f\n" % (ch.getMinPosition(), ch.getMaxPosition())) continue #print("Setting Stepper TargetPosition to " + str(targetPosition)) ch.setTargetPosition(targetPosition) #print("Position is " + str(ch.getPosition())) if (targetPosition == ch.getPosition()): buf = 0 ''' * Perform clean up and exit ''' print("Cleaning up...") ch.close() return 0 except PhidgetException as e: sys.stderr.write("\nExiting with error(s)...") DisplayError(e) #traceback.print_exc() print("Cleaning up...") ch.close() return 1 except EndProgramSignal as e: print(e) print("Cleaning up...") ch.close() return 1 except RuntimeError as e: sys.stderr.write("Runtime Error: \n\t" + e) traceback.print_exc() return 1 finally: print(print("\nExiting...")) #sort(-20000) #sort(150000)
class SteeringController(): def __init__(self): # 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 print("\n--------------------------------------") print("\nSetting OnAttachHandler...") self.ch.setOnAttachHandler(self.onAttachHandler) print("Setting OnDetachHandler...") self.ch.setOnDetachHandler(self.onDetachHandler) print("Setting OnErrorHandler...") self.ch.setOnErrorHandler(self.onErrorHandler) print("\nSetting OnPositionChangeHandler...") self.ch.setOnPositionChangeHandler(self.onPositionChangeHandler) print("\nOpening 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 # 1.8deg/step * (1/16) step / (Gear Ratio = 26 + (103/121)) self.ch.setRescaleFactor((1.8 / 16) / (26. + (103. / 121.))) # Set control mode # self.ch.setControlMode(ControlMode.CONTROL_MODE_STEP) #self.ch.setControlMode(ControlMode.CONTROL_MODE_RUN) #===== Run main loop self.mainLoop() 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("\n\t-> Channel Class: " + channelClassName + "\n\t-> Serial Number: " + str(serialNumber) + "\n\t-> Channel: " + str(channel_num) + "\n") # Set data time interval interval_time = 500 # ms 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): print("[Position Event] -> Position: " + str(Position)) def mainLoop(self): end = False while (end != True): buf = sys.stdin.readline(100) if not buf: continue if (buf[0] == 'Q' or buf[0] == 'q'): end = True continue try: targetPosition = float(buf) except ValueError as e: print("Input must be a number, or Q to quit.") continue if (targetPosition > self.ch.getMaxPosition() or targetPosition < self.ch.getMinPosition()): print("TargetPosition must be between %.2f and %.2f\n" % (self.ch.getMinPosition(), self.ch.getMaxPosition())) continue print("Setting Stepper TargetPosition to " + str(targetPosition)) try: self.ch.setTargetPosition(targetPosition) except PhidgetException as e: DisplayError(e) print(self.ch.getTargetPosition()) print(self.ch.getVelocity()) self.close() def close(self): self.ch.setOnPositionChangeHandler(None) print("Cleaning up...") self.ch.close() print("\nExiting...") return 0
class SteeringController(): def __init__(self): # Set control mode # 0 = step mode (target position) # 1 = run mode (target velocity) self.control_mode = 0 # needs to be position for this code self.angle = 0 # starting angle before joystick command received #=========================# # Create ROS Node Objects #=========================# rospy.init_node("steering_position") rospy.on_shutdown(self.close) # shuts down the Phidget properly self.joy_sub = rospy.Subscriber("/joy", Joy, self.joy_callback, queue_size=1) self.position_pub = rospy.Publisher("~motor_position", Float64, queue_size=10) self.velocity_pub = rospy.Publisher("~motor_velocity", Float64, queue_size=10) # Specify general parameters self.rl_axes = 3 self.deadman_button = rospy.get_param("~deadman", 4) self.scale_angle = rospy.get_param("~scale_angle", 2 * math.pi) print("Deadman button: " + str(self.deadman_button)) print("Scale angle: " + str(self.scale_angle)) #================================# # 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 Parameters #====================# # Set Rescale Factor # (pi rad / 180 deg) * (1.8deg/step * (1/16) step) / (Gear Ratio = 26 + (103/121)) self.rescale_factor = (math.pi / 180.) * (1.8 / 16) / (26. + (103. / 121.)) self.ch.setRescaleFactor( self.rescale_factor) # converts steps to radians # Set max velocity for position control if (self.control_mode == 0): max_speed = 250000 # (1/16) steps / sec max_speed *= self.rescale_factor self.ch.setVelocityLimit(max_speed) #===============# # 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 joy_callback(self, msg): # Check if deadman switch is pressed if (msg.buttons[self.deadman_button]): # Read right joystick analog "Left/Right" value self.angle = self.scale_angle * msg.axes[self.rl_axes] max_position = self.ch.getMaxPosition() min_position = self.ch.getMinPosition() if (self.angle > max_position): self.angle = max_position print( "Desired position greater than max, setting to max value of %.2f" % max_position) elif (self.angle < min_position): self.angle = min_position print( "Desired position less than min, setting to min value of %.2f" % min_position) try: self.ch.setTargetPosition(self.angle) except PhidgetException as e: DisplayError(e)
def main(): try: """ * Allocate a new Phidget Channel object """ try: ch = Stepper() except PhidgetException as e: sys.stderr.write("Runtime Error -> Creating Stepper: \n\t") DisplayError(e) raise except RuntimeError as e: sys.stderr.write("Runtime Error -> Creating Stepper: \n\t" + e) raise """ * Set matching parameters to specify which channel to open """ ch.setDeviceSerialNumber(522972) ch.setChannel(0) """ * Add event handlers before calling open so that no events are missed. """ print("\n--------------------------------------") print("\nSetting OnAttachHandler...") ch.setOnAttachHandler(onAttachHandler) print("Setting OnDetachHandler...") ch.setOnDetachHandler(onDetachHandler) print("Setting OnErrorHandler...") ch.setOnErrorHandler(onErrorHandler) print("\nSetting OnPositionChangeHandler...") ch.setOnPositionChangeHandler(onPositionChangeHandler) """ * Open the channel with a timeout """ print("\nOpening and Waiting for Attachment...") try: ch.openWaitForAttachment(5000) except PhidgetException as e: PrintOpenErrorMessage(e, ch) raise EndProgramSignal("Program Terminated: Open Failed") print( "--------------------\n" "\n | Stepper motor position can be controlled by setting its Target Position.\n" " | The target position can be a number between MinPosition and MaxPosition.\n" " | For this example, acceleration and velocity limit are left as their default values, but can be changed in custom code.\n" "\nInput a desired position and press ENTER\n" "Input Q and press ENTER to quit\n") end = False # Rescale Factor # 1.8deg/step * (1/16) step / (Gear Ratio = 26 + (103/121)) # ch.setRescaleFactor((1.8/16)/(26.+(103./121.))) ch.setControlMode(ControlMode.CONTROL_MODE_STEP) # ch.setControlMode(ControlMode.CONTROL_MODE_RUN) while (end != True): buf = sys.stdin.readline(100) if not buf: continue if (buf[0] == 'Q' or buf[0] == 'q'): end = True continue # Position Control try: targetPosition = float(buf) except ValueError as e: print("Input must be a number, or Q to quit.") continue if (targetPosition > ch.getMaxPosition() or targetPosition < ch.getMinPosition()): print("TargetPosition must be between %.2f and %.2f\n" % (ch.getMinPosition(), ch.getMaxPosition())) continue print("Setting Stepper TargetPosition to " + str(targetPosition)) ch.setTargetPosition(targetPosition) # # Velocity Control # try: # targetVelocity = float(buf) # except ValueError as e: # print("Input must be a number, or Q to quit.") # continue # # if (targetVelocity > ch.getMaxVelocityLimit() or targetVelocity < ch.getMinVelocityLimit()): # print("TargetPosition must be between %.2f and %.2f\n" % (ch.getMinVelocityLimit(), ch.getMaxVelocityLimit())) # continue # # print("Setting Stepper VelocityLimit to " + str(targetVelocity)) # ch.setVelocityLimit(targetVelocity) ''' * Perform clean up and exit ''' #clear the PositionChange event handler ch.setOnPositionChangeHandler(None) print("Cleaning up...") ch.close() print("\nExiting...") return 0 except PhidgetException as e: sys.stderr.write("\nExiting with error(s)...") DisplayError(e) traceback.print_exc() print("Cleaning up...") #clear the PositionChange event handler ch.setOnPositionChangeHandler(None) ch.close() return 1 except EndProgramSignal as e: print(e) print("Cleaning up...") #clear the PositionChange event handler ch.setOnPositionChangeHandler(None) ch.close() return 1 finally: print("Press ENTER to end program.") readin = sys.stdin.readline()