diff --git a/config/devices.properties b/config/devices.properties index 2ab3f36..8a2c023 100644 --- a/config/devices.properties +++ b/config/devices.properties @@ -1,12 +1,12 @@ img=ch.psi.pshell.prosilica.Prosilica|25001 "PacketSize=1522;PixelFormat=Mono8;BinningX=1;BinningY=1;RegionX=300;RegionY=200;Width=1000;Height=1000;MulticastEnable=Off"|||false -gripper_cam=ch.psi.pshell.imaging.MjpegSource|http://129.129.110.114/axis-cgi/mjpg/video.cgi||-1| -microscan=ch.psi.pshell.serial.TcpDevice|129.129.110.200:2001||| -microscan_cmd=ch.psi.pshell.serial.TcpDevice|129.129.110.200:2003||| +gripper_cam=ch.psi.pshell.imaging.MjpegSource|http://axis-accc8e9cc87b.psi.ch/axis-cgi/mjpg/video.cgi?camera=1||-1| +microscan=ch.psi.pshell.serial.TcpDevice|MicroHAWK38C528:2001||| +microscan_cmd=ch.psi.pshell.serial.TcpDevice|MicroHAWK38C528:2003||| ue=LaserUE|COM4|||false -#robot=RobotTcp|127.0.0.1:1000||| -#onewire=ch.psi.pshell.serial.TcpDevice|129.129.126.83:5000||| puck_detection=ch.psi.mxsc.PuckDetection|tell-raspberrypi:5556||| -#robot_modbus=ch.psi.pshell.modbus.ModbusTCP|129.129.126.100:502||| +#onewire=ch.psi.pshell.serial.TcpDevice|129.129.126.83:5000||| +#robot=RobotTcp|127.0.0.1:1000||| +#$robot_modbus=ch.psi.pshell.modbus.ModbusTCP|129.129.126.100:502||| #jf1=ch.psi.pshell.modbus.AnalogInput|robot_modbus 0||100| #jf2=ch.psi.pshell.modbus.AnalogInput|robot_modbus 1||100| #jf3=ch.psi.pshell.modbus.AnalogInput|robot_modbus 2||100| diff --git a/devices/dewar_level.properties b/devices/dewar_level.properties index 92a5279..38fab62 100644 --- a/devices/dewar_level.properties +++ b/devices/dewar_level.properties @@ -1,4 +1,4 @@ -#Tue Jun 19 16:41:20 CEST 2018 +#Wed Aug 08 14:23:54 CEST 2018 offset=-25.81632 precision=2 scale=0.003888 diff --git a/devices/img.properties b/devices/img.properties index cb23248..db77fa4 100644 --- a/devices/img.properties +++ b/devices/img.properties @@ -1,4 +1,4 @@ -#Thu Jun 14 10:54:07 CEST 2018 +#Wed Aug 08 11:53:16 CEST 2018 colormap=Grayscale colormapAutomatic=false colormapMax=18.133 @@ -9,16 +9,16 @@ grayscale=false invert=false rescaleFactor=1.0 rescaleOffset=0.0 -roiHeight=872 -roiWidth=869 -roiX=74 +roiHeight=-1 +roiWidth=-1 +roiX=0 roiY=0 -rotation=17.93079800677477 +rotation=0.0 rotationCrop=true scale=1.0 -spatialCalOffsetX=-435.0 -spatialCalOffsetY=-436.0 -spatialCalScaleX=0.540300793168967 -spatialCalScaleY=0.5385447475747382 +spatialCalOffsetX=NaN +spatialCalOffsetY=NaN +spatialCalScaleX=NaN +spatialCalScaleY=NaN spatialCalUnits=mm transpose=false diff --git a/devices/led_ctrl_1.properties b/devices/led_ctrl_1.properties index ff1c589..c0cba8a 100644 --- a/devices/led_ctrl_1.properties +++ b/devices/led_ctrl_1.properties @@ -1,4 +1,4 @@ -#Fri Jun 29 16:49:51 CEST 2018 +#Wed Aug 08 14:23:56 CEST 2018 maxValue=0.4 minValue=0.0 offset=0.0 diff --git a/devices/led_ctrl_2.properties b/devices/led_ctrl_2.properties index ff1c589..c0cba8a 100644 --- a/devices/led_ctrl_2.properties +++ b/devices/led_ctrl_2.properties @@ -1,4 +1,4 @@ -#Fri Jun 29 16:49:51 CEST 2018 +#Wed Aug 08 14:23:56 CEST 2018 maxValue=0.4 minValue=0.0 offset=0.0 diff --git a/devices/led_ctrl_3.properties b/devices/led_ctrl_3.properties index ff1c589..c0cba8a 100644 --- a/devices/led_ctrl_3.properties +++ b/devices/led_ctrl_3.properties @@ -1,4 +1,4 @@ -#Fri Jun 29 16:49:51 CEST 2018 +#Wed Aug 08 14:23:56 CEST 2018 maxValue=0.4 minValue=0.0 offset=0.0 diff --git a/plugins/MXSC-1.10.0.jar b/plugins/MXSC-1.10.0.jar index 0c137d2..f3f6315 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 0a89edd..15e9571 100644 --- a/plugins/Recovery.form +++ b/plugins/Recovery.form @@ -18,27 +18,36 @@ - - + + - - + + + + - - + + + + + - - + + + + + + - + @@ -47,44 +56,66 @@ - + + - + + - + + + - + - + - + + + + + + + + + - + + + + + + + + + + + diff --git a/plugins/Recovery.java b/plugins/Recovery.java index d6bd7a3..8f34918 100644 --- a/plugins/Recovery.java +++ b/plugins/Recovery.java @@ -5,6 +5,8 @@ import ch.psi.pshell.ui.Panel; import ch.psi.utils.State; import ch.psi.utils.swing.SwingUtils; +import java.awt.Color; +import java.util.List; /** * @@ -13,6 +15,7 @@ public class Recovery extends Panel { public Recovery() { initComponents(); + startTimer(3000, 200); } //Overridable callbacks @@ -23,83 +26,80 @@ public class Recovery extends Panel { @Override public void onStateChange(State state, State former) { + updateButton(); + } + + void updateButton(){ + buttonRecover.setEnabled((getContext().getState() == State.Ready) && + (!textSegment.getText().trim().isEmpty()) && + (textPosition.getText().trim().isEmpty()) ); + } @Override public void onExecutedFile(String fileName, Object result) { } - //Callback to perform update - in event thread @Override - protected void doUpdate() { - } - - @Override - public boolean isActive() { - return true; - } - - - void execute(String statement){ - execute(statement, false); - } - - void execute(String statement, boolean background){ - execute(statement, background, false); - } - - void execute(String statement, boolean background, boolean showReturn){ - try { - evalAsync(statement, background).handle((ret, ex) -> { - if (ex != null){ - showException((Exception)ex); - } else if (showReturn){ - SwingUtils.showMessage(getTopLevel(), "Return", String.valueOf(ret)); - } - return ret; - }); + protected void onTimer() { + try{ + 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]"); } catch (Exception ex) { - showException(ex); + System.out.println(ex); + ledValidSegment.setColor(Color.BLACK); + textSegment.setText(""); } - } - - void execute(String script, Object args){ - try { - runAsync(script, args).handle((ret, ex) -> { - if (ex != null){ - showException((Exception)ex); - } - return ret; - }); + try{ + String point = (String) eval("robot.get_current_point()", true); + ledKnownPosition.setColor((point == null) ? Color.RED : Color.GREEN); + textPosition.setText((point == null) ? "": point); } catch (Exception ex) { - showException(ex); - } - } - - - + System.out.println(ex); + ledKnownPosition.setColor(Color.BLACK); + textPosition.setText(""); + } + updateButton(); + } + + @SuppressWarnings("unchecked") // //GEN-BEGIN:initComponents private void initComponents() { - ledLidControlActive = new ch.psi.pshell.swing.Led(); + ledKnownPosition = new ch.psi.pshell.swing.Led(); jLabel6 = new javax.swing.JLabel(); - ledLidInitialized = new ch.psi.pshell.swing.Led(); + ledValidSegment = new ch.psi.pshell.swing.Led(); jLabel7 = new javax.swing.JLabel(); - buttonInitHexiposi = new javax.swing.JButton(); + buttonRecover = new javax.swing.JButton(); + buttonAbort = new javax.swing.JButton(); + textPosition = new javax.swing.JTextField(); + textSegment = new javax.swing.JTextField(); jLabel6.setText("Known position"); jLabel7.setText("Valid segment"); - buttonInitHexiposi.setText("Recover"); - buttonInitHexiposi.setEnabled(false); - buttonInitHexiposi.addActionListener(new java.awt.event.ActionListener() { + buttonRecover.setText("Recover"); + buttonRecover.addActionListener(new java.awt.event.ActionListener() { public void actionPerformed(java.awt.event.ActionEvent evt) { - buttonInitHexiposiActionPerformed(evt); + buttonRecoverActionPerformed(evt); } }); + buttonAbort.setText("Abort"); + buttonAbort.setEnabled(false); + buttonAbort.addActionListener(new java.awt.event.ActionListener() { + public void actionPerformed(java.awt.event.ActionEvent evt) { + buttonAbortActionPerformed(evt); + } + }); + + textPosition.setEditable(false); + + textSegment.setEditable(false); + javax.swing.GroupLayout layout = new javax.swing.GroupLayout(this); this.setLayout(layout); layout.setHorizontalGroup( @@ -110,48 +110,98 @@ public class Recovery extends Panel { .addContainerGap() .addGroup(layout.createParallelGroup(javax.swing.GroupLayout.Alignment.LEADING) .addGroup(layout.createSequentialGroup() - .addComponent(ledLidControlActive, javax.swing.GroupLayout.PREFERRED_SIZE, javax.swing.GroupLayout.DEFAULT_SIZE, javax.swing.GroupLayout.PREFERRED_SIZE) + .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)) + .addComponent(jLabel6) + .addPreferredGap(javax.swing.LayoutStyle.ComponentPlacement.UNRELATED) + .addComponent(textPosition)) .addGroup(layout.createSequentialGroup() - .addComponent(ledLidInitialized, javax.swing.GroupLayout.PREFERRED_SIZE, javax.swing.GroupLayout.DEFAULT_SIZE, javax.swing.GroupLayout.PREFERRED_SIZE) + .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)))) + .addComponent(jLabel7) + .addGap(16, 16, 16) + .addComponent(textSegment))) + .addGap(8, 8, 8)) .addGroup(layout.createSequentialGroup() - .addContainerGap(96, Short.MAX_VALUE) - .addComponent(buttonInitHexiposi))) - .addContainerGap(116, Short.MAX_VALUE)) + .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()) ); + + layout.linkSize(javax.swing.SwingConstants.HORIZONTAL, new java.awt.Component[] {buttonAbort, buttonRecover}); + layout.setVerticalGroup( layout.createParallelGroup(javax.swing.GroupLayout.Alignment.LEADING) .addGroup(layout.createSequentialGroup() .addContainerGap() .addGroup(layout.createParallelGroup(javax.swing.GroupLayout.Alignment.BASELINE) - .addComponent(ledLidControlActive, javax.swing.GroupLayout.PREFERRED_SIZE, javax.swing.GroupLayout.DEFAULT_SIZE, javax.swing.GroupLayout.PREFERRED_SIZE) - .addComponent(jLabel6)) + .addComponent(ledKnownPosition, javax.swing.GroupLayout.PREFERRED_SIZE, javax.swing.GroupLayout.DEFAULT_SIZE, javax.swing.GroupLayout.PREFERRED_SIZE) + .addComponent(jLabel6) + .addComponent(textPosition, javax.swing.GroupLayout.PREFERRED_SIZE, javax.swing.GroupLayout.DEFAULT_SIZE, javax.swing.GroupLayout.PREFERRED_SIZE)) .addPreferredGap(javax.swing.LayoutStyle.ComponentPlacement.RELATED) .addGroup(layout.createParallelGroup(javax.swing.GroupLayout.Alignment.BASELINE) - .addComponent(ledLidInitialized, javax.swing.GroupLayout.PREFERRED_SIZE, javax.swing.GroupLayout.DEFAULT_SIZE, javax.swing.GroupLayout.PREFERRED_SIZE) - .addComponent(jLabel7)) + .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) - .addComponent(buttonInitHexiposi) + .addComponent(buttonRecover) + .addPreferredGap(javax.swing.LayoutStyle.ComponentPlacement.UNRELATED) + .addComponent(buttonAbort) .addContainerGap(javax.swing.GroupLayout.DEFAULT_SIZE, Short.MAX_VALUE)) ); }// //GEN-END:initComponents - private void buttonInitHexiposiActionPerformed(java.awt.event.ActionEvent evt) {//GEN-FIRST:event_buttonInitHexiposiActionPerformed + private void buttonRecoverActionPerformed(java.awt.event.ActionEvent evt) {//GEN-FIRST:event_buttonRecoverActionPerformed try{ - this.showDevicePanel("hexiposi"); + buttonAbort.setEnabled(true); + evalAsync("recover()", false).handle((ret, ex) -> { + if (ex != null){ + showException((Exception)ex); + } else { + SwingUtils.showMessage(getTopLevel(), "Return", String.valueOf(ret)); + } + buttonAbort.setEnabled(false); + return ret; + }); } catch (Exception ex) { showException(ex); } - }//GEN-LAST:event_buttonInitHexiposiActionPerformed + }//GEN-LAST:event_buttonRecoverActionPerformed + + private void buttonAbortActionPerformed(java.awt.event.ActionEvent evt) {//GEN-FIRST:event_buttonAbortActionPerformed + try { + abort(); + try{ + eval("robot.disable()", true); + } catch (Exception ex){ + this.showException(ex); + } + try{ + eval("stop_task()", true); + } catch (Exception ex){ + this.showException(ex); + } + try{ + eval("robot.reset_motion()", true); + } catch (Exception ex){ + this.showException(ex); + } + } catch (InterruptedException ex) { + showException(ex); + } + }//GEN-LAST:event_buttonAbortActionPerformed // Variables declaration - do not modify//GEN-BEGIN:variables - private javax.swing.JButton buttonInitHexiposi; + private javax.swing.JButton buttonAbort; + private javax.swing.JButton buttonRecover; private javax.swing.JLabel jLabel6; private javax.swing.JLabel jLabel7; - private ch.psi.pshell.swing.Led ledLidControlActive; - private ch.psi.pshell.swing.Led ledLidInitialized; + private ch.psi.pshell.swing.Led ledKnownPosition; + private ch.psi.pshell.swing.Led ledValidSegment; + private javax.swing.JTextField textPosition; + private javax.swing.JTextField textSegment; // End of variables declaration//GEN-END:variables } diff --git a/script/data/get_samples_info.py b/script/data/get_samples_info.py new file mode 100644 index 0000000..00a5bc3 --- /dev/null +++ b/script/data/get_samples_info.py @@ -0,0 +1,4 @@ + + + +set_return(get_samples_info(as_json=False)) \ No newline at end of file diff --git a/script/data/samples.py b/script/data/samples.py new file mode 100644 index 0000000..7db91a4 --- /dev/null +++ b/script/data/samples.py @@ -0,0 +1,23 @@ +import json +import org.python.core.PyDictionary as PyDictionary + + +samples_info = [] + +def set_samples_info(info): + global samples_info + if (is_string(info)): + info = json.loads(info) + if not is_list(info): + raise Exception("Sample info must be a list (given object type is " + str(type(info)) + ")") + #for sample in info: + # if not (type(sample) is PyDictionary): + # raise Exception("Sample info element must be a dictionary (given object type is " + str(type(sample)) + ")") + samples_info = info + return samples_info + + + +def get_samples_info(as_json=True): + global sample_info + return json.dumps(samples_info) if as_json else samples_info \ No newline at end of file diff --git a/script/data/set_samples_info.py b/script/data/set_samples_info.py new file mode 100644 index 0000000..bc7e803 --- /dev/null +++ b/script/data/set_samples_info.py @@ -0,0 +1,4 @@ + + + +set_samples_info(args[0]) \ No newline at end of file diff --git a/script/devices/Hexiposi.py b/script/devices/Hexiposi.py index 77794db..01f5a1e 100644 --- a/script/devices/Hexiposi.py +++ b/script/devices/Hexiposi.py @@ -127,7 +127,7 @@ class Hexiposi(DiscretePositionerBase): -#http://129.129.110.83:8002/hexiposi/get +#http://myriotell:8002/hexiposi/get dev = Hexiposi("hexiposi", "myriotell:8002/hexiposi") add_device(dev, True) diff --git a/script/devices/RobotSC.py b/script/devices/RobotSC.py index a8e3a21..7bd8b14 100644 --- a/script/devices/RobotSC.py +++ b/script/devices/RobotSC.py @@ -210,13 +210,19 @@ class RobotSC(RobotTCP): robot.waitState(State.Ready, 1000) #robot.state.assertReady() if simulation: - #add_device(RobotSC("robot","129.129.126.92:1000"),force = True) - add_device(RobotSC("robot","129.129.110.81:1000"),force = True) + add_device(RobotSC("robot","localhost:1000"),force = True) else: - add_device(RobotSC("robot", "129.129.110.100:1000"), force = True) + #add_device(RobotSC("robot", "129.129.243.120:1000"), force = True) + #add_device(RobotSC("robot", "pcp068129.psi.ch:1000"), force = True) + #add_device(RobotSC("robot", "saresb-cons-06.psi.ch:1000"), force = True) + add_device(RobotSC("robot", "129.129.243.90:1000"), force = True) + -#robot.set_monitor_speed(20) robot.set_default_desc(DESC_DEFAULT) +robot.default_speed = 20 +robot.set_tool(TOOL_DEFAULT) +robot.setPolling(DEFAULT_ROBOT_POLLING) + class jf1(ReadonlyRegisterBase): diff --git a/script/devices/RobotTCP.py b/script/devices/RobotTCP.py index 662a2b5..fb8616c 100644 --- a/script/devices/RobotTCP.py +++ b/script/devices/RobotTCP.py @@ -45,7 +45,8 @@ class RobotTCP(TcpDevice, Stoppable): self.frame = FRAME_DEFAULT self.polling_interval = 0.01 self.reset = True - self.default_tolerance = 10 + self.default_tolerance = 10 + self.default_speed = 100 self.task_start_retries = 3 self.exception_on_task_start_failure = True #Tasks may start and be finished when checked @@ -68,8 +69,11 @@ class RobotTCP(TcpDevice, Stoppable): return self.tool - def assert_tool(self, tool): - if self.tool != tool: + def assert_tool(self, tool=None): + if tool is None: + if self.tool is None: + raise Exception("Tool is undefined") + elif self.tool != tool: raise Exception("Invalid tool: " + self.tool) def set_default_desc(self,default_desc): @@ -213,11 +217,13 @@ class RobotTCP(TcpDevice, Stoppable): def get_tool_trsf(self, name=None): if name is None: + self.assert_tool() name = self.tool return self.get_trsf(name+".trsf") def set_tool_trsf(self, trsf, name=None): if name is None: + self.assert_tool() name = self.tool self.set_trsf(trsf, name+".trsf") @@ -275,9 +281,12 @@ class RobotTCP(TcpDevice, Stoppable): if (ret==-1): raise Exception("The robot is not in remote working mode") if (ret==-2): raise Exception("The monitor speed is under the control of the operator") if (ret==-3): raise Exception("The specified speed is not supported") + + def set_default_speed(self): + set_monitor_speed(self.default_speed) def is_calibrated(self): - return self.eval_bool("isCalibrated()") + return self.eval_bool("isCalibrated()") def save_program(self): ret = self.execute('save', timeout=5000) @@ -367,6 +376,10 @@ class RobotTCP(TcpDevice, Stoppable): def movej(self, joint_or_point, tool=None, desc=None, sync=False): 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): + robot.set_pnt(joint_or_point , "tcp_p") + joint_or_point = "tcp_p" ret = self.eval_int("movej(" + joint_or_point + ", " + tool + ", " + desc +")") if sync: self.wait_end_of_move() @@ -375,6 +388,9 @@ class RobotTCP(TcpDevice, Stoppable): def movel(self, point, tool=None, desc=None, sync=False): if desc is None: desc = self.default_desc if tool is None: tool = self.tool + if not is_string(point): + robot.set_pnt(point , "tcp_p") + point = "tcp_p" ret = self.eval_int("movel(" + point + ", " + tool + ", " + desc +")") if sync: self.wait_end_of_move() @@ -588,13 +604,15 @@ class RobotTCP(TcpDevice, Stoppable): def get_flange_pos(self): return self.eval_pnt("jointToPoint(" +FLANGE + ", " + self.frame + ", herej())") - def get_cartesian_pos(self): + def get_cartesian_pos(self): + self.assert_tool() return self.eval_pnt("jointToPoint(" + self.tool + ", " + self.frame + ", herej())") #self.set_jnt(self.herej()) #return self.joint_to_point(tool, frame) def get_cartesian_destination(self): - return self.here(self.tool, self.frame) + self.assert_tool() + return self.here(self.tool, self.frame) def get_distance_to_pnt(self, name): #self.here(self.tool, self.frame) #??? @@ -672,6 +690,7 @@ class RobotTCP(TcpDevice, Stoppable): m.initialize() #Alignment def align(self, point = None, frame = FRAME_DEFAULT, desc = None): + self.assert_tool() if desc is None: desc = self.default_desc if point is None: self.get_cartesian_pos() diff --git a/script/local.py b/script/local.py index e7b45da..df1ab13 100644 --- a/script/local.py +++ b/script/local.py @@ -5,6 +5,7 @@ import traceback 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 run("setup/Layout") @@ -29,6 +30,7 @@ add_device(img.getCamera(), force = True) # Utility modules ################################################################################################### +run("data/samples") run("motion/tools") run("motion/mount") run("motion/unmount") @@ -45,6 +47,7 @@ run("motion/move_scanner") run("motion/dry") run("motion/homing_hexiposi") run("motion/robot_recover") +run("motion/recover") run("imgproc/Utils") run("tools/Math") @@ -81,10 +84,6 @@ def check_puck_detection(): def stop_puck_detection(): run("tools/StopPuckDetection") -DEWAR_LEVEL_RT = 5.0 - -def is_room_temp(): - return dewar_level.read() <= DEWAR_LEVEL_RT ################################################################################################### @@ -111,7 +110,7 @@ except: try: robot.setPolling(DEFAULT_ROBOT_POLLING) - robot.set_tool(TOOL_CALIBRATION) + robot.set_tool(TOOL_DEFAULT) robot.set_motors_enabled(True) robot.set_joint_motors_enabled(True) except: @@ -143,6 +142,36 @@ except: #gripper_cam.paused = True + + +################################################################################################### +# Device monitoring +################################################################################################### + +DEWAR_LEVEL_RT = 5.0 +is_room_temperature = False + +def is_room_temp(): + return is_room_temperature + + +class DewarLevelListener (DeviceListener): + def onValueChanged(self, device, value, former): + global is_room_temperature + if value is not None: + is_room_temperature = value <= DEWAR_LEVEL_RT +dewar_level_listener = DewarLevelListener() + +for l in dewar_level.listeners: + #if isinstance(l, DewarLevelListener): #Class changes... + if Nameable.getShortClassName(l.getClass()) == "DewarLevelListener": + dewar_level.removeListener(l) + +dewar_level.addListener(dewar_level_listener) +dewar_level_listener.onValueChanged(dewar_level, dewar_level.take(), None) + + + ################################################################################################### # Global variables ################################################################################################### diff --git a/script/motion/recover.py b/script/motion/recover.py index 37e7d77..d3690d1 100644 --- a/script/motion/recover.py +++ b/script/motion/recover.py @@ -1,5 +1,109 @@ -def recover(): - """ - """ - print "recover" +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 + + +known_segments = [ ("pHome", "pPark", 10), \ + ("pHome", "pDewarHome", 10), \ + ("pHome", "pGonioHome", 10), \ + ("pHome", "pScanHome", 10), \ + ("pHome", "pHeaterHome", 10), \ + ("pDewarHome", "pDewarWait", 10), \ + ("pGonioHome", "pGonioGet", 10), \ + ("pHeaterHome", "pHeater", 10), \ + ("pHeater", "pHeaterBottom", 10), \ + ] + + +def is_on_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) + + #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_current_segment(): + for segment in known_segments: + if is_on_segment(segment): + return 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) + 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]] + 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) + + +#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) + else: + robot.movel(segment[0], tool=None, desc=DESC_SLOW, sync=True) + #print "Recovered to point " + str(robot.get_current_point()) + + +def recover(): + #Initial checks + robot.assert_no_task() + robot.reset_motion() + robot.wait_ready() + + if robot.get_current_point() is not None: + raise Exception("Robot is in known location") + + #Enabling + enable_motion() + + + is_on_known_segment = False + for segment in known_segments: + if is_on_segment(segment): + #try: + # robot.set_monitor_speed(5) + is_on_known_segment = True + move_to_segment(segment) + move_to_safest_point(segment) + return "Success recovering to point: " + str(robot.get_current_point()) + #finally: + # robot.set_default_speed() + if not is_on_known_segment: + raise Exception("Robot is not in known segment") + + + + + \ No newline at end of file diff --git a/script/test/TestBugPcAPI2.py b/script/test/TestBugPcAPI2.py index 0747cd0..5e92b37 100644 --- a/script/test/TestBugPcAPI2.py +++ b/script/test/TestBugPcAPI2.py @@ -1,6 +1,6 @@ import ch.psi.pshell.imaging.MjpegSource as MjpegSource MjpegSource2 = get_context().pluginManager.getDynamicClass("MjpegSource2") -add_device(MjpegSource("gripper_cam", "http://129.129.110.114/axis-cgi/mjpg/video.cgi"), True) +add_device(MjpegSource("gripper_cam", "http://axis-accc8e9cc87b.psi.ch/axis-cgi/mjpg/video.cgi"), True) #gripper_cam.polling=1000 gripper_cam.monitored = True show_panel(gripper_cam) \ No newline at end of file diff --git a/script/test/TestEuclidean.py b/script/test/TestEuclidean.py new file mode 100644 index 0000000..4376dc7 --- /dev/null +++ b/script/test/TestEuclidean.py @@ -0,0 +1,23 @@ +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 Line + + +p = Vector3D(0, 0, 3) + +p1 = Vector3D(0,0,0) +p2 = Vector3D(0,1,1 ) +tolerance = 0.001 + +l = Line(p1, p2, tolerance) + + +print l.distance(p) +print p1.distance(p) +print p2.distance(p) +print "---" + +print l.getAbscissa(p) +print l.pointAt(l.getAbscissa(p)) + +#print l.closestPoint(Line(p, p, 0.01)) \ No newline at end of file diff --git a/script/test/TestMicrohawk.py b/script/test/TestMicrohawk.py index fbe8471..0ea517e 100644 --- a/script/test/TestMicrohawk.py +++ b/script/test/TestMicrohawk.py @@ -2,8 +2,8 @@ import ch.psi.pshell.serial.TcpDevice as TcpDevice import ch.psi.pshell.serial.UdpDevice as UdpDevice -microscan = TcpDevice("microscan", "129.129.126.200:2001") -#microscan = UdpDevice("microscan", "129.129.126.200:2001") +microscan = TcpDevice("microscan", "MicroHAWK38C528:2001") +#microscan = UdpDevice("microscan", "MicroHAWK38C528:2001") microscan.initialize() diff --git a/script/test/TestRecover.py b/script/test/TestRecover.py new file mode 100644 index 0000000..340853f --- /dev/null +++ b/script/test/TestRecover.py @@ -0,0 +1,15 @@ +print "Pos=" + str(robot.get_cartesian_pos()) +for p in robot.get_known_points(): + print p + " = " + str(get_pnt(p)) + +print "-------------" + +for segment in known_segments: + is_on_segment(segment) + +print "-------------" +for segment in known_segments: + try: + move_to_segment(segment) + except: + print sys.exc_info()[1] \ No newline at end of file