def main(initial_vel, remote_details=None): if remote_details: host, port = remote_details print("NETWORK", remote_details) Net.addServer('localhost', host, port, '', 0) stepper0 = Stepper() stepper0.setHubPort(0) stepper0.openWaitForAttachment(5000) stepper0.setControlMode(StepperControlMode.CONTROL_MODE_RUN) stepper0.setCurrentLimit(0.8) print("Attached: ", stepper0.getAttached()) if initial_vel is not None: vels = (initial_vel, ) else: vels = (-460, -230, 0, 230, 460) stepper0.setEngaged(True) try: while True: v = random.choice(vels) stepper0.setVelocityLimit(v) print("==", v) time.sleep(5) except KeyboardInterrupt: pass print("==", 0) stepper0.setVelocityLimit(0) stepper0.close()
except PhidgetException as e: print("Phidget Exception %i: %s" % (e.code, e.details)) print("Press Enter to Exit...\n") readin = sys.stdin.read(1) exit(1) print("Engaging the motor\n") ch.setEngaged(1) run() print("Setting Position to -600000 for 5 seconds...\n") ch.setTargetPosition(-600000) time.sleep(60) print("Setting Position to 0 for 5 seconds...\n") ch.setTargetPosition(0) outfile = open('input.txt', 'w') outfile.write(list) outfile.close() time.sleep(60) try: ch.close() except PhidgetException as e: print("Phidget Exception %i: %s" % (e.code, e.details)) print("Press Enter to Exit...\n") readin = sys.stdin.read(1) exit(1) print("Closed Stepper device") exit(0)
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): # Set control mode # 0 = step mode (target position) # 1 = run mode (target velocity) self.control_mode = 1 self.velocity = 0 # rad/s, starting velocity self.angle = 0 # current steering angle self.angle_setpoint = 0 # current steering angle setpoint # TODO: makes these ROS parameters self.angle_tolerance = 0.02 # stops moving the motor when the angle is within +/- the tolerance of the desired setpoint # Max and Min angles to turn of the velocity if they are reached self.max_angle = rospy.get_param("/forklift/steering/max_angle", 75 * (math.pi / 180.)) self.min_angle = rospy.get_param("/forklift/steering/min_angle", -75 * (math.pi / 180.)) self.velocity_tolerance = 0.01 # DEBUG: print max angle values used print( "[steering_node] Bounding setpoint angles to, Max: {0:0.3f} ({1:0.3f} deg) Min: {2:0.3f} ({3:0.3f} deg)" .format(self.max_angle, self.max_angle * (180 / math.pi), self.min_angle, self.min_angle * (180 / math.pi))) self.max_accel_scale = 0.003 self.max_vel_scale = 0.17 self.velocity_current = 0 # current velocity from the motor controller self.gear = 0 # current gear of the forklift, should only steer if not in neutral (gear = 0) #===============================================================# # These parameters are used in the stall detection and handling #===============================================================# # Tuning parameters self.max_repeats = 5 # the maximum number of times the motor can be seen as not moving before reseting self.ramp_time_vel = 1.5 # number of seconds to ramp up to full velocity again self.ramp_time_accel = 1.5 # number of seconds to ramp up to full acceleration again # Operation states self.moving = False # indicates whether the motor is currently moving self.repeats = 0 # adds the number of times the motor is seen as not moving # indicates whether the velocity command is sent as normal or if the # ramp-up prodecure should be used. # 0 = normal mode # 1 = ramp-up mode self.operation_mode = 0 self.ramp_start = time.time() # time when the ramp up procedure began #=========================# # Create ROS Node Objects #=========================# rospy.init_node("steering_node") # Specify general parameters self.rl_axes = 3 self.manual_deadman_button = rospy.get_param("~manual_deadman", 4) # Indicates whether the deadman switch has been pressed on the joystick # It must be pressed in order for the system to be able to start running self.manual_deadman_on = False self.autonomous_deadman_button = rospy.get_param( "~autonomous_deadman", 5) self.autonomous_deadman_on = False self.timeout = rospy.get_param( "~timeout", 1 ) # number of seconds allowed since the last setpoint message before sending a 0 command self.timeout_start = time.time() self.scale_angle = rospy.get_param("~scale_angle", 1) self.scale_angle = min(self.scale_angle, 1) print("Manual deadman button: " + str(self.manual_deadman_button)) print("Autonomous deadman button: " + str(self.autonomous_deadman_button)) print("Scale angle: " + str(self.scale_angle)) # Publishers and Subscribers rospy.on_shutdown(self.close) # shuts down the Phidget properly self.setpoint_sub = rospy.Subscriber("/steering_node/angle_setpoint", Float64, self.setpoint_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) self.moving_sub = rospy.Subscriber("/steering_node/motor/is_moving", Bool, self.moving_callback, queue_size=3) self.angle_sub = rospy.Subscriber("/steering_node/filtered_angle", Float64, self.angle_callback, queue_size=3) self.gear_sub = rospy.Subscriber("/velocity_node/gear", Int8, self.gear_callback, queue_size=3) self.joystick_sub = rospy.Subscriber("/joy", Joy, self.joystick_callback, queue_size=1) # Run 'spin' loop at 30Hz self.rate = rospy.Rate(30) #================================# # 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 # Serial number of previous phidget that broke #self.ch.setDeviceSerialNumber(522972) # Current Serial number self.ch.setDeviceSerialNumber(522722) 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(1000) 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.rescale_factor = (math.pi / 180.) * (1.8 / 16) / (26. + (103. / 121.)) self.ch.setRescaleFactor( self.rescale_factor) # converts steps to radians # 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) # Define max acceleration and velocity self.max_velocity = self.max_vel_scale * self.ch.getMaxVelocityLimit() self.max_acceleration = self.max_accel_scale * self.ch.getMaxAcceleration( ) self.ch.setAcceleration(self.max_acceleration) #===============# # Run main loop #===============# # self.mainLoop() self.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_current = 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 spin(self): while not rospy.is_shutdown(): self.control_loop() self.rate.sleep() #===================================# # Velocity Set Function # * change this function whenever you use a different source for setting the # * velocity besides the controller. #===================================# def control_loop(self): # Check if deadman switch is pressed if ((self.manual_deadman_on or self.autonomous_deadman_on) and (time.time() - self.timeout_start) < self.timeout and (self.gear != 0)): # FIXME: Uncomment this line if you are able to test it. Also uncomment the corresponding line in the 'else' condition. # self.ch.setEngaged(True) # Determine direction error = self.angle_setpoint - self.angle if not (error == 0): direction = abs(error) / error else: direction = 1 if (abs(error) > self.angle_tolerance): self.velocity = direction * self.scale_angle * self.max_velocity else: self.velocity = 0 #===== Stall Check and Set Velocity =====# try: # Check if the system is stalled if (self.check_stall()): # Initiate "ramp-up" mode self.reset_rampup() if (self.operation_mode == 0): # Normal mode self.ch.setVelocityLimit(self.velocity) # Scale down the acceleration as the velocity increases # # (Linear) # accel_vel_scale = (self.max_velocity - abs(self.velocity_current))/(self.max_velocity) # accel_vel_scale = min(accel_vel_scale, 1) # accel_vel_scale = max(accel_vel_scale, 0) # (Inverse) # parameters unchanged_length = 0.75 # increase this value to increase the range where the accelerations remains unreduced final_scale = 10 # increase this value to decrease the final scale value at max velocity try: accel_vel_scale = 1 / ( final_scale * (abs(self.velocity_current) / self.max_velocity)** unchanged_length) except ZeroDivisionError: accel_vel_scale = 1 accel_vel_scale = min(accel_vel_scale, 1) accel_vel_scale = max(accel_vel_scale, 0) # Set acceleration print("Current velocity: %f, max: %f" % (self.velocity_current, self.max_velocity)) print("Accel scale: %f" % accel_vel_scale) self.ch.setAcceleration(accel_vel_scale * self.max_acceleration) else: # Ramp-up mode t_curr = time.time() scale_vel = min( (t_curr - self.ramp_start) / self.ramp_time_vel, 1)**3 scale_accel = min( (t_curr - self.ramp_start) / self.ramp_time_accel, 1) scale_accel = max(scale_accel, 0.001) # DEBUG: print accel scaling print("t_curr: %f" % t_curr) print("time diff: %f" % (t_curr - self.ramp_start)) print("accel scale: %f" % scale_accel) print("vel scale: %f" % scale_vel) print("Accel: %f" % (scale_accel * self.max_acceleration)) print("Vel: %f" % (scale_vel * self.velocity)) self.ch.setAcceleration(scale_accel * self.max_acceleration) self.ch.setVelocityLimit(scale_vel * self.velocity) # When ramping has finished resume normal operation if (scale_vel == 1 and scale_accel == 1): self.operation_mode = 0 except PhidgetException as e: DisplayError(e) else: self.ch.setVelocityLimit(0) # FIXME: Uncomment this line if you are able to test it. Also uncomment the corresponding line in the 'if' condition. # self.ch.setEngaged(False) def setpoint_callback(self, msg): # Read in new setpoint and saturate against the bounds self.angle_setpoint = min(msg.data, self.max_angle) self.angle_setpoint = max(self.angle_setpoint, self.min_angle) def angle_callback(self, msg): # Read the current steering angle self.angle = msg.data def moving_callback(self, msg): # Update 'moving' to indicate whether the motor is moving or not self.moving = msg.data def gear_callback(self, msg): # Update gear self.gear = msg.data def check_stall(self): stalled = False if (abs(self.ch.getVelocity()) > self.velocity_tolerance and self.moving == False): self.repeats += 1 if (self.repeats > self.max_repeats): stalled = True else: self.repeats = 0 return stalled def reset_rampup(self): if (self.operation_mode == 0): self.ch.setVelocityLimit(0) self.ramp_start = time.time() self.operation_mode = 1 self.ch.setAcceleration(self.max_acceleration) def joystick_callback(self, msg): # Update timeout time self.timeout_start = time.time() # One of these buttons must be on for this node to send a steering command if (msg.buttons[self.manual_deadman_button]): self.manual_deadman_on = True else: self.manual_deadman_on = False if (msg.buttons[self.autonomous_deadman_button]): self.autonomous_deadman_on = True else: self.autonomous_deadman_on = False
class StepperController(): def __init__(self, serialNum, rescaleFactor, defaultVel, maxVelocity=360, currentLimit=1.8): self.stepper = None self.serialNum = serialNum self.defaultVelocity = defaultVel self.maxVelocity = maxVelocity self.rescaleFactor = rescaleFactor self.currentLimit = currentLimit self.velocitySetting = 0 self.initStepper() def initStepper(self): self.stepper = Stepper() try: self.stepper.setDeviceSerialNumber(self.serialNum) self.stepper.setChannel(0) self.stepper.setOnErrorHandler(self.onErrorEvent) self.stepper.setOnAttachHandler(self.onStepperAttached) self.stepper.setOnDetachHandler(self.onStepperDetached) self.stepper.openWaitForAttachment(5000) self.stepper.setRescaleFactor(self.rescaleFactor) self.stepper.setVelocityLimit(self.maxVelocity) self.stepper.setCurrentLimit(self.currentLimit) # Use CONTROL_MODE_RUN self.stepper.setControlMode(1) self.stepper.setEngaged(1) except PhidgetException as e: self.terminateValveControl("Phidget Exception " + str(e.code) + " " + e.details) def onStepperAttached(self, attached): try: print("\nAttach Event Detected (Information Below)") print("===========================================") print("Library Version: %s" % attached.getLibraryVersion()) print("Serial Number: %d" % attached.getDeviceSerialNumber()) print("Channel: %d" % attached.getChannel()) print("Channel Class: %s" % attached.getChannelClass()) print("Channel Name: %s" % attached.getChannelName()) print("Device ID: %d" % attached.getDeviceID()) print("Device Version: %d" % attached.getDeviceVersion()) print("Device Name: %s" % attached.getDeviceName()) print("Device Class: %d" % attached.getDeviceClass()) print("\n") except PhidgetException as e: self.terminateValveControl("Phidget Exception " + str(e.code) + " " + e.details) def onStepperDetached(self, detached): try: print("\nDetach event on Port %d Channel %d" % (detached.getHubPort(), detached.getChannel())) except PhidgetException as e: terminateValveControl("onStepperDetached! Phidget Exception " + str(e.code) + " " + e.details) self.stepper.close() self.initStepper() self.setVelocity(self.velocitySetting) def onErrorEvent(self, e, eCode, description): print("Error event #%i : %s" % (eCode, description)) self.terminateValveControl("Phidgets onError raised") def terminateValveControl(self, msg="NO_MSG"): endTest(msg) print(msg) if (self.stepper is not None): self.stepper.setVelocityLimit(0) self.stepper.setEngaged(0) self.stepper.close() def setVelocity(self, vel): assert abs(vel) <= self.maxVelocity self.velocitySetting = vel try: self.stepper.setVelocityLimit(vel) except Exception as e: print(str(e)) def setDefaultVelocity(self, newDefaultVel): if (newDefaultVel > 0 and newDefaultVel <= self.maxVelocity): self.defaultVelocity = newDefaultVel return "Default velocity set to " + str(newDefaultVel) + " deg/s" else: return "Set default velocity error: value out of range" def __del__(self): if (self.stepper is not None): try: self.setVelocity(0) self.stepper.setEngaged(0) except Exception: print("Failed to deactive stepper")
class SteeringController(): def __init__(self): # Set control mode # 0 = step mode (target position) # 1 = run mode (target velocity) self.control_mode = 1 self.velocity = 0 # rad/s, starting velocity self.angle = 0 # rad, starting angle before joystick command received # Max and Min angles to turn of the velocity if they are reached self.max_angle = 2*math.pi self.min_angle = -2*math.pi self.max_accel_scale = 0.01 self.max_vel_scale = -0.75 # negative value is used to reverse the steering direction, makes right direction on analog stick equal right turn going forward #===============================================================# # These parameters are used in the stall detection and handling #===============================================================# # Tuning parameters self.max_repeats = 5 # the maximum number of times the motor can be seen as not moving before reseting self.ramp_time_vel = 1 # number of seconds to ramp up to full velocity again self.ramp_time_accel = 1 # number of seconds to ramp up to full acceleration again # Operation states self.moving = False # indicates whether the motor is currently moving self.repeats = 0 # adds the number of times the motor is seen as not moving # indicates whether the velocity command is sent as normal or if the # ramp-up prodecure should be used. # 0 = normal mode # 1 = ramp-up mode self.operation_mode = 0 self.ramp_start = time.time() # time when the ramp up procedure began #=========================# # Create ROS Node Objects #=========================# rospy.init_node("steering_controller") rospy.on_shutdown(self.close) # shuts down the Phidget properly # Specify general parameters self.rl_axes = 3 self.deadman_button = rospy.get_param("~deadman", 4) self.scale_angle = rospy.get_param("~scale_angle", 1) self.scale_angle = min(self.scale_angle, 1) print("Deadman button: " + str(self.deadman_button)) print("Scale angle: " + str(self.scale_angle)) 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) self.moving_sub = rospy.Subscriber("/steering_node/motor/is_moving", Bool, self.moving_callback, queue_size = 3) #================================# # 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.rescale_factor = (math.pi/180.)*(1.8/16)/(26.+(103./121.)) self.ch.setRescaleFactor(self.rescale_factor) # converts steps to radians # 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) # Set acceleration self.max_acceleration = self.max_accel_scale*self.ch.getMaxAcceleration() self.ch.setAcceleration(self.max_acceleration) #===============# # 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 #===================================# # Velocity Set Function # * change this function whenever you use a different source for setting the # * velocity besides the controller. #===================================# def joy_callback(self, msg): # Check if deadman switch is pressed if (msg.buttons[self.deadman_button]): # check if the current angle is beyond the bounds self.angle = self.ch.getPosition() # Read right joystick analog "Left/Right" value # (only go to 90% ov maximum velocity so it doesn't stall out) self.velocity = self.max_vel_scale*self.scale_angle*msg.axes[self.rl_axes]*self.ch.getMaxVelocityLimit() # DEBUG: Print # print("Angle: " + str(self.angle)) # print("Velocity: " + str(self.angle)) # print("Max: " + str(self.max_angle) + ", Min: " + str(self.min_angle)) #===== Uses Min/Max =====# # if not ((self.angle > self.max_angle and self.velocity > 0) or (self.angle < self.min_angle and self.velocity < 0)): # try: # self.ch.setVelocityLimit(self.velocity) # except PhidgetException as e: # DisplayError(e) # else: # try: # self.ch.setVelocityLimit(0) # except PhidgetException as e: # DisplayError(e) #===== No Min/Max considered =====# try: # Check if the system is stalled if (self.check_stall()): # Initiate "ramp-up" mode self.reset_rampup() if (self.operation_mode == 0): # Normal mode self.ch.setVelocityLimit(self.velocity) else: # Ramp-up mode t_curr = time.time() scale_vel = min((t_curr - self.ramp_start)/self.ramp_time_vel, 1)**2 scale_accel = min((t_curr - self.ramp_start)/self.ramp_time_accel, 1) self.ch.setAcceleration(self.max_acceleration) self.ch.setVelocityLimit(scale_vel*self.velocity) # When ramping has finished resume normal operation if (scale_vel == 1 and scale_accel == 1): self.operation_mode = 0 except PhidgetException as e: DisplayError(e) def moving_callback(self, msg): # Update 'moving' to indicate whether the motor is moving or not self.moving = msg.data def check_stall(self): stalled = False if (self.ch.getVelocity() != 0 and self.moving == False): self.repeats += 1 if (self.repeats > self.max_repeats): stalled = True else: self.repeats = 0 return stalled def reset_rampup(self): self.ramp_start = time.time() self.operation_mode = 1
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)
class FilterWheel: def __init__(self): 'Sets up internal variables and initializes the stepper and serial port.' self._VELOCITY_LIMIT = 5000 self._filterPos = None self._hallData = None self.stepper = Stepper() self.stepper.openWaitForAttachment(10000) self.stepper.setControlMode(StepperControlMode.CONTROL_MODE_RUN) self.stepper.setAcceleration(20000) self.stepper.setCurrentLimit(0.9) self.SerialPortAddress = '/dev/ttyACM0' self.SerialPort = serial.Serial(self.SerialPortAddress, 9600, timeout=2) print("Filterwheel connection successful.") def disconnDev(self): 'Disconnects the stepper and serial port.' self.stepper.setVelocityLimit(0) self.stepper.setEngaged(False) self.stepper.close() self.SerialPort.close() print("Disconnect successful") return def getHallData(self, index): '''Gets the Hall sensor data and returns the sensor value at the index. Indices 0 and 1 store if a magnet is detected (0 returned) or not (1).''' self.SerialPort.write('s') self._hallData = self.SerialPort.readline().rstrip('\r\n').split(',') return int(self._hallData[index]) def getFilterPos(self): 'Returns the position of the filterwheel, an integer between 0 and 5.' return str(self._filterPos) def home(self): 'Homes the filter wheel to position 0.' self.stepper.setEngaged(True) self.stepper.setVelocityLimit(self._VELOCITY_LIMIT) while self.getHallData(0) != 0 or self.getHallData(1) != 0: pass self._filterPos = 0 self.stepper.setVelocityLimit(0) self.stepper.setEngaged(False) print("Homed") return 'home 1' def moveFilter(self, num): 'Moves the filter to the specified position, an integer between 0 and 5.' self.stepper.setEngaged(True) self.stepper.setVelocityLimit(self._VELOCITY_LIMIT) if self._filterPos == None: print("Not homed, homing first.") self.home() if num >= self._filterPos: swaps = abs(num - self._filterPos) else: swaps = 6 - self._filterPos + num while swaps != 0: while self.getHallData(0) == 0: pass while self.getHallData(0) != 0: pass swaps -= 1 self._filterPos = num self.stepper.setVelocityLimit(0) self.stepper.setEngaged(False) print("At filter position %d." % num) return 'True'
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
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()
class Positioner: def __init__(self, stepper_sn, debug=False): self.debug = debug try: self.channel = Stepper() except Exception as e: print('\x1b[1;31mPhidget::SN::{}\x1b[0m <> {}'.format( stepper_sn, e)) try: self.channel.setDeviceSerialNumber(stepper_sn) self.channel.setIsHubPortDevice(False) self.channel.setChannel(0) self.channel.setOnAttachHandler(PhidgetHandlers.on_attach_handler) self.channel.setOnDetachHandler(PhidgetHandlers.on_detach_handler) self.channel.setOnErrorHandler(PhidgetHandlers.on_error_handler) self.channel.setOnPositionChangeHandler( PhidgetHandlers.on_position_change_handler) self.channel.openWaitForAttachment(10000) except PhidgetException as e: print('\x1b[1;31mPhidget::SN::{}\x1b[0m <> {}'.format( stepper_sn, e)) def __del__(self): self.close() def _get_device_str(self): return str('\x1b[1;34mPhidget::SN::{}\x1b[0m'.format( self.channel.getDeviceSerialNumber())) def close(self): print('\x1b[1;34mPhidget::SN::{}\x1b[0m > Closing channel {}'.format( self.channel.getDeviceSerialNumber(), self.channel.getChannel())) self.channel.close() def set_acceleration(self, acc): self.channel.setAcceleration(acc) def set_target_absolute_position(self, position): # One full rotation is 2 mm movement along the rail shaft_conversion = 1 / 2 self.channel.setTargetPosition(position * shaft_conversion) def wait_to_settle(self): is_moving = self.channel.getIsMoving() while is_moving: time.sleep(0.01) is_moving = self.channel.getIsMoving() position = self.channel.getPosition() print( '\x1b[1;34mPhidget::SN::{}\x1b[0m < Position {:8.4f}\t<=>\t{:8.4f} mm' .format(self.channel.getDeviceSerialNumber(), position, position * 2)) return is_moving def set_velocity_limit(self, v_lim): self.channel.setVelocityLimit(v_lim) def print_movement_info(self): print('\x1b[1;34mPhidget::SN::{}\x1b[0m < minAcceleration {:8.4f}'. format(self.channel.getDeviceSerialNumber(), self.channel.getMinAcceleration())) print('\x1b[1;34mPhidget::SN::{}\x1b[0m < maxAcceleration {:8.4f}'. format(self.channel.getDeviceSerialNumber(), self.channel.getMaxAcceleration())) print('\x1b[1;34mPhidget::SN::{}\x1b[0m < Acceleration {:8.4f}'.format( self.channel.getDeviceSerialNumber(), self.channel.getAcceleration())) print( '\x1b[1;34mPhidget::SN::{}\x1b[0m < velocityLimit {:8.4f}'.format( self.channel.getDeviceSerialNumber(), self.channel.getVelocityLimit()))