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