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-08-29 10:19:38 +02:00
parent 1af729d724
commit 1229efe275
12 changed files with 233 additions and 132 deletions
Download Patch File Download Diff File Expand all files Collapse all files
script/devices/RobotSC.py
+12 -17
View File
@@ -22,8 +22,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", "movePark", "moveGonio","moveHeater", "moveScanner"])
self.set_known_points(["pHome", "pPark", "pDewarHome", "pGonioHome", "pDewarWait", "pGonioGet", "pScanHome", "pHeaterHome", "pHeater", "pHeaterBottom", "pScanStop","pHelium"])
self.set_tasks(["getDewar", "putDewar", "putGonio", "getGonio", "robotRecover", "moveDewar", "movePark", "moveGonio","moveHeater", "moveScanner","moveHome"])
self.set_known_points(["pPark", "pHome","pDewarHome", "pGonioHome", "pDewarWait", "pGonioGet", "pScanHome", "pHeaterHome", "pHeater", "pHeaterBottom", "pScanStop","pHelium"])
self.setPolling(DEFAULT_ROBOT_POLLING)
def move_dewar(self):
@@ -31,6 +31,11 @@ class RobotSC(RobotTCP):
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
self.assert_dewar()
def move_home(self):
self.start_task('moveHome')
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
self.assert_home()
def get_dewar(self, segment, puck, sample):
segment = self.toSegmentNumber(segment)
self.start_task('getDewar',segment, puck, sample, is_room_temp())
@@ -73,11 +78,7 @@ class RobotSC(RobotTCP):
self.start_task('moveGonio')
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
self.assert_gonio()
def move_home(self):
self.start_task('moveHome')
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
self.assert_home()
def move_park(self):
self.start_task('movePark')
@@ -136,15 +137,12 @@ class RobotSC(RobotTCP):
robot.set_profile("remote")
def set_local(self):
robot.set_profile("default")
def is_home(self):
return self.is_in_point("pHome")
robot.set_profile("default")
def is_park(self):
return self.is_in_point("pPark")
def is_dewar_home(self):
def is_home(self):
return self.is_in_point("pDewarHome")
def is_dewar(self):
@@ -173,10 +171,7 @@ class RobotSC(RobotTCP):
def is_cleared(self):
#return self.is_home() or self.is_park() or self.is_dewar() or self.is_dewar_home()
return self.get_current_point() is not None
def assert_home(self):
self.assert_in_point("pHome")
return self.get_current_point() is not None
def assert_heater_home(self):
self.assert_in_point("pHeaterHome")
@@ -190,7 +185,7 @@ class RobotSC(RobotTCP):
def assert_park(self):
self.assert_in_point("pPark")
def assert_dewar_home(self):
def assert_home(self):
self.assert_in_point("pDewarHome")
def assert_dewar(self):
script/devices/RobotTCP.py
+2 -2
View File
@@ -45,7 +45,7 @@ class RobotTCP(TcpDevice, Stoppable):
self.frame = FRAME_DEFAULT
self.polling_interval = 0.01
self.reset = True
self.default_tolerance = 10
self.default_tolerance = 5
self.default_speed = 100
self.task_start_retries = 3
@@ -377,7 +377,7 @@ class RobotTCP(TcpDevice, Stoppable):
if desc is None: desc = self.default_desc
if tool is None: tool = self.tool
#If joint_or_point is a list assumes ir is a point
if not is_string(point):
if not is_string(joint_or_point):
robot.set_pnt(joint_or_point , "tcp_p")
joint_or_point = "tcp_p"
ret = self.eval_int("movej(" + joint_or_point + ", " + tool + ", " + desc +")")
script/devices/Wago.py
+48 -1
View File
@@ -93,7 +93,6 @@ def is_led_room_temp():
###################################################################################################
# Safety release
###################################################################################################
@@ -120,3 +119,51 @@ def release_psys():
time.sleep(0.01)
release_psys_safety.write(False)
###################################################################################################
# Drier
###################################################################################################
MAX_HEATER_TIME = 15000
def set_air_stream(state):
"""
"""
valve_1.write(state)
valve_2.write(not state)
set_heater_chrono = None
def monitor_heater_time():
while get_heater():
if set_heater_chrono.isTimeout(MAX_HEATER_TIME):
set_heater(False)
log("Heater timeout expired: turned off", False)
return
time.sleep(0.1)
def set_heater(state):
"""
"""
global set_heater_chrono
if get_heater() != state:
gripper_dryer.write(state)
if state:
set_heater_chrono = Chrono()
fork(monitor_heater_time)
def get_air_stream():
"""
"""
return valve_1.read()
def get_heater():
"""
"""
return gripper_dryer.read()
script/local.py
+1
View File
@@ -6,6 +6,7 @@ from ch.psi.pshell.serial import TcpDevice
from ch.psi.pshell.modbus import ModbusTCP
import ch.psi.mxsc.Controller as Controller
import ch.psi.pshell.core.Nameable as Nameable
import ch.psi.utils.Chrono as Chrono
run("setup/Layout")
script/motion/dry.py
-25
View File
@@ -1,29 +1,4 @@
def set_air_stream(state):
"""
"""
valve_1.write(state)
valve_2.write(not state)
def set_heater(state):
"""
"""
gripper_dryer.write(state)
def get_air_stream():
"""
"""
return valve_1.read()
def get_heater():
"""
"""
return gripper_dryer.read()
def dry(heat_time, speed):
"""
script/motion/recover.py
+73 -33
View File
@@ -2,20 +2,21 @@ import org.apache.commons.math3.geometry.euclidean.threed.Segment as Segment
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D as Vector3D
import org.apache.commons.math3.geometry.euclidean.threed.Line as Line3D
RECOVER_DESC = "mRecovery"
RECOVER_TOOL = TOOL_DEFAULT
known_segments = [ ("pHome", "pPark", 10), \
("pHome", "pDewarHome", 10), \
("pHome", "pGonioHome", 10), \
("pHome", "pScanHome", 10), \
("pHome", "pHeaterHome", 10), \
known_segments = [ ("pHome", "pPark", 50), \
("pHome", "pDewarHome", 30), \
("pHome", "pGonioHome", 30), \
("pHome", "pScanHome", 25), \
("pHome", "pHeaterHome", 25), \
("pDewarHome", "pDewarWait", 10), \
("pGonioHome", "pGonioGet", 10), \
("pHeaterHome", "pHeater", 10), \
("pHeater", "pHeaterBottom", 10), \
]
def is_on_segment(segment):
def get_dist_to_line(segment):
tolerance = segment[2]
p1, p2 = robot.get_pnt(segment[0]), robot.get_pnt(segment[1])
p = robot.get_cartesian_pos()
@@ -23,57 +24,93 @@ def is_on_segment(segment):
v1 = Vector3D(p1[0], p1[1], p1[2])
v2 = Vector3D(p2[0], p2[1], p2[2])
l = Line3D(v1, v2, 0.01)
return l.distance(v)
def get_dist_to_segment(segment):
tolerance = segment[2]
p1, p2 = robot.get_pnt(segment[0]), robot.get_pnt(segment[1])
p = robot.get_cartesian_pos()
v = Vector3D(p[0], p[1], p[2])
v1 = Vector3D(p1[0], p1[1], p1[2])
v2 = Vector3D(p2[0], p2[1], p2[2])
l = Line3D(v1, v2, 0.01)
d = l.distance(v)
pj = get_pojection_at_line(segment)
d1, d2 = v1.distance(v), v2.distance(v)
dp1, dp2 = v1.distance(pj), v2.distance(pj)
d12 = v1.distance(v2)
if (dp1 + dp2) > (d12 + tolerance):
d = max(d,min(d1,d2))
return d
def is_on_segment(segment):
tolerance = segment[2]
d = get_dist_to_segment(segment)
#Check if on segment
d = l.distance(v)
if d > tolerance:
#print "Current robot position " + str(p) + " not on segment " + str(segment) + " - distance=" + str(d)
return False
# Check if inside segment
d1, d2 = v1.distance(v), v2.distance(v)
if d1>d2:
d1, d2 = d2, d1
d1, d2 = d1, d2
if d<(d1-tolerance) or d>(d2+tolerance):
#print "Current robot position " + str(p) + " not on segment " + str((d1, d2)) + " of segment " + str(segment) + " - distance=" + str(d)
return False
#print "Current robot position " + str(p) + " on segment " + str(segment) + " - distance=" + str(d)
return True
def get_pojection_at_line(segment):
tolerance = segment[2]
p1, p2 = robot.get_pnt(segment[0]), robot.get_pnt(segment[1])
p = robot.get_cartesian_pos()
v = Vector3D(p[0], p[1], p[2])
v1 = Vector3D(p1[0], p1[1], p1[2])
v2 = Vector3D(p2[0], p2[1], p2[2])
l = Line3D(v1, v2, 0.01)
a = l.getAbscissa(v)
lv = l.pointAt(a)
return lv
def get_current_segment():
for segment in known_segments:
if is_on_segment(segment):
return segment
return None
def get_current_distance():
for segment in known_segments:
if is_on_segment(segment):
return get_dist_to_segment(segment)
return None
def move_to_segment(segment):
tolerance = segment[2]
p1, p2 = robot.get_pnt(segment[0]), robot.get_pnt(segment[1])
p = robot.get_cartesian_pos()
v = Vector3D(p[0], p[1], p[2])
v1 = Vector3D(p1[0], p1[1], p1[2])
v2 = Vector3D(p2[0], p2[1], p2[2])
l = Line3D(v1, v2, 0.01)
a = l.getAbscissa(v)
lv = l.pointAt(a)
lv = get_pojection_at_line(segment)
dlv = lv.distance(v)
if dlv> (tolerance + 0.1):
raise Exception( "Error moving from " + str(p) + " to segment - distance=" + str(dlv))
d = [lv.x, lv.y, lv.z, p[3], p[4], p[5]]
d = [lv.x, lv.y, lv.z, p[3], p[4], p[5]]
print "Moving from " + str(p) + " to segment " + str(segment) + " - distance=" + str(dlv) + " - dest=" + str(d)
robot.movel(d, tool=None, desc=DESC_SLOW, sync=True)
try:
robot.movel(d, tool=RECOVER_TOOL, desc=RECOVER_DESC, sync=True)
print "Done"
except:
print sys.exc_info()[1]
#Moves to first point of the segment ehich is safer, unless in the vicinity of the second
def move_to_safest_point(segment, vicinity_tolerance = 100):
d1, d2 = robot.get_distance_to_pnt(segment[0]), robot.get_distance_to_pnt(segment[1])
if (d2<=d1) and (d2 <= vicinity_tolerance):
robot.movel(segment[1], tool=None, desc=DESC_SLOW, sync=True)
print "Moving to secondary point " + str(segment[1] + " - d1=" + str(d1) + " d2=" + str(d2) )
robot.movel(segment[1], tool=RECOVER_TOOL, desc=RECOVER_DESC, sync=True)
else:
robot.movel(segment[0], tool=None, desc=DESC_SLOW, sync=True)
#print "Recovered to point " + str(robot.get_current_point())
print "Moving to primary point " + str(segment[0] + " - d1=" + str(d1) + " d2=" + str(d2) )
robot.movel(segment[0], tool=RECOVER_TOOL, desc=RECOVER_DESC, sync=True)
print "Done"
#print "Recovered to point " + str(robot.get_curjoint_or_pointrent_point())
def recover():
@@ -97,7 +134,10 @@ def recover():
is_on_known_segment = True
move_to_segment(segment)
move_to_safest_point(segment)
return "Success recovering to point: " + str(robot.get_current_point())
location = robot.get_current_point()
if location is None:
raise Exception("Robot didn't reach known point")
return "Success recovering to point: " + str(location)
#finally:
# robot.set_default_speed()
if not is_on_known_segment: