From 887b52c9e331e591486ab3ede470948ca6e70c28 Mon Sep 17 00:00:00 2001 From: gac-S_Changer Date: Tue, 23 Jul 2019 11:35:08 +0200 Subject: [PATCH] --- plugins/Recovery.form | 43 +++++++++++++++++++++++++++------- plugins/Recovery.java | 50 +++++++++++++++++++++++++++++++--------- script/motion/recover.py | 17 +++++++++----- 3 files changed, 85 insertions(+), 25 deletions(-) diff --git a/plugins/Recovery.form b/plugins/Recovery.form index e892c04..60f2786 100644 --- a/plugins/Recovery.form +++ b/plugins/Recovery.form @@ -27,9 +27,18 @@ - - - + + + + + + + + + + + + @@ -69,12 +78,23 @@ - - - - + + + + + + + + + + + + + + + - + @@ -136,5 +156,12 @@ + + + + + + + diff --git a/plugins/Recovery.java b/plugins/Recovery.java index 844c3c1..4967e94 100644 --- a/plugins/Recovery.java +++ b/plugins/Recovery.java @@ -38,9 +38,10 @@ public class Recovery extends Panel { } void updateButton(){ - buttonRecover.setEnabled((getContext().getState() == State.Ready) && - (!textSegment.getText().trim().isEmpty()) && - (textPosition.getText().trim().isEmpty()) ); + buttonRecover.setEnabled((getContext().getState() == State.Ready) && + (textPosition.getText().trim().isEmpty()) && + ((!textSegment.getText().trim().isEmpty()) || (ledIsInDewar.getColor()==Color.GREEN)) + ); } @@ -58,6 +59,7 @@ public class Recovery extends Panel { textSegment.setText(""); ledKnownPosition.setColor(Color.BLACK); textPosition.setText(""); + ledIsInDewar.setColor(Color.BLACK); } else { if (getState()==State.Ready){ String point = null; @@ -90,6 +92,13 @@ public class Recovery extends Panel { textSegment.setText(""); textDistance.setText(""); } + try{ + Boolean is_in_dewar = (Boolean) eval("is_in_dewar()", true); + ledIsInDewar.setColor(is_in_dewar ? Color.GREEN : Color.RED); + } catch (Exception ex) { + System.out.println(ex); + ledIsInDewar.setColor(Color.BLACK); + } } } updateButton(); @@ -110,6 +119,8 @@ public class Recovery extends Panel { textSegment = new javax.swing.JTextField(); textDistance = new javax.swing.JTextField(); jLabel8 = new javax.swing.JLabel(); + ledIsInDewar = new ch.psi.pshell.swing.Led(); + jLabel9 = new javax.swing.JLabel(); jLabel6.setText("Known position"); @@ -139,6 +150,8 @@ public class Recovery extends Panel { jLabel8.setText("Distance:"); + jLabel9.setText("Is in Dewar"); + javax.swing.GroupLayout layout = new javax.swing.GroupLayout(this); this.setLayout(layout); layout.setHorizontalGroup( @@ -153,9 +166,15 @@ public class Recovery extends Panel { .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) + .addGroup(layout.createParallelGroup(javax.swing.GroupLayout.Alignment.LEADING) + .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)) + .addGroup(layout.createSequentialGroup() + .addComponent(ledIsInDewar, javax.swing.GroupLayout.PREFERRED_SIZE, javax.swing.GroupLayout.DEFAULT_SIZE, javax.swing.GroupLayout.PREFERRED_SIZE) + .addPreferredGap(javax.swing.LayoutStyle.ComponentPlacement.RELATED) + .addComponent(jLabel9))) .addGap(16, 16, 16) .addGroup(layout.createParallelGroup(javax.swing.GroupLayout.Alignment.LEADING) .addGroup(layout.createSequentialGroup() @@ -188,11 +207,18 @@ 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)) - .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) + .addGroup(layout.createParallelGroup(javax.swing.GroupLayout.Alignment.LEADING) + .addGroup(layout.createSequentialGroup() + .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))) + .addGroup(layout.createSequentialGroup() + .addGap(30, 30, 30) + .addGroup(layout.createParallelGroup(javax.swing.GroupLayout.Alignment.BASELINE) + .addComponent(ledIsInDewar, javax.swing.GroupLayout.PREFERRED_SIZE, javax.swing.GroupLayout.DEFAULT_SIZE, javax.swing.GroupLayout.PREFERRED_SIZE) + .addComponent(jLabel9)))) + .addPreferredGap(javax.swing.LayoutStyle.ComponentPlacement.RELATED, 62, Short.MAX_VALUE) .addComponent(buttonRecover) .addPreferredGap(javax.swing.LayoutStyle.ComponentPlacement.UNRELATED) .addComponent(buttonAbort) @@ -246,6 +272,8 @@ public class Recovery extends Panel { private javax.swing.JLabel jLabel6; private javax.swing.JLabel jLabel7; private javax.swing.JLabel jLabel8; + private javax.swing.JLabel jLabel9; + private ch.psi.pshell.swing.Led ledIsInDewar; private ch.psi.pshell.swing.Led ledKnownPosition; private ch.psi.pshell.swing.Led ledValidSegment; private javax.swing.JTextField textDistance; diff --git a/script/motion/recover.py b/script/motion/recover.py index d58bf7e..cf0b498 100644 --- a/script/motion/recover.py +++ b/script/motion/recover.py @@ -118,9 +118,9 @@ def move_to_safest_point(segment, vicinity_tolerance = 100): #print "Recovered to point " + str(robot.get_curjoint_or_pointrent_point()) def is_in_dewar(): - z_dwr = robot.get_pnt('pDewar')[2] + z_hom = robot.get_pnt('pHome')[2] z_cur=robot.get_cartesian_pos()[2] - if z_cur > (z_dwr + 30): + if z_cur > (z_hom - 30): return False d_dwr = robot.get_distance_to_pnt('pDewar') if d_dwr > 300: @@ -157,17 +157,22 @@ def recover(move_park = True): print "Success recovered to point: " + str(location) if move_park: robot.move_park() - return "Success recovered to park position" + return "Success recovering to park position" else: - return "Success recovered to point: " + str(location) + return "Success recovering to point: " + str(location) #finally: # robot.set_default_speed() if not is_on_known_segment: print ("Robot is not in known segment") if is_in_dewar(): robot.robot_recover() - else: - raise Exception("Robot is not in known segment or inside the dewar") + if move_park: + robot.move_park() + return "Success recovering from dewar to park position" + else: + return "Success recovering from dewar" + else: + raise Exception("Robot is not in known segment nor inside the dewar")