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))
def sort(pos=5000):
try:
"""
* Allocate a new Phidget Channel object
"""
ch = Stepper()
"""
* Set matching parameters to specify which channel to open
"""
#You may remove this line and hard-code the addressing parameters to fit your application
channelInfo = AskForDeviceParameters(ch)
ch.setDeviceSerialNumber(channelInfo.deviceSerialNumber)
ch.setHubPort(channelInfo.hubPort)
ch.setIsHubPortDevice(channelInfo.isHubPortDevice)
ch.setChannel(channelInfo.channel)
if (channelInfo.netInfo.isRemote):
ch.setIsRemote(channelInfo.netInfo.isRemote)
if (channelInfo.netInfo.serverDiscovery):
try:
Net.enableServerDiscovery(
PhidgetServerType.PHIDGETSERVER_DEVICEREMOTE)
except PhidgetException as e:
PrintEnableServerDiscoveryErrorMessage(e)
raise EndProgramSignal(
"Program Terminated: EnableServerDiscovery Failed")
else:
Net.addServer("Server", channelInfo.netInfo.hostname,
channelInfo.netInfo.port,
channelInfo.netInfo.password, 0)
"""
* Add event handlers before calling open so that no events are missed.
"""
print("\n--------------------------------------")
#print("\nSetting OnAttachHandler...")
ch.setOnAttachHandler(onAttachHandler)
#print("Setting OnDetachHandler...")
ch.setOnDetachHandler(onDetachHandler)
#print("Setting OnErrorHandler...")
ch.setOnErrorHandler(onErrorHandler)
#print("\nSetting OnPositionChangeHandler...")
ch.setOnPositionChangeHandler(onPositionChangeHandler)
"""
* Open the channel with a timeout
"""
#print("\nOpening and Waiting for Attachment...")
try:
ch.openWaitForAttachment(5000)
except PhidgetException as e:
PrintOpenErrorMessage(e, ch)
raise EndProgramSignal("Program Terminated: Open Failed")
"""
* Sorting process, set position for the stepper to move
"""
end = False
buf = pos
ch.setVelocityLimit(50000)
ch.setAcceleration(200000)
while (end != True):
if (buf == 0):
end = True
break
targetPosition = buf
if (targetPosition > ch.getMaxPosition()
or targetPosition < ch.getMinPosition()):
print("TargetPosition must be between %.2f and %.2f\n" %
(ch.getMinPosition(), ch.getMaxPosition()))
continue
#print("Setting Stepper TargetPosition to " + str(targetPosition))
ch.setTargetPosition(targetPosition)
#print("Position is " + str(ch.getPosition()))
if (targetPosition == ch.getPosition()):
buf = 0
'''
* Perform clean up and exit
'''
print("Cleaning up...")
ch.close()
return 0
except PhidgetException as e:
sys.stderr.write("\nExiting with error(s)...")
DisplayError(e)
#traceback.print_exc()
print("Cleaning up...")
ch.close()
return 1
except EndProgramSignal as e:
print(e)
print("Cleaning up...")
ch.close()
return 1
except RuntimeError as e:
sys.stderr.write("Runtime Error: \n\t" + e)
traceback.print_exc()
return 1
finally:
print(print("\nExiting..."))
#sort(-20000)
#sort(150000)
class SteeringController():
def __init__(self):
# Set control mode
# 0 = step mode (target position)
# 1 = run mode (target velocity)
self.control_mode = 1
self.velocity = 0 # rad/s, starting velocity
self.angle = 0 # 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 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 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()))
