143 lines
4.5 KiB
Python
143 lines
4.5 KiB
Python
POSITION_TOLERANCE = 50
|
|
|
|
|
|
def is_manual_mode():
|
|
"""
|
|
return if operating in local mode. TODO: should be based on IO, not on robot flag.
|
|
"""
|
|
return robot.working_mode == "manual"
|
|
|
|
def release_safety():
|
|
"""
|
|
Release sefety signals acording to working mode
|
|
"""
|
|
if air_pressure_ok.take() != True:
|
|
raise Exception("Cannot release safety: air preassure not ok")
|
|
if n2_pressure_ok.take() != True:
|
|
raise Exception("Cannot release safety: n2 pressure not ok")
|
|
auto = not is_manual_mode()
|
|
if auto:
|
|
if feedback_psys_safety.read() == False:
|
|
release_psys()
|
|
time.sleep(0.5)
|
|
if feedback_psys_safety.read() == False:
|
|
raise Exception("Cannot enable power: check doors")
|
|
|
|
if feedback_local_safety.read() == False:
|
|
release_local()
|
|
time.sleep(0.5)
|
|
if feedback_local_safety.read() == False:
|
|
raise Exception("Cannot enable power: check sample changer emergency stop button")
|
|
|
|
def enable_motion():
|
|
"""
|
|
Check safety and enable arm power if in remote mode
|
|
"""
|
|
release_safety()
|
|
system_check(robot_move=True)
|
|
auto = not is_manual_mode()
|
|
if auto:
|
|
if not robot.state.isNormal():
|
|
raise Exception("Cannot enable power: robot state is " + str(robot.state))
|
|
robot.enable()
|
|
|
|
|
|
def wait_end_of_move():
|
|
robot.update()
|
|
while (not robot.settled) or (not robot.empty) or (not robot.isReady()) :
|
|
time.sleep(0.01)
|
|
|
|
|
|
def move_to_home():
|
|
#robot.reset_motion("jHome")
|
|
robot.movej("pHome", robot.tool , DESC_SCAN)
|
|
wait_end_of_move()
|
|
|
|
def move_to_laser(from_point = "pHome"):
|
|
robot.reset_motion()
|
|
tool = robot.tool
|
|
d = robot.get_distance_to_pnt("pLaser")
|
|
if d<0:
|
|
raise Exception ("Error calculating distance to laser: " + str(d))
|
|
if d<POSITION_TOLERANCE:
|
|
print "FROM LASER"
|
|
robot.movel("pLaser", robot.tool, DESC_SCAN, sync = True)
|
|
return
|
|
d = robot.get_distance_to_pnt("pLaserHome")
|
|
if d<0:
|
|
raise Exception ("Error calculating distance to laser appro: " + str(d))
|
|
if d<POSITION_TOLERANCE:
|
|
print "FROM APPRO"
|
|
robot.movel("pLaser", robot.tool, DESC_SCAN, sync = True)
|
|
return
|
|
|
|
d = robot.get_distance_to_pnt(from_point)
|
|
if d<0:
|
|
raise Exception ("Error calculating distance to " + from_point + ": " + str(d))
|
|
if d<POSITION_TOLERANCE:
|
|
print "FROM " + from_point
|
|
robot.movej("pLaserHome", robot.tool, DESC_DEFAULT)
|
|
robot.movel("pLaser", robot.tool, DESC_SCAN, sync = True)
|
|
return
|
|
raise Exception ("Must be in home position to start move to laser")
|
|
|
|
|
|
def update_tool(tool=None, x_offset=0.0, y_offset=0.0, z_offset=0.0):
|
|
#Updating tool:
|
|
t=robot.get_tool_trsf(tool)
|
|
t[0]=t[0] - x_offset
|
|
t[1]=t[1] - y_offset
|
|
t[2]=t[2] - z_offset
|
|
robot.set_tool_trsf(t, tool)
|
|
print "Updated " + (str(robot.tool) if (tool is None) else tool) + ": " + str(t)
|
|
robot.save_program()
|
|
|
|
|
|
def assert_valid_address(segment, puck, sample):
|
|
if (segment == AUX_SEGMENT) and (puck == 1):
|
|
return
|
|
if is_string(segment):
|
|
segment = ord(segment.upper()) - ord('A') +1
|
|
if (segment<=0) or (segment >6):
|
|
raise Exception ("Invalid segment: " + str(segment))
|
|
if (puck<=0) or (puck >5):
|
|
raise Exception ("Invalid puck: " + str(puck))
|
|
if (sample<=0) or (sample >16):
|
|
raise Exception ("Invalid sample: " + str(sample))
|
|
if get_puck_dev(segment, puck).isDisabled():
|
|
raise Exception ("Puck is disabled")
|
|
|
|
def assert_valid_sample(sample):
|
|
if (sample<=0) or (sample >16):
|
|
raise Exception ("Invalid sample: " + str(sample))
|
|
|
|
def get_puck_name(segment, puck):
|
|
try:
|
|
assert_valid_address(segment, puck, 1)
|
|
if type(segment) is int:
|
|
segment = chr( ord('A') + (segment-1))
|
|
elif is_string(segment):
|
|
segment = segment.upper()
|
|
else:
|
|
return None
|
|
return segment + str(puck)
|
|
except:
|
|
return None
|
|
|
|
def get_sample_name(segment, puck, sample):
|
|
puck_name = get_puck_name(segment, puck)
|
|
return None if (puck_name is None) else puck_name + str(sample)
|
|
|
|
|
|
def is_pin_detected_in_scanner():
|
|
samples = []
|
|
for i in range(10):
|
|
samples.append(laser_distance.read())
|
|
time.sleep(0.05)
|
|
av = mean(samples)
|
|
for s in samples:
|
|
if s<=1:
|
|
return False
|
|
if abs(s-av) > 0.1:
|
|
return False
|
|
return True |