pshell_config/mxsc
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Browse Source
This commit is contained in:
gac-S_Changer
2018-10-16 18:05:43 +02:00
parent cfc4b3a0da
commit 3d86c27140
19 changed files with 333 additions and 111 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
script/calibration/ToolCalibration2.py
View File

@@ -54,7 +54,7 @@ off_x = x1 - x2
#For composing cannot use tcp_p, need another auxiliary point. tcp_t is also destroyed.
robot.set_pnt(robot.get_cartesian_pos(), "pTemp")
robot.set_trsf([off_x, 0,0,0,0,0])
c=robot.compose("pTemp", "fTable", "tcp_t" )
c=robot.compose("pTemp", FRAME_TABLE, "tcp_t" )
robot.set_pnt(c, "pTemp")
robot.movel("pTemp", cal_tool, DESC_SCAN, sync=True)

50
script/calibration/ToolCalibration3.py
View File

@@ -4,6 +4,7 @@ from mathutils import fit_gaussian, Gaussian
cal_tool = TOOL_CALIBRATION
robot.set_tool(cal_tool)
robot.enable()
move_to_laser()
@@ -13,13 +14,16 @@ robot.set_joint_motors_enabled(True)
initial_pos = robot.get_cartesian_pos()
#robot.align()
run("calibration/ScanX")
try:
robot.set_frame(FRAME_TABLE)
run("calibration/ScanX")
finally:
robot.set_default_frame()
pos1 = robot.get_cartesian_pos()
x1, l1 = closest_x, closest_y
print "Closest 1: ", [x1, l1]
print "Scan 1 result: ", [x1, l1]
print "Position 1: ", pos1
@@ -30,31 +34,45 @@ else:
robot_j6.move(pj6 + 180.0)
run("calibration/ScanX")
try:
robot.set_frame(FRAME_TABLE)
run("calibration/ScanX")
finally:
robot.set_default_frame()
pos2 =robot.get_cartesian_pos()
x2, l2 = closest_x, closest_y
pos2 = robot.get_cartesian_pos()
print "Closest 2: ", [x2, l2]
print "Position 2: ", pos2
print "Scan 2 result: ", [x2, l2]
print "Position 2: ", pos1
off_l = l2 - l1
print "Offset l: ", off_l
#For composing cannot use tcp_p, need another auxiliary point. tcp_t is also destroyed.
robot.set_pnt(robot.get_cartesian_pos(), "pTemp")
robot.set_trsf([0, -off_l, 0, 0, 0, 0])
c=robot.compose("pTemp", FRAME_TABLE, "tcp_t" )
robot.set_pnt(c, "pTemp")
robot.movel("pTemp", cal_tool, DESC_SCAN, sync=True)
pos3 = robot.get_cartesian_pos()
print "Position 3: ", pos3
#Updates the tool
t=robot.get_tool_trsf(TOOL_DEFAULT)
print "Former tool: " + str(t)
xoff = (x1-x2)/2
yoff = (l2 - l1)/2
xoff = (pos3[0]-pos1[0])/2
yoff = (pos3[1]-pos1[1])/2
xrot = math.degrees(math.atan(yoff/t[2]))
yrot = math.degrees(math.atan(xoff/t[2]))
print "Former tool: " + str(t)
t[0]=xoff
t[1]=-yoff
print "Offset: ", [t[0], t[1]]
print "CALIBRATED tool: " + str(t)
#robot.set_tool_trsf(t, TOOL_DEFAULT)
print "Calibrated tool: " + str(t)
robot.set_tool_trsf(t, TOOL_DEFAULT)
robot.set_tool(TOOL_DEFAULT)
d = robot.get_distance_to_pnt("pLaser")

62
script/devices/RobotSC.py
View File

@@ -2,11 +2,15 @@ TOOL_CALIBRATION = "tCalib"
TOOL_SUNA= "tSuna"
TOOL_DEFAULT= TOOL_SUNA
FRAME_TABLE = "fTable"
DESC_FAST = "mFast"
DESC_SLOW = "mSlow"
DESC_SCAN = "mScan"
DESC_DEFAULT = DESC_FAST
AUX_SEGMENT = "X"
DEFAULT_ROBOT_POLLING = 500
TASK_WAIT_ROBOT_POLLING = 50
@@ -22,8 +26,8 @@ joint_forces = False
class RobotSC(RobotTCP):
def __init__(self, name, server, timeout = 1000, retries = 1):
RobotTCP.__init__(self, name, server, timeout, retries)
self.set_tasks(["getDewar", "putDewar", "putGonio", "getGonio", "robotRecover", "moveDewar", "moveCold", "movePark", "moveGonio","moveHeater", "moveScanner","moveHome"])
self.set_known_points(["pPark", "pGonioHome", "pDewarWait", "pGonioGet", "pScanHome", "pHeaterHome", "pHeater", "pHeaterBottom", "pScanStop","pHelium", "pHome", "pCold"])
self.set_tasks(["getDewar", "putDewar", "putGonio", "getGonio", "recover", "moveDewar", "moveCold", "movePark", "moveGonio","moveHeater", "moveScanner","moveHome", "moveAux"])
self.set_known_points(["pPark", "pGonio", "pDewar", "pGonioG", "pScan", "pHeater", "pHeat", "pHeatB", "pScanStop","pHelium", "pHome", "pCold", "pAux"])
self.setPolling(DEFAULT_ROBOT_POLLING)
def move_dewar(self):
@@ -53,12 +57,12 @@ class RobotSC(RobotTCP):
self.start_task('putDewar',segment, puck, sample, is_room_temp())
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
#self.assert_dewar()
self.assert_cold
self.assert_cold()
def put_gonio(self):
pin_offset = get_pin_offset()
pin_angle_offset = get_pin_angle_offset()
self.start_task('putGonio', pin_offset, pin_angle_offset)
self.start_task('putGonio', pin_offset)
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
self.assert_gonio()
@@ -68,6 +72,18 @@ class RobotSC(RobotTCP):
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
self.assert_gonio()
def get_aux(self, sample):
self.assert_aux()
self.start_task('getAuxiliary', sample)
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
self.assert_aux()
def put_aux(self, sample):
self.assert_aux()
self.start_task('putAuxiliary', sample)
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
self.assert_aux()
def move_scanner(self):
self.start_task('moveScanner')
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
@@ -104,10 +120,15 @@ class RobotSC(RobotTCP):
def robot_recover(self):
self.start_task('robotRecover')
self.start_task('recover')
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
self.assert_home()
def move_aux(self):
self.start_task('moveAux')
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
self.assert_aux()
def toSegmentNumber(self, segment):
if is_string(segment):
segment = ord(segment.upper()) - ord('A') +1
@@ -158,25 +179,28 @@ class RobotSC(RobotTCP):
return self.is_in_point("pHome")
def is_dewar(self):
return self.is_in_point("pDewarWait")
return self.is_in_point("pDewar")
def is_heater(self):
return self.is_in_point("pHeater")
return self.is_in_point("pHeat")
def is_heater_home(self):
return self.is_in_point("pHeaterHome")
return self.is_in_point("pHeater")
def is_heater_bottom(self):
return self.is_in_point("pHeaterBottom")
return self.is_in_point("pHeatB")
def is_gonio(self):
return self.is_in_point("pGonioHome")
return self.is_in_point("pGonio")
def is_helium(self):
return self.is_in_point("pHelium")
def is_scanner(self):
return self.is_in_point("pScanHome")
return self.is_in_point("pScan")
def is_aux(self):
return self.is_in_point("pAux")
#def is_scan_stop(self):
# return self.is_in_point("pScanStop")
@@ -186,16 +210,16 @@ class RobotSC(RobotTCP):
return self.get_current_point() is not None
def assert_heater_home(self):
self.assert_in_point("pHeaterHome")
self.assert_in_point("pHeater")
def assert_cold(self):
self.assert_in_point("pCold")
def assert_heater(self):
self.assert_in_point("pHeater")
self.assert_in_point("pHeat")
def assert_heater_bottom(self):
self.assert_in_point("pHeaterBottom")
self.assert_in_point("pHeatB")
def assert_park(self):
self.assert_in_point("pPark")
@@ -204,16 +228,19 @@ class RobotSC(RobotTCP):
self.assert_in_point("pHome")
def assert_dewar(self):
self.assert_in_point("pDewarWait")
self.assert_in_point("pDewar")
def assert_gonio(self):
self.assert_in_point("pGonioHome")
self.assert_in_point("pGonio")
def assert_helium(self):
self.assert_in_point("pHelium")
def assert_scanner(self):
self.assert_in_point("pScanHome")
self.assert_in_point("pScan")
def assert_aux(self):
self.assert_in_point("pAux")
#def assert_scan_stop(self):
# self.assert_in_point("pScanStop")
@@ -236,6 +263,7 @@ else:
robot.set_default_desc(DESC_DEFAULT)
robot.default_speed = 20
robot.set_frame(FRAME_DEFAULT)
robot.set_tool(TOOL_DEFAULT)
robot.setPolling(DEFAULT_ROBOT_POLLING)

30
script/devices/RobotTCP.py
View File

@@ -69,7 +69,20 @@ class RobotTCP(TcpDevice, Stoppable):
def get_tool(self):
return self.tool
def set_frame(self, frame):
self.frame = frame
self.evaluate("tcp_curframe=" + frame)
if self.cartesian_motors_enabled:
self.update()
self.set_motors_enabled(True)
self.waitCacheChange(5000)
def get_frame(self):
return self.frame
def set_default_frame(self):
self.set_frame(FRAME_DEFAULT)
def assert_tool(self, tool=None):
if tool is None:
if self.tool is None:
@@ -470,15 +483,18 @@ class RobotTCP(TcpDevice, Stoppable):
def distance_p(self, pnt1, pnt2):
return self.eval_float("distance(" + pnt1 + ", " + pnt2 + ")")
def compose(self, pnt, frame = FRAME_DEFAULT, trsf = "tcp_t"):
def compose(self, pnt, frame = None, trsf = "tcp_t"):
if frame is None: frame = self.frame
return self.eval_pnt("compose(" + pnt + ", " + frame + ", " + trsf + ")")
def here(self, tool=None, frame=FRAME_DEFAULT):
def here(self, tool=None, frame=None):
if tool is None: tool = self.tool
if frame is None: frame = self.frame
return self.eval_pnt("here(" + tool + ", " + frame + ")")
def joint_to_point(self, tool=None, frame=FRAME_DEFAULT, joint="tcp_j"):
def joint_to_point(self, tool=None, frame=None, joint="tcp_j"):
if tool is None: tool = self.tool
if frame is None: frame = self.frame
return self.eval_pnt("jointToPoint(" + tool + ", " + frame + ", " + joint +")")
def point_to_joint(self, tool=None, initial_joint="tcp_j", point="tcp_p"):
@@ -486,7 +502,8 @@ class RobotTCP(TcpDevice, Stoppable):
if self.eval_bool("pointToJoint(" + tool + ", " + initial_joint + ", " + point +", j)"):
return self.get_jnt()
def position(self, point, frame=FRAME_DEFAULT):
def position(self, point, frame=None):
if frame is None: frame = self.frame
return self.eval_trf("position(" + point + ", " + frame + ")")
#Profile
@@ -715,14 +732,15 @@ class RobotTCP(TcpDevice, Stoppable):
for m in self.joint_motors:
m.initialize()
#Alignment
def align(self, point = None, frame = FRAME_DEFAULT, desc = None):
def align(self, point = None, frame = None, desc = None):
if frame is None: frame = self.frame
self.assert_tool()
if desc is None: desc = self.default_desc
if point is None:
self.get_cartesian_pos()
else:
self.set_pnt(point)
np=self.eval_trf('align(tcp_p.trsf, ' + FRAME_DEFAULT + '.trsf)')
np=self.eval_trf('align(tcp_p.trsf, ' + frame + '.trsf)')
self.set_pnt(np)
self.movej("tcp_p", self.tool, desc)
#TODO: The first command is not executed (but receive a move id)

6
script/local.py
View File

@@ -93,6 +93,9 @@ run("motion/move_home")
run("motion/move_park")
run("motion/move_cold")
run("motion/move_scanner")
run("motion/move_aux")
run("motion/get_aux")
run("motion/put_aux")
run("motion/dry")
run("motion/homing_hexiposi")
run("motion/scan_pin")
@@ -123,6 +126,8 @@ def get_puck_img_detection(segment, puck):
return str(Controller.getInstance().getPuck(str(segment).upper() + str(puck)).imageDetection)
def assert_puck_detected(segment, puck):
if (segment == AUX_SEGMENT) and (puck == 1):
return
if get_puck_elect_detection(segment, puck) != "Present":
raise Exception ("Puck " + str(segment).upper() + str(puck) + " not present")
@@ -163,6 +168,7 @@ except:
try:
robot.setPolling(DEFAULT_ROBOT_POLLING)
robot.set_tool(TOOL_DEFAULT)
robot.set_frame(FRAME_DEFAULT)
robot.set_motors_enabled(True)
robot.set_joint_motors_enabled(True)
except:

21
script/motion/get_aux.py Normal file
View File

@@ -0,0 +1,21 @@
def get_aux(sample):
"""
"""
print "get_aux: ",sample
#Initial checks
assert_valid_sample(sample)
robot.assert_no_task()
robot.reset_motion()
robot.wait_ready()
robot.assert_cleared()
hexiposi.assert_homed()
#Enabling
enable_motion()
if not robot.is_aux():
robot.move_aux()
robot.get_aux(sample)

55
script/motion/mount.py
View File

@@ -9,6 +9,7 @@ def mount(segment, puck, sample, force=False, read_dm=False):
start = time.time()
is_aux = segment == AUX_SEGMENT
puck_address = get_puck_address(puck)
if puck_address is None:
puck_obj = get_puck_obj_by_id(puck)
@@ -39,16 +40,24 @@ def mount(segment, puck, sample, force=False, read_dm=False):
#Enabling
enable_motion()
set_hexiposi(segment)
if not force:
visual_check_hexiposi(segment)
if not robot.is_dewar():
robot.move_dewar()
if is_aux:
if not robot.is_aux():
robot.move_aux()
robot.get_dewar(segment, puck, sample)
robot.get_aux(sample)
robot.move_home()
else:
set_hexiposi(segment)
if not force:
visual_check_hexiposi(segment)
if not robot.is_dewar():
robot.move_dewar()
robot.get_dewar(segment, puck, sample)
set_setting("mounted_sample_position", get_sample_name(segment, puck, sample))
@@ -77,18 +86,22 @@ def mount(segment, puck, sample, force=False, read_dm=False):
dry_mount_count = 0
set_setting("dry_mount_counter", dry_mount_count+1)
#TODO: Auto-dry procedure
robot.move_cold()
mount_sample_detected = smart_magnet.check_mounted(idle_time=0.25, timeout = 1.0)
#TODO: Should do on finally?
update_samples_info_sample_mount(get_puck_name(segment, puck), sample, mount_sample_detected, mount_sample_id)
if mount_sample_detected == False:
raise Exception("No pin detected on gonio")
if is_force_dry():
print "Auto dry"
log("Starting auto dry", False)
dry()
if is_aux:
robot.move_home()
else:
robot.move_cold()
mount_sample_detected = smart_magnet.check_mounted(idle_time=0.25, timeout = 1.0)
#TODO: Should do on finally?
update_samples_info_sample_mount(get_puck_name(segment, puck), sample, mount_sample_detected, mount_sample_id)
if mount_sample_detected == False:
raise Exception("No pin detected on gonio")
if is_force_dry():
print "Auto dry"
log("Starting auto dry", False)
dry()
return [mount_sample_detected, mount_sample_id]
finally:
smart_magnet.set_default_current()

18
script/motion/move_aux.py Normal file
View File

@@ -0,0 +1,18 @@
def move_aux():
"""
"""
print "move_aux"
#Initial checks
robot.assert_no_task()
robot.reset_motion()
robot.wait_ready()
robot.assert_cleared()
#robot.assert_in_known_point()
#Enabling
enable_motion()
if not robot.is_aux():
robot.move_aux()

21
script/motion/put_aux.py Normal file
View File

@@ -0,0 +1,21 @@
def put_aux(sample):
"""
"""
print "put_aux: ",sample
#Initial checks
assert_valid_sample(sample)
robot.assert_no_task()
robot.reset_motion()
robot.wait_ready()
robot.assert_cleared()
hexiposi.assert_homed()
#Enabling
enable_motion()
if not robot.is_aux():
robot.move_aux()
robot.put_aux(sample)

15
script/motion/recover.py
View File

@@ -6,14 +6,15 @@ RECOVER_DESC = "mRecovery"
RECOVER_TOOL = TOOL_DEFAULT
known_segments = [ ("pHome", "pPark", 50), \
("pHome", "pGonioHome", 30), \
("pHome", "pScanHome", 25), \
("pHome", "pHeaterHome", 75), \
("pHome", "pDewarWait", 10), \
("pHome", "pGonioH", 30), \
("pHome", "pScan", 25), \
("pHome", "pHeater", 75), \
("pHome", "pDewar", 10), \
("pHome", "pHelium", 230), \
("pGonioHome", "pGonioGet", 10), \
("pPark", "pHeater", 40), \
("pHeaterHome", "pHeaterBottom", 10), \
("pGonioH", "pGonioG", 10), \
("pPark", "pHeat", 40), \
("pHeater", "pHeatB", 10), \
("pPark", "pAux", 30), \
]
def get_dist_to_line(segment):

9
script/motion/scan_pin.py
View File

@@ -31,8 +31,13 @@ def scan_pin(segment, puck, sample, force=False):
robot.move_dewar()
robot.put_dewar(segment, puck, sample)
return (detected, dm)
ret = "Empty"
if detected:
if (dm is None) or (len(dm.strip())==0):
ret = "Present"
else:
ret = str(dm)
return ret
def scan_puck(segment, puck, force=False):

6
script/motion/tools.py
View File

@@ -129,6 +129,8 @@ def update_tool(tool=None, x_offset=0.0, y_offset=0.0, z_offset=0.0):
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:
@@ -137,6 +139,10 @@ def assert_valid_address(segment, puck, sample):
raise Exception ("Invalid puck")
if sample<=0 or sample >16:
raise Exception ("Invalid sample")
def assert_valid_sample(sample):
if sample<=0 or sample >16:
raise Exception ("Invalid sample")
def get_puck_name(segment, puck):
try:

25
script/motion/unmount.py
View File

@@ -9,6 +9,7 @@ def unmount(segment = None, puck = None, sample = None, force=False):
raise Exception("Mounted sample position is not defined")
segment, puck , sample = pos[0:1], int(pos[1]), int(pos[2])
print "Mounted sample position: ", segment, puck , sample
is_aux = segment == AUX_SEGMENT
#Initial checks
assert_valid_address(segment, puck, sample)
@@ -31,11 +32,12 @@ def unmount(segment = None, puck = None, sample = None, force=False):
#Enabling
enable_motion()
if not is_aux:
set_hexiposi(segment)
set_hexiposi(segment)
if not force:
visual_check_hexiposi(segment)
if not force:
visual_check_hexiposi(segment)
#location = robot.get_current_point()
@@ -45,10 +47,17 @@ def unmount(segment = None, puck = None, sample = None, force=False):
smart_magnet.set_unmount_current()
robot.get_gonio()
#TODO: Shuld check if smart magnet detection is off?
update_samples_info_sample_unmount(get_puck_name(segment, puck), sample)
robot.move_dewar()
robot.put_dewar(segment, puck, sample)
if is_aux:
robot.move_dewar()
robot.move_aux()
robot.put_aux( sample)
robot.move_home()
else:
#TODO: Shuld check if smart magnet detection is off?
update_samples_info_sample_unmount(get_puck_name(segment, puck), sample)
robot.move_dewar()
robot.put_dewar(segment, puck, sample)
set_setting("mounted_sample_position", None)
finally:
smart_magnet.set_default_current()