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()
