def __init__(self):
r""" The initialization of the card.
Tries to communicate to the board and returns an error if the card fails to establish.
"""
self.errorvalue = False
try:
self.Stepper = Stepper()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
sys.exit(1)
try:
self.Stepper.setOnAttachHandler(StepperAttached)
self.Stepper.setOnDetachHandler(StepperDetached)
self.Stepper.setOnErrorHandler(ErrorEvent)
self.Stepper.setOnPositionChangeHandler(PositionChangeHandler)
print("Waiting for the Phidget Stepper Object to be attached...")
self.Stepper.openWaitForAttachment(3000)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Press Enter to Exit...\n")
readin = sys.stdin.read(1)
sys.exit(1)
def __init__(self, serial=0, x_channel=0, y_channel=1, z_channel=3):
"""
:param serial:
:param x_channel:
:param y_channel:
:param z_channel:
"""
super().__init__()
# Load drivers dependent on system architecture
__arch = platform.architecture()
if __arch[0] == '64bit':
__drivers = os.path.abspath('drivers/x64')
else:
__drivers = os.path.abspath('drivers/x86')
os.environ['PATH'] = __drivers + ";" + os.environ['PATH']
self._connected = False
self._serial = serial
self._channels = [x_channel, y_channel, z_channel]
self._axes = [Stepper(), Stepper(), Stepper()]
self._initialized = [False] * 3
self._position_changed_callback = None
self._queue_active = False
self._queue = Queue()
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 __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 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 __init__(self, parent=None):
super(autofocuser, self).__init__(parent)
self.ch = Stepper()
self.ch.openWaitForAttachment(5000)
self.ch.setEngaged(True)
self.full_scale = 27300
self.image_count = 0
self.track_position = False
self.pool = ThreadPool(processes=3)
self.velocity = 0
self.ch.setDataInterval(100)
self.position = 0
self.focused_position = 0
self.status_dict = {
'current limit': self.ch.getCurrentLimit,
'control mode': self.ch.getControlMode,
# 'setting min position: ': self.ch.setMinPosition(0),
'min position': self.ch.getMinPosition,
# 'setting max position: ': self.ch.setMaxPosition(self.full_scale),
'max position': self.ch.getMaxPosition,
'rescale factor': self.ch.getRescaleFactor,
'target position': self.ch.getTargetPosition,
'acceleration': self.ch.getAcceleration,
'engaged?': self.ch.getEngaged,
'max velocity:': self.ch.getMaxVelocityLimit,
'data interval': self.ch.getDataInterval,
'min data interval': self.ch.getMinDataInterval
}
for k, v in self.status_dict.items():
comment('{}: {}'.format(k, v()))
self.ch.setOnVelocityChangeHandler(self.velocity_change_handler)
self.ch.setOnPositionChangeHandler(self.position_change_handler)
self.image_title = 0
self.focus_model = load_model(
os.path.join(experiment_folder_location, 'VGG_model_5.hdf5'))
self.focus_model._make_predict_function()
self.belt_slip_offset = 120
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))
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'
break
except KeyboardInterrupt:
print('Stopping.')
except RPLidarException as e:
print(e)
print("complete")
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
outfile.close()
print(lidar.motor)
try:
ch = Stepper()
except RuntimeError as e:
print("Runtime Exception %s" % e.details)
print("Press Enter to Exit...\n")
readin = sys.stdin.read(1)
exit(1)
try:
ch.setOnAttachHandler(StepperAttached)
ch.setOnDetachHandler(StepperDetached)
ch.setOnErrorHandler(ErrorEvent)
ch.setOnPositionChangeHandler(PositionChangeHandler)
# Please review the Phidget22 channel matching documentation for details on the device
# and class architecture of Phidget22, and how channels are matched to device features.
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 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 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):
# 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 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()))
class PhidgetStepper(object):
r"""The main UI class for the PhidgetStepper1067 interface.
This class contains the ability to communicate wiht the interface and read card data for reference.
"""
########################################################################
# Initialize/Establish connection with the board #
########################################################################
def __init__(self):
r""" The initialization of the card.
Tries to communicate to the board and returns an error if the card fails to establish.
"""
self.errorvalue = False
try:
self.Stepper = Stepper()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
sys.exit(1)
try:
self.Stepper.setOnAttachHandler(StepperAttached)
self.Stepper.setOnDetachHandler(StepperDetached)
self.Stepper.setOnErrorHandler(ErrorEvent)
self.Stepper.setOnPositionChangeHandler(PositionChangeHandler)
print("Waiting for the Phidget Stepper Object to be attached...")
self.Stepper.openWaitForAttachment(3000)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Press Enter to Exit...\n")
readin = sys.stdin.read(1)
sys.exit(1)
########################################################################
# DISPLAY: displays important information about the steppers #
########################################################################
# def displayDeviceInfo(self):
# r""" Displays card data.
# If card is succesfully initialized, the data on the card is dsiplayed in the terminal.
# """
# print("|------------|----------------------------------|--------------|------------|")
# print("|- Attached -|- Type -|- Serial No. -|- Version -|")
# print("|------------|----------------------------------|--------------|------------|")
# print("|- %8s -|- %30s -|- %10d -|- %8d -|" % (self.Stepper.isAttached(), self.Stepper.getDeviceName(), self.Stepper.getSerialNum(), self.Stepper.getDeviceVersion()))
# print("|------------|----------------------------------|--------------|------------|")
# print("Number of Motors: %i" % (self.Stepper.getMotorCount()))
########################################################################
# SETUP: sets up the velocity and acceleration limits #
########################################################################
def stepperSetup(self, Velocity_limit_mm, Acceleration_limit_mm, Current_limit):
"""
Set up the input parameters of the Stepper Motor.
Selection of the velocity, acceleration, and current limits for the stepper.
Args:
Velocity_limit_mm (float): The maximum velocity of the sled in mm/sec. Optimal setting is ~15-20 mm/sec.
Acceleration_limit_mm (float): The maximum acceleration of the sled in mm/sec^2. Optimal setting is ~50-70 mm/sec^2.
Current_limit (float, [0-4]): The current limit (in Amps) delivered to the motor.
Returns:
None
Raises:
AttributeError: None
"""
self.setEngaged(False)
#VELOCITY: make sure the velocity input isnt out of bounds
self.setVelocity(Velocity_limit_mm)
#ACCELERATION: make sure the acceleration isnt out of bounds
self.setAcceleration(Acceleration_limit_mm)
#CURRENT: limits the amount of current supplied to the board
self.setCurrentLimit(Current_limit)
# Initialize the Stepper Position
current_position = self.getCurrentPosition()
self.setTargetPosition(current_position)
self.setEngaged(False)
########################################################################
# GETTERS: Get all values relevant to the motor in form self.getter #
########################################################################
def getCurrentLimit(self):
"""
Returns the Current Limit for the stepper motor.
Args:
None
Returns:
currentLimit (float): will be a value [0, 4].
Raises:
AttributeError: None
"""
currentLimit = self.Stepper.getCurrentLimit(0)
return currentLimit
def getVelocityLimit(self):
"""
Returns the Velocity Limit for the stepper motor, essentially the upper bound for velocity during any operation of the stepper motor. (Not to be confused with self.getVelocity() which returns the current velocity of the motor (0 if not moving))
Args:
None
Returns:
velocityLimit (float): The velocity limit value will be in mm/sec.
Raises:
AttributeError: None
"""
velocityLimit = self.Stepper.getVelocityLimit(0)*self.getConversionFactor()
return velocityLimit
def getVelocity(self):
"""
Returns the current Velocity of the stepper motor. (Not to be confused with self.getVelocityLimit() which returns the upper bound for the velocity of the motor)
Args:
None
Returns:
velocity (float): The current velocity value will be in mm/sec.
Raises:
AttributeError: None
"""
velocity = self.Stepper.getVelocity(0)*self.getConversionFactor()
return velocity
def getAcceleration(self):
"""
Returns the Acceleration limit of the stepper motor.
Args:
None
Returns:
acceleration (float): The current acceleraion value will be in mm/sec^2.
Raises:
AttributeError: None
"""
acceleration = self.Stepper.getAcceleration(0)*self.getConversionFactor()
return acceleration
def getCurrentPosition(self):
"""
Returns the current Position of the stepper motor.
Args:
None
Returns:
currentPosition (float): The current position value will be in mm. Referenced from the zero value, wherever it is defined to be.
Raises:
AttributeError: None
"""
currentPosition = self.Stepper.getCurrentPosition(0)*self.getConversionFactor()
return currentPosition
def getConversionFactor(self):
"""
Returns the Conversion Factor for the stepper motor/threaded rod combo. If those two things are changed, conversion factor wil change.
Args:
None
Returns:
conversionFactor (float): The conversion factor value will be in mm/steps.
Raises:
AttributeError: None
"""
conversionFactor = self.setConversionFactor()
return conversionFactor
########################################################################
# SETTERS: Set all values for motor in form self.setter(value) #
########################################################################
def setEngaged(self, state): #state is a True or False Boolean
"""
Sets the state (connectedness) of the stepper motor.
Args:
State (bool): True means that the stepper will be engaged, False means it will be disengaged.
Returns:
None.
Raises:
AttributeError: None
"""
self.Stepper.setEngaged(0, state)
def setConversionFactor(self, factor=2*16*1.016/(1000*200)):
"""
Sets the Conversion Factor of the stepper motor. This is used to convert the TPI (Threads Per Inch) into mm for many getFunctions.
Args:
factor (float): The factor needs to be calculated by hand per rig setup, but only needs to be done once. DefaultValue = 2*16*1.016/(1000*200).
Returns:
None.
Raises:
AttributeError: None
"""
return factor
def setCurrentLimit(self, current):
"""
Sets the Current Limit for the stepper motor.
Args:
current (float): Any value is accepted, but if a value is > maxCurrent or < minCurrent, then 0, 4 are set respectively. Unit is in amps.
Returns:
None.
Raises:
AttributeError: None
"""
min_current = self.Stepper.getCurrentMin(0)
max_current = self.Stepper.getCurrentMax(0)
if current < min_current:
self.Stepper.setCurrentLimit(0, min_current)
elif current > max_current:
self.Stepper.setCurrentLimit(0, max_current)
else:
self.Stepper.setCurrentLimit(0, current)
def setVelocity(self, velocity):
"""
Sets the Velocity of the stepper motor. During operation, the velocity of the motor, will not exceed this value.
Args:
velocity (float): Any value is accepted, but if a value is > maxVelocity or < minVelocity, then minVelocity/maxVelocity are set respectively. Units are in mm/sec.
Returns:
None.
Raises:
AttributeError: None
"""
velocity_steps = velocity/self.getConversionFactor() #converts from mm/sec -> 1/16th step/sec
max_V = self.Stepper.getVelocityMax(0)
min_V = self.Stepper.getVelocityMin(0)
if velocity_steps > max_V:
self.Stepper.setVelocityLimit(0,max_V)
elif velocity_steps < min_V:
self.Stepper.setVelocityLimit(0,min_V)
else:
self.Stepper.setVelocityLimit(0, int(velocity_steps))
def setAcceleration(self, acceleration):
"""
Sets the Acceleration of the stepper motor. During operation, the acceleration of the motor, will not exceed this value.
Args:
acceleration (float): Any value is accepted, but if a value is > maxAcceleration or < minAcceleration, then minAcceleration/maxAcceleration are set respectively. Units are in mm/sec^2.
Returns:
None.
Raises:
AttributeError: None
"""
acceleration = acceleration/self.getConversionFactor() #converts from mm/sec -> 1/16th step/sec
max_A = self.Stepper.getAccelerationMax(0)
min_A = self.Stepper.getAccelerationMin(0)
if acceleration > max_A:
self.Stepper.setAcceleration(0,max_A)
elif acceleration < min_A:
self.Stepper.setAcceleration(0,min_A)
else:
self.Stepper.setAcceleration(0, int(acceleration))
def setCurrentPosition(self, position):
"""
Sets the Current Position of the stepper motor. This is used for "zero"ing out the motor position. Should only be called when setEngaged(False)
Args:
position (float): Any value is accepted, but if position is < minPosition or > maxPosition, set min/max respectively. Motor will know that it is in that new, set, position.
Returns:
None.
Raises:
AttributeError: None
"""
self.setEngaged(False)
position = position/self.getConversionFactor()
min_position = 0
max_position = self.Stepper.getPositionMax(0)
if position < 0:
self.Stepper.setCurrentPosition(0, min_position)
elif position > max_position:
self.Stepper.setCurrentPosition(0, max_position)
else:
self.Stepper.setCurrentPosition(0, int(position))
self.setEngaged(True)
def setTargetPosition(self, targetPosition):
"""
Sets the Target Podiyion of the stepper motor. One of the main methods in home and jog. By having the current position be different from the target position, the motor will move.
Args:
targetPosition (float): Any value is accepted, but if a value is > maxVelocity or < minVelocity, then minVelocity/maxVelocity are set respectively. Units are in mm/sec.
Returns:
None.
Raises:
AttributeError: None
"""
#Large positive Value will move steppers counterclockwise
#Large negative value will move steppers clockwise
#target position is in mm
targetPosition = targetPosition/self.getConversionFactor()
self.setEngaged(True)
self.Stepper.setTargetPosition(0, int(targetPosition))
########################################################################
# MOVEMENT FEATURES #
########################################################################
def jog(self, mm_interval):
"""
Jogs the motor a certain distance in a certain direction.
The motor will jog forward (towards the center) if the mm_interval is positive, and backward (away from center) if the mm_interval is negative.
Args:
mm_interval (float): The distance in mm that you would like to jog the motor.
Returns:
None.
Raises:
AttributeError: None
"""
self.setEngaged(True)
self.setTargetPosition(self.getCurrentPosition()+mm_interval)
self.setEngaged(False)
def errorOut(self):
self.errorvalue = True
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()
class autofocuser(QtCore.QObject):
'''
assumes that the stage is maxed out in the CCW direction at start
which is the maxed out negative direction. therefore any of our
steps in the positive direction will cause us to move from the
max position of the focus
'''
# TODO implement a property that prevents the motor from ever going outside of its range.
position_and_variance_signal = QtCore.pyqtSignal('PyQt_PyObject')
def __init__(self, parent=None):
super(autofocuser, self).__init__(parent)
self.ch = Stepper()
self.ch.openWaitForAttachment(5000)
self.ch.setEngaged(True)
self.full_scale = 27300
self.image_count = 0
self.track_position = False
self.pool = ThreadPool(processes=3)
self.velocity = 0
self.ch.setDataInterval(100)
self.position = 0
self.focused_position = 0
self.status_dict = {
'current limit': self.ch.getCurrentLimit,
'control mode': self.ch.getControlMode,
# 'setting min position: ': self.ch.setMinPosition(0),
'min position': self.ch.getMinPosition,
# 'setting max position: ': self.ch.setMaxPosition(self.full_scale),
'max position': self.ch.getMaxPosition,
'rescale factor': self.ch.getRescaleFactor,
'target position': self.ch.getTargetPosition,
'acceleration': self.ch.getAcceleration,
'engaged?': self.ch.getEngaged,
'max velocity:': self.ch.getMaxVelocityLimit,
'data interval': self.ch.getDataInterval,
'min data interval': self.ch.getMinDataInterval
}
for k, v in self.status_dict.items():
comment('{}: {}'.format(k, v()))
self.ch.setOnVelocityChangeHandler(self.velocity_change_handler)
self.ch.setOnPositionChangeHandler(self.position_change_handler)
self.image_title = 0
self.focus_model = load_model(
os.path.join(experiment_folder_location, 'VGG_model_5.hdf5'))
self.focus_model._make_predict_function()
self.belt_slip_offset = 120
# self.step_to_position(self.full_scale)
# self.autofocus()
def velocity_change_handler(self, self2, velocity):
# print('VELOCITY CHANGED:',velocity)
self.velocity = velocity
def position_change_handler(self, self2, position):
# print('POSITION CHANGED:',position)
self.position = position
@QtCore.pyqtSlot('PyQt_PyObject')
def vid_process_slot(self, image):
self.image = image
# print(image.shape)
self.image_count += 1
# print('image received in autofocus')
def get_position(self):
self.position = self.ch.getPosition()
comment('stepper position: {}'.format(self.position))
return self.position
def step_to_relative_position(self, position):
self.ch.setAcceleration(10000)
self.ch.setVelocityLimit(10000)
# TODO check if the given position will make us exceed the range
self.ch.setTargetPosition(self.position + position)
self.position += position
def roll_forward(self):
self.ch.setControlMode(1)
self.ch.setAcceleration(15000)
self.ch.setVelocityLimit(-2000)
def roll_backward(self):
self.ch.setControlMode(1)
self.ch.setAcceleration(15000)
self.ch.setVelocityLimit(2000)
def stop_roll(self):
self.ch.setVelocityLimit(0)
self.ch.setControlMode(0)
self.ch.setAcceleration(10000)
# comment('focus at {} steps'.format(self.get_position()))
def swing_range(self):
self.ch.setVelocityLimit(2000)
self.ch.setTargetPosition(-self.full_scale + 2500)
def retract_objective(self):
self.focused_position = self.get_position()
self.step_to_relative_position(-70000)
while self.ch.getIsMoving() == True:
time.sleep(.1)
return True
def return_objective_to_focus(self):
self.ch.setTargetPosition(self.focused_position)
def get_network_output(self, img):
img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
img = cv2.resize(img, (100, 100))
img = np.expand_dims(img, axis=4)
img = np.expand_dims(img, axis=0)
with graph.as_default():
prediction = self.focus_model.predict(img, batch_size=1)[0][0]
print('focus metric:', prediction)
return prediction
@QtCore.pyqtSlot()
def autofocus(self):
positions_to_check = 40
steps_between_positions = 100
threshold = .75
num_good_scores = 0
for i in range(positions_to_check):
QApplication.processEvents()
self.step_to_relative_position(steps_between_positions)
score = self.get_network_output(self.image)
if score > threshold: num_good_scores += 1
if num_good_scores > 2: break
self.step_to_relative_position(-2 * steps_between_positions -
self.belt_slip_offset)
# now verify that it is focused
QApplication.processEvents()
# time.sleep(1)
# QApplication.processEvents()
# print('checking focus...')
# score = self.get_network_output(self.image)
# i = 0
# while score < threshold:
# i += 1
# self.step_to_relative_position(-steps_between_positions)
# QApplication.processEvents()
# score = self.get_network_output(self.image)
# if i > 4: break
def autofocus_old(self):
# we want to slowly roll through some range and get a bunch of outputs
# from the network
focus_metrics = []
variances = []
focus_metrics.append(self.get_network_output(self.image))
variances.append(cv2.Laplacian(self.image, cv2.CV_64F).var())
positions_to_check = 40
steps_between_positions = 100
for i in range(positions_to_check):
QApplication.processEvents()
self.step_to_relative_position(steps_between_positions)
# time.sleep(.1)
focus_metrics.append(self.get_network_output(self.image))
variances.append(cv2.Laplacian(self.image, cv2.CV_64F).var())
print(focus_metrics)
variances = [var / max(variances) for var in variances]
# now we want to find where we have several high-scoring positions together
num_highscores = 0
threshold = .75
for i in range(positions_to_check):
if focus_metrics[i] > threshold:
num_highscores += 1
if num_highscores > 3:
focused_position = i - 2
print(focused_position)
num_highscores = 0
steps_to_go_back = (positions_to_check -
focused_position) * steps_between_positions
self.position_and_variance_signal.emit((variances, focus_metrics))
self.step_to_relative_position(-steps_to_go_back -
self.belt_slip_offset)
# range = 2000
# variance1, location1, variances1 = self.focus_over_range(range)
# self.step_to_relative_position(-range)
# variance2, location2, variances2 = self.focus_over_range(-range)
# variances2.reverse()
# total_variances = variances2 + variances1
# self.position_and_variance_signal.emit(([],total_variances))
# if variance1 > variance2:
# self.ch.setTargetPosition(location1)
# elif variance2 > variance1:
# self.ch.setTargetPosition(location2)
# while self.ch.getIsMoving() == True:
# time.sleep(.1)
def focus_over_range(self, range):
self.pool.apply_async(self.step_to_relative_position(range))
variances = []
positions = []
old_position = 0
self.image_title += 1
self.ch.setAcceleration(15000)
self.ch.setVelocityLimit(2000)
while self.ch.getIsMoving() == True:
QApplication.processEvents()
new_position = self.position
if old_position != new_position: positions.append(self.position)
old_position = new_position
# we want to focus on what's towards the center of our image
h, w = self.image.shape[0], self.image.shape[1]
img = self.image[int(.25 * h):int(.75 * h),
int(.25 * w):int(.75 * w)]
variance = cv2.Laplacian(img, cv2.CV_64F).var()
variances.append(variance)
# print(os.path.join(experiment_folder_location,
# '{}___{}.tif'.format(self.image_title,now())))
# cv2.imwrite(os.path.join(experiment_folder_location,
# '{}___{}.tif'.format(self.image_title,now())),self.image)
unit_scaled_location_of_highest_variance = variances.index(
max(variances)) / len(variances)
print('location of highest variance: {}'.format(
unit_scaled_location_of_highest_variance))
closest_position = int(
np.rint(
len(positions) * unit_scaled_location_of_highest_variance)) - 1
print('closest_position', closest_position)
print('max variance of {} occurred at location {}'.format(
max(variances), positions[closest_position]))
# self.ch.setTargetPosition(positions[closest_position])
# self.position_and_variance_signal.emit((positions,variances))
while self.ch.getIsMoving() == True:
time.sleep(.1)
return max(variances), positions[closest_position], variances
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 __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()
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")
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 __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()
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
break
except KeyboardInterrupt:
print('Stopping.')
except RPLidarException as e:
print(e)
print("complete")
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
outfile.close()
print(lidar.motor)
try:
ch = Stepper()
except RuntimeError as e:
print("Runtime Exception %s" % e.details)
print("Press Enter to Exit...\n")
readin = sys.stdin.read(1)
exit(1)
try:
ch.setOnAttachHandler(StepperAttached)
ch.setOnDetachHandler(StepperDetached)
ch.setOnErrorHandler(ErrorEvent)
ch.setOnPositionChangeHandler(PositionChangeHandler)
# Please review the Phidget22 channel matching documentation for details on the device
# and class architecture of Phidget22, and how channels are matched to device features.
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)
global ch
try:
ch.close()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Press Enter to Exit...\n")
exit(1)
print("Closed Stepper device")
if __name__ == '__main__':
try:
rospy.init_node('stepper_motor', anonymous=True)
# Initialize stepper motors
try:
ch = Stepper()
except RuntimeError as e:
print("Runtime Exception %s" % e.details)
exit(1)
try:
ch.setOnAttachHandler(StepperAttached)
ch.setOnDetachHandler(StepperDetached)
ch.setOnErrorHandler(ErrorEvent)
ch.setOnPositionChangeHandler(PositionChangeHandler)
print("Waiting for the Phidget Stepper Object to be attached...")
SERIAL_ID = rospy.get_param('~serialId')
ch.setDeviceSerialNumber(SERIAL_ID)
ch.openWaitForAttachment(5000)
except PhidgetException as e:
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()
import sys
import time
from Phidget22.Devices.Stepper import *
from Phidget22.PhidgetException import *
from Phidget22.Phidget import *
from Phidget22.Net import *
try:
ch = Stepper()
except RuntimeError as e:
print("Runtime Exception %s" % e.details)
print("Press Enter to Exit...\n")
readin = sys.stdin.read(1)
exit(1)
def StepperAttached(e):
try:
attached = e
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")
fstepper_velocity = 2000
zstepper_velocity = 2000
fstepper_accelleration = 1000
zstepper_accelleration = 1000
output_channels = [
'output_filterDARK', 'output_filterGFP', 'output_filterRFP',
'output_filterCFP', 'output_filterYFP', 'output_camera',
'output_incubator', 'output_humidity'
]
input_channels = ['input_zLimitSwitch', 'input_fLimitSwitch']
########################## Initialize InterfaceKit
f_stepper = Stepper()
z_stepper = Stepper()
interfaceKit_output = [DigitalOutput() for i in range(0, len(output_channels))]
interfaceKit_input = [DigitalInput() for i in range(0, len(input_channels))]
########################## USER-DEFINED PARAMETERS
#MODIFY TO DEFINE SERIAL NUMBERS!!
z_stepper_serial = '506023'
f_stepper_serial = '522559'
interfaceKit_serial = '313877'
arduino_usbport = '/dev/ttyUSB0'
#DEFAULT VALUES
fpos_filterBRIGHT = -3250
def init_arm():
global stepper_r
stepper_r = Stepper()
stepper_r.setHubPort(0)
global stepper_p
stepper_p = Stepper()
stepper_p.setHubPort(1)
global limit_switch
limit_switch = DigitalInput()
limit_switch.setIsHubPortDevice(True)
limit_switch.setHubPort(2)
stepper_r.openWaitForAttachment(5000)
stepper_p.openWaitForAttachment(5000)
limit_switch.openWaitForAttachment(5000)
stepper_r.setCurrentLimit(2.0)
stepper_r.setRescaleFactor((1.0 / 16.0) * (STEP_ANGLE / R_JOINT_RATIO))
stepper_p.setRescaleFactor((1.0 / 16.0) * (STEP_ANGLE / DEG_PER_IN_P))
stepper_r.setAcceleration(ACC)
stepper_p.setAcceleration(ACC)
stepper_r.setVelocityLimit(MAX_VEL)
stepper_p.setVelocityLimit(MAX_VEL)
stepper_r.setEngaged(True)
stepper_p.setEngaged(True)
# Drive P joint until limit switch is hit to find 0 coordinate
calib_pos = 0
offset_pos = None
while calib_pos < 2 * JOINT_LENGTH_P and offset_pos is None:
calib_pos += 0.25
stepper_p.setTargetPosition(-calib_pos)
while stepper_p.getIsMoving():
if limit_switch.getState():
offset_pos = stepper_p.getPosition()
break
print(offset_pos)
stepper_p.setTargetPosition(offset_pos + DIST_SWITCH_TO_CENTER)
while stepper_p.getIsMoving():
print('target: ' + str(offset_pos + DIST_SWITCH_TO_CENTER))
print('current: ' + str(stepper_p.getPosition()))
print("centering")
stepper_p.addPositionOffset(-(offset_pos + DIST_SWITCH_TO_CENTER))