diff --git a/devices/led_ctrl_1.properties b/devices/led_ctrl_1.properties
index f03f306..e363697 100644
--- a/devices/led_ctrl_1.properties
+++ b/devices/led_ctrl_1.properties
@@ -1,5 +1,5 @@
-#Fri Aug 24 10:56:15 CEST 2018
-maxValue=1.0
+#Fri Aug 24 20:52:17 CEST 2018
+maxValue=0.4
minValue=0.0
offset=0.0
precision=2
diff --git a/devices/led_ctrl_2.properties b/devices/led_ctrl_2.properties
index f03f306..e363697 100644
--- a/devices/led_ctrl_2.properties
+++ b/devices/led_ctrl_2.properties
@@ -1,5 +1,5 @@
-#Fri Aug 24 10:56:15 CEST 2018
-maxValue=1.0
+#Fri Aug 24 20:52:17 CEST 2018
+maxValue=0.4
minValue=0.0
offset=0.0
precision=2
diff --git a/devices/led_ctrl_3.properties b/devices/led_ctrl_3.properties
index f03f306..e363697 100644
--- a/devices/led_ctrl_3.properties
+++ b/devices/led_ctrl_3.properties
@@ -1,5 +1,5 @@
-#Fri Aug 24 10:56:15 CEST 2018
-maxValue=1.0
+#Fri Aug 24 20:52:17 CEST 2018
+maxValue=0.4
minValue=0.0
offset=0.0
precision=2
diff --git a/plugins/MXSC-1.10.0.jar b/plugins/MXSC-1.10.0.jar
index 725e824..f46cf9b 100644
Binary files a/plugins/MXSC-1.10.0.jar and b/plugins/MXSC-1.10.0.jar differ
diff --git a/plugins/Recovery.form b/plugins/Recovery.form
index 15e9571..e892c04 100644
--- a/plugins/Recovery.form
+++ b/plugins/Recovery.form
@@ -17,37 +17,40 @@
+
-
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
-
-
+
+
+
+
+
+
+
+
+
@@ -66,11 +69,16 @@
-
+
+
+
+
+
+
-
+
@@ -117,5 +125,16 @@
+
+
+
+
+
+
+
+
+
+
+
diff --git a/plugins/Recovery.java b/plugins/Recovery.java
index 4f394ee..738b078 100644
--- a/plugins/Recovery.java
+++ b/plugins/Recovery.java
@@ -16,7 +16,7 @@ public class Recovery extends Panel {
public Recovery() {
initComponents();
- startTimer(3000, 200);
+ startTimer(1000, 200);
}
//Overridable callbacks
@@ -54,10 +54,18 @@ public class Recovery extends Panel {
List segment = (List) eval("get_current_segment()", true);
ledValidSegment.setColor((segment == null) ? Color.RED : Color.GREEN);
textSegment.setText((segment == null) ? "": segment.get(0) + "->" + segment.get(1) + " [" + segment.get(2) + "mm]");
+
+ try{
+ Object distance = eval("get_current_distance()", true);
+ textDistance.setText((distance == null) ? "" : String.format("%1.2f",((Number)distance).doubleValue()));
+ } catch (Exception ex) {
+ textDistance.setText("");
+ }
} catch (Exception ex) {
System.out.println(ex);
ledValidSegment.setColor(Color.BLACK);
textSegment.setText("");
+ textDistance.setText("");
}
try{
String point = (String) eval("robot.get_current_point()", true);
@@ -85,6 +93,8 @@ public class Recovery extends Panel {
buttonAbort = new javax.swing.JButton();
textPosition = new javax.swing.JTextField();
textSegment = new javax.swing.JTextField();
+ textDistance = new javax.swing.JTextField();
+ jLabel8 = new javax.swing.JLabel();
jLabel6.setText("Known position");
@@ -109,35 +119,43 @@ public class Recovery extends Panel {
textSegment.setEditable(false);
+ textDistance.setEditable(false);
+ textDistance.setHorizontalAlignment(javax.swing.JTextField.TRAILING);
+
+ jLabel8.setText("Distance:");
+
javax.swing.GroupLayout layout = new javax.swing.GroupLayout(this);
this.setLayout(layout);
layout.setHorizontalGroup(
layout.createParallelGroup(javax.swing.GroupLayout.Alignment.LEADING)
.addGroup(layout.createSequentialGroup()
+ .addContainerGap()
.addGroup(layout.createParallelGroup(javax.swing.GroupLayout.Alignment.LEADING)
.addGroup(layout.createSequentialGroup()
- .addContainerGap()
+ .addComponent(ledKnownPosition, javax.swing.GroupLayout.PREFERRED_SIZE, javax.swing.GroupLayout.DEFAULT_SIZE, javax.swing.GroupLayout.PREFERRED_SIZE)
+ .addPreferredGap(javax.swing.LayoutStyle.ComponentPlacement.RELATED)
+ .addComponent(jLabel6)
+ .addPreferredGap(javax.swing.LayoutStyle.ComponentPlacement.UNRELATED)
+ .addComponent(textPosition))
+ .addGroup(layout.createSequentialGroup()
+ .addComponent(ledValidSegment, javax.swing.GroupLayout.PREFERRED_SIZE, javax.swing.GroupLayout.DEFAULT_SIZE, javax.swing.GroupLayout.PREFERRED_SIZE)
+ .addPreferredGap(javax.swing.LayoutStyle.ComponentPlacement.RELATED)
+ .addComponent(jLabel7)
+ .addGap(16, 16, 16)
.addGroup(layout.createParallelGroup(javax.swing.GroupLayout.Alignment.LEADING)
.addGroup(layout.createSequentialGroup()
- .addComponent(ledKnownPosition, javax.swing.GroupLayout.PREFERRED_SIZE, javax.swing.GroupLayout.DEFAULT_SIZE, javax.swing.GroupLayout.PREFERRED_SIZE)
+ .addGap(0, 0, Short.MAX_VALUE)
+ .addComponent(jLabel8)
.addPreferredGap(javax.swing.LayoutStyle.ComponentPlacement.RELATED)
- .addComponent(jLabel6)
- .addPreferredGap(javax.swing.LayoutStyle.ComponentPlacement.UNRELATED)
- .addComponent(textPosition))
- .addGroup(layout.createSequentialGroup()
- .addComponent(ledValidSegment, javax.swing.GroupLayout.PREFERRED_SIZE, javax.swing.GroupLayout.DEFAULT_SIZE, javax.swing.GroupLayout.PREFERRED_SIZE)
- .addPreferredGap(javax.swing.LayoutStyle.ComponentPlacement.RELATED)
- .addComponent(jLabel7)
- .addGap(16, 16, 16)
- .addComponent(textSegment)))
- .addGap(8, 8, 8))
- .addGroup(layout.createSequentialGroup()
- .addContainerGap(136, Short.MAX_VALUE)
- .addGroup(layout.createParallelGroup(javax.swing.GroupLayout.Alignment.TRAILING)
- .addComponent(buttonAbort, javax.swing.GroupLayout.PREFERRED_SIZE, 73, javax.swing.GroupLayout.PREFERRED_SIZE)
- .addComponent(buttonRecover))
- .addPreferredGap(javax.swing.LayoutStyle.ComponentPlacement.RELATED, 144, Short.MAX_VALUE)))
- .addContainerGap())
+ .addComponent(textDistance, javax.swing.GroupLayout.PREFERRED_SIZE, 86, javax.swing.GroupLayout.PREFERRED_SIZE))
+ .addComponent(textSegment))))
+ .addGap(8, 8, 8))
+ .addGroup(layout.createSequentialGroup()
+ .addContainerGap(136, Short.MAX_VALUE)
+ .addGroup(layout.createParallelGroup(javax.swing.GroupLayout.Alignment.TRAILING)
+ .addComponent(buttonAbort, javax.swing.GroupLayout.PREFERRED_SIZE, 73, javax.swing.GroupLayout.PREFERRED_SIZE)
+ .addComponent(buttonRecover))
+ .addContainerGap(144, Short.MAX_VALUE))
);
layout.linkSize(javax.swing.SwingConstants.HORIZONTAL, new java.awt.Component[] {buttonAbort, buttonRecover});
@@ -155,11 +173,15 @@ public class Recovery extends Panel {
.addComponent(ledValidSegment, javax.swing.GroupLayout.PREFERRED_SIZE, javax.swing.GroupLayout.DEFAULT_SIZE, javax.swing.GroupLayout.PREFERRED_SIZE)
.addComponent(jLabel7)
.addComponent(textSegment, javax.swing.GroupLayout.PREFERRED_SIZE, javax.swing.GroupLayout.DEFAULT_SIZE, javax.swing.GroupLayout.PREFERRED_SIZE))
- .addGap(18, 18, 18)
+ .addPreferredGap(javax.swing.LayoutStyle.ComponentPlacement.UNRELATED)
+ .addGroup(layout.createParallelGroup(javax.swing.GroupLayout.Alignment.BASELINE)
+ .addComponent(jLabel8)
+ .addComponent(textDistance, javax.swing.GroupLayout.PREFERRED_SIZE, javax.swing.GroupLayout.DEFAULT_SIZE, javax.swing.GroupLayout.PREFERRED_SIZE))
+ .addPreferredGap(javax.swing.LayoutStyle.ComponentPlacement.RELATED, 81, Short.MAX_VALUE)
.addComponent(buttonRecover)
.addPreferredGap(javax.swing.LayoutStyle.ComponentPlacement.UNRELATED)
.addComponent(buttonAbort)
- .addContainerGap(javax.swing.GroupLayout.DEFAULT_SIZE, Short.MAX_VALUE))
+ .addContainerGap())
);
}// //GEN-END:initComponents
@@ -208,8 +230,10 @@ public class Recovery extends Panel {
private javax.swing.JButton buttonRecover;
private javax.swing.JLabel jLabel6;
private javax.swing.JLabel jLabel7;
+ private javax.swing.JLabel jLabel8;
private ch.psi.pshell.swing.Led ledKnownPosition;
private ch.psi.pshell.swing.Led ledValidSegment;
+ private javax.swing.JTextField textDistance;
private javax.swing.JTextField textPosition;
private javax.swing.JTextField textSegment;
// End of variables declaration//GEN-END:variables
diff --git a/script/devices/RobotSC.py b/script/devices/RobotSC.py
index b8f2de9..50af69a 100644
--- a/script/devices/RobotSC.py
+++ b/script/devices/RobotSC.py
@@ -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):
diff --git a/script/devices/RobotTCP.py b/script/devices/RobotTCP.py
index 40910a2..f70e948 100644
--- a/script/devices/RobotTCP.py
+++ b/script/devices/RobotTCP.py
@@ -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 +")")
diff --git a/script/devices/Wago.py b/script/devices/Wago.py
index b57b4d1..6a47090 100644
--- a/script/devices/Wago.py
+++ b/script/devices/Wago.py
@@ -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()
diff --git a/script/local.py b/script/local.py
index 3ad6711..152b1e7 100644
--- a/script/local.py
+++ b/script/local.py
@@ -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")
diff --git a/script/motion/dry.py b/script/motion/dry.py
index f4a70f6..322f109 100644
--- a/script/motion/dry.py
+++ b/script/motion/dry.py
@@ -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):
"""
diff --git a/script/motion/recover.py b/script/motion/recover.py
index d3690d1..1eb73fe 100644
--- a/script/motion/recover.py
+++ b/script/motion/recover.py
@@ -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: