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

4
devices/led_ctrl_1.properties
View File

@@ -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

4
devices/led_ctrl_2.properties
View File

@@ -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

4
devices/led_ctrl_3.properties
View File

@@ -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

BIN
plugins/MXSC-1.10.0.jar
View File

Binary file not shown.

71
plugins/Recovery.form
View File

@@ -17,37 +17,40 @@
<DimensionLayout dim="0">
<Group type="103" groupAlignment="0" attributes="0">
<Group type="102" attributes="0">
<EmptySpace min="-2" max="-2" attributes="0"/>
<Group type="103" groupAlignment="0" attributes="0">
<Group type="102" attributes="0">
<Group type="102" alignment="0" attributes="0">
<Component id="ledKnownPosition" min="-2" max="-2" attributes="0"/>
<EmptySpace min="-2" max="-2" attributes="0"/>
<Group type="103" groupAlignment="0" attributes="0">
<Group type="102" alignment="0" attributes="0">
<Component id="ledKnownPosition" min="-2" max="-2" attributes="0"/>
<EmptySpace min="-2" max="-2" attributes="0"/>
<Component id="jLabel6" min="-2" max="-2" attributes="0"/>
<EmptySpace type="unrelated" min="-2" max="-2" attributes="0"/>
<Component id="textPosition" max="32767" attributes="0"/>
</Group>
<Group type="102" alignment="0" attributes="0">
<Component id="ledValidSegment" min="-2" max="-2" attributes="0"/>
<EmptySpace min="-2" max="-2" attributes="0"/>
<Component id="jLabel7" min="-2" max="-2" attributes="0"/>
<EmptySpace min="-2" pref="16" max="-2" attributes="0"/>
<Component id="textSegment" max="32767" attributes="0"/>
</Group>
</Group>
<EmptySpace min="-2" pref="8" max="-2" attributes="0"/>
<Component id="jLabel6" min="-2" max="-2" attributes="0"/>
<EmptySpace type="unrelated" min="-2" max="-2" attributes="0"/>
<Component id="textPosition" max="32767" attributes="0"/>
</Group>
<Group type="102" alignment="0" attributes="0">
<EmptySpace pref="136" max="32767" attributes="0"/>
<Group type="103" groupAlignment="1" attributes="0">
<Component id="buttonAbort" linkSize="1" min="-2" pref="73" max="-2" attributes="0"/>
<Component id="buttonRecover" linkSize="1" alignment="1" min="-2" max="-2" attributes="0"/>
<Component id="ledValidSegment" min="-2" max="-2" attributes="0"/>
<EmptySpace min="-2" max="-2" attributes="0"/>
<Component id="jLabel7" min="-2" max="-2" attributes="0"/>
<EmptySpace min="-2" pref="16" max="-2" attributes="0"/>
<Group type="103" groupAlignment="0" attributes="0">
<Group type="102" alignment="0" attributes="0">
<EmptySpace min="0" pref="0" max="32767" attributes="0"/>
<Component id="jLabel8" min="-2" max="-2" attributes="0"/>
<EmptySpace max="-2" attributes="0"/>
<Component id="textDistance" min="-2" pref="86" max="-2" attributes="0"/>
</Group>
<Component id="textSegment" max="32767" attributes="0"/>
</Group>
<EmptySpace pref="144" max="32767" attributes="0"/>
</Group>
</Group>
<EmptySpace min="-2" max="-2" attributes="0"/>
<EmptySpace min="-2" pref="8" max="-2" attributes="0"/>
</Group>
<Group type="102" alignment="0" attributes="0">
<EmptySpace pref="136" max="32767" attributes="0"/>
<Group type="103" groupAlignment="1" attributes="0">
<Component id="buttonAbort" linkSize="1" min="-2" pref="73" max="-2" attributes="0"/>
<Component id="buttonRecover" linkSize="1" alignment="1" min="-2" max="-2" attributes="0"/>
</Group>
<EmptySpace pref="144" max="32767" attributes="0"/>
</Group>
</Group>
</DimensionLayout>
@@ -66,11 +69,16 @@
<Component id="jLabel7" alignment="3" min="-2" max="-2" attributes="0"/>
<Component id="textSegment" alignment="3" min="-2" max="-2" attributes="0"/>
</Group>
<EmptySpace type="separate" max="-2" attributes="0"/>
<EmptySpace type="unrelated" max="-2" attributes="0"/>
<Group type="103" groupAlignment="3" attributes="0">
<Component id="jLabel8" alignment="3" min="-2" max="-2" attributes="0"/>
<Component id="textDistance" alignment="3" min="-2" max="-2" attributes="0"/>
</Group>
<EmptySpace pref="81" max="32767" attributes="0"/>
<Component id="buttonRecover" min="-2" max="-2" attributes="0"/>
<EmptySpace type="unrelated" max="-2" attributes="0"/>
<Component id="buttonAbort" min="-2" max="-2" attributes="0"/>
<EmptySpace max="32767" attributes="0"/>
<EmptySpace max="-2" attributes="0"/>
</Group>
</Group>
</DimensionLayout>
@@ -117,5 +125,16 @@
<Property name="editable" type="boolean" value="false"/>
</Properties>
</Component>
<Component class="javax.swing.JTextField" name="textDistance">
<Properties>
<Property name="editable" type="boolean" value="false"/>
<Property name="horizontalAlignment" type="int" value="11"/>
</Properties>
</Component>
<Component class="javax.swing.JLabel" name="jLabel8">
<Properties>
<Property name="text" type="java.lang.String" value="Distance:"/>
</Properties>
</Component>
</SubComponents>
</Form>

68
plugins/Recovery.java
View File

@@ -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())
);
}// </editor-fold>//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

29
script/devices/RobotSC.py
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):

4
script/devices/RobotTCP.py
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 +")")

49
script/devices/Wago.py
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()

1
script/local.py
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")

25
script/motion/dry.py
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):
"""

106
script/motion/recover.py
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: