diff --git a/config/devices.properties b/config/devices.properties index 28feb86..cbd53dc 100644 --- a/config/devices.properties +++ b/config/devices.properties @@ -4,7 +4,7 @@ microscan_cmd=ch.psi.pshell.serial.TcpDevice|129.129.126.200: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|raspberrypi:5556||| +puck_detection=ch.psi.mxsc.PuckDetection|tell-raspberrypi:5556||| #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| diff --git a/config/plugins.properties b/config/plugins.properties index c14bc69..e9e96d2 100644 --- a/config/plugins.properties +++ b/config/plugins.properties @@ -1,3 +1,4 @@ +Expert.java=enabled MXSC-1.9.0.jar=enabled RobotPanel.java=enabled Wayne.java=disabled diff --git a/devices/img.properties b/devices/img.properties index 79e039f..2f441d2 100644 --- a/devices/img.properties +++ b/devices/img.properties @@ -1,4 +1,4 @@ -#Tue Oct 24 14:01:22 CEST 2017 +#Mon Apr 09 18:08:09 CEST 2018 colormap=Grayscale colormapAutomatic=false colormapMax=18.133 @@ -9,16 +9,16 @@ grayscale=false invert=false rescaleFactor=1.0 rescaleOffset=0.0 -roiHeight=1200 -roiWidth=1200 -roiX=200 +roiHeight=-1 +roiWidth=-1 +roiX=0 roiY=0 rotation=0.0 rotationCrop=true scale=1.0 -spatialCalOffsetX=-600.0 -spatialCalOffsetY=-600.0 -spatialCalScaleX=0.4 -spatialCalScaleY=-0.4 +spatialCalOffsetX=NaN +spatialCalOffsetY=NaN +spatialCalScaleX=NaN +spatialCalScaleY=NaN spatialCalUnits=mm transpose=false diff --git a/plugins/Expert.form b/plugins/Expert.form new file mode 100644 index 0000000..503794f --- /dev/null +++ b/plugins/Expert.form @@ -0,0 +1,223 @@ + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
diff --git a/plugins/Expert.java b/plugins/Expert.java new file mode 100644 index 0000000..d9eb961 --- /dev/null +++ b/plugins/Expert.java @@ -0,0 +1,283 @@ +/* + * Copyright (c) 2014-2017 Paul Scherrer Institute. All rights reserved. + */ + +import ch.psi.pshell.core.Context; +import ch.psi.pshell.ui.Panel; +import ch.psi.utils.State; +import java.util.logging.Level; +import java.util.logging.Logger; + +/** + * + */ +public class Expert extends Panel { + + public Expert() { + initComponents(); + } + + //Overridable callbacks + @Override + public void onInitialize(int runCount) { + + } + + @Override + public void onStateChange(State state, State former) { + + } + + @Override + public void onExecutedFile(String fileName, Object result) { + } + + //Callback to perform update - in event thread + @Override + protected void doUpdate() { + } + + void execute(String statement){ + execute(statement, false); + } + + void execute(String statement, boolean background){ + try { + evalAsync(statement, background).handle((ret, ex) -> { + if (ex != null){ + showException((Exception)ex); + } + return ret; + }); + } catch (Exception ex) { + showException(ex); + } + } + + void execute(String script, Object args){ + try { + runAsync(script, args).handle((ret, ex) -> { + if (ex != null){ + showException((Exception)ex); + } + return ret; + }); + } catch (Exception ex) { + showException(ex); + } + } + + @SuppressWarnings("unchecked") + // //GEN-BEGIN:initComponents + private void initComponents() { + + buttonEnableAll = new javax.swing.JButton(); + buttonMount = new javax.swing.JButton(); + jLabel1 = new javax.swing.JLabel(); + spinnerSegment = new javax.swing.JSpinner(); + jLabel2 = new javax.swing.JLabel(); + spinnerPuck = new javax.swing.JSpinner(); + jLabel3 = new javax.swing.JLabel(); + spinnerSample = new javax.swing.JSpinner(); + checkFirst = new javax.swing.JCheckBox(); + jPanel1 = new javax.swing.JPanel(); + buttonEnable = new javax.swing.JButton(); + buttonDisable = new javax.swing.JButton(); + buttonReleaseLocal = new javax.swing.JButton(); + buttonReleasePsys = new javax.swing.JButton(); + + buttonEnableAll.setText("Enable All"); + buttonEnableAll.addActionListener(new java.awt.event.ActionListener() { + public void actionPerformed(java.awt.event.ActionEvent evt) { + buttonEnableAllActionPerformed(evt); + } + }); + + buttonMount.setText("Mount"); + buttonMount.addActionListener(new java.awt.event.ActionListener() { + public void actionPerformed(java.awt.event.ActionEvent evt) { + buttonMountActionPerformed(evt); + } + }); + + jLabel1.setText("Segment:"); + + spinnerSegment.setModel(new javax.swing.SpinnerNumberModel(1, 1, 6, 1)); + + jLabel2.setText("Puck:"); + + spinnerPuck.setModel(new javax.swing.SpinnerNumberModel(1, 1, 5, 1)); + + jLabel3.setText("Sample:"); + + spinnerSample.setModel(new javax.swing.SpinnerNumberModel(1, 1, 16, 1)); + + checkFirst.setSelected(true); + checkFirst.setText("First mount"); + + jPanel1.setBorder(javax.swing.BorderFactory.createTitledBorder("Robot")); + + buttonEnable.setText("Enable"); + buttonEnable.addActionListener(new java.awt.event.ActionListener() { + public void actionPerformed(java.awt.event.ActionEvent evt) { + buttonEnableActionPerformed(evt); + } + }); + + buttonDisable.setText("Disable"); + buttonDisable.addActionListener(new java.awt.event.ActionListener() { + public void actionPerformed(java.awt.event.ActionEvent evt) { + buttonDisableActionPerformed(evt); + } + }); + + javax.swing.GroupLayout jPanel1Layout = new javax.swing.GroupLayout(jPanel1); + jPanel1.setLayout(jPanel1Layout); + jPanel1Layout.setHorizontalGroup( + jPanel1Layout.createParallelGroup(javax.swing.GroupLayout.Alignment.LEADING) + .addGroup(jPanel1Layout.createSequentialGroup() + .addContainerGap(59, Short.MAX_VALUE) + .addGroup(jPanel1Layout.createParallelGroup(javax.swing.GroupLayout.Alignment.LEADING) + .addComponent(buttonEnable) + .addComponent(buttonDisable)) + .addContainerGap(75, Short.MAX_VALUE)) + ); + + jPanel1Layout.linkSize(javax.swing.SwingConstants.HORIZONTAL, new java.awt.Component[] {buttonDisable, buttonEnable}); + + jPanel1Layout.setVerticalGroup( + jPanel1Layout.createParallelGroup(javax.swing.GroupLayout.Alignment.LEADING) + .addGroup(jPanel1Layout.createSequentialGroup() + .addContainerGap() + .addComponent(buttonEnable) + .addPreferredGap(javax.swing.LayoutStyle.ComponentPlacement.RELATED) + .addComponent(buttonDisable) + .addContainerGap()) + ); + + buttonReleaseLocal.setText("Release Local"); + buttonReleaseLocal.addActionListener(new java.awt.event.ActionListener() { + public void actionPerformed(java.awt.event.ActionEvent evt) { + buttonReleaseLocalActionPerformed(evt); + } + }); + + buttonReleasePsys.setText("Release PSYS"); + buttonReleasePsys.addActionListener(new java.awt.event.ActionListener() { + public void actionPerformed(java.awt.event.ActionEvent evt) { + buttonReleasePsysActionPerformed(evt); + } + }); + + javax.swing.GroupLayout layout = new javax.swing.GroupLayout(this); + this.setLayout(layout); + layout.setHorizontalGroup( + layout.createParallelGroup(javax.swing.GroupLayout.Alignment.LEADING) + .addGroup(layout.createSequentialGroup() + .addGap(20, 20, 20) + .addGroup(layout.createParallelGroup(javax.swing.GroupLayout.Alignment.LEADING) + .addComponent(jPanel1, javax.swing.GroupLayout.PREFERRED_SIZE, javax.swing.GroupLayout.DEFAULT_SIZE, javax.swing.GroupLayout.PREFERRED_SIZE) + .addGroup(layout.createSequentialGroup() + .addGroup(layout.createParallelGroup(javax.swing.GroupLayout.Alignment.LEADING) + .addComponent(buttonMount, javax.swing.GroupLayout.PREFERRED_SIZE, 106, javax.swing.GroupLayout.PREFERRED_SIZE) + .addGroup(layout.createSequentialGroup() + .addGap(10, 10, 10) + .addComponent(checkFirst))) + .addGap(18, 18, 18) + .addGroup(layout.createParallelGroup(javax.swing.GroupLayout.Alignment.LEADING) + .addComponent(jLabel3) + .addComponent(jLabel2) + .addComponent(jLabel1)) + .addPreferredGap(javax.swing.LayoutStyle.ComponentPlacement.RELATED) + .addGroup(layout.createParallelGroup(javax.swing.GroupLayout.Alignment.LEADING) + .addComponent(spinnerSample, javax.swing.GroupLayout.PREFERRED_SIZE, javax.swing.GroupLayout.DEFAULT_SIZE, javax.swing.GroupLayout.PREFERRED_SIZE) + .addComponent(spinnerPuck, javax.swing.GroupLayout.PREFERRED_SIZE, javax.swing.GroupLayout.DEFAULT_SIZE, javax.swing.GroupLayout.PREFERRED_SIZE) + .addComponent(spinnerSegment, javax.swing.GroupLayout.PREFERRED_SIZE, javax.swing.GroupLayout.DEFAULT_SIZE, javax.swing.GroupLayout.PREFERRED_SIZE))) + .addGroup(layout.createSequentialGroup() + .addGap(5, 5, 5) + .addGroup(layout.createParallelGroup(javax.swing.GroupLayout.Alignment.LEADING, false) + .addComponent(buttonEnableAll, javax.swing.GroupLayout.DEFAULT_SIZE, javax.swing.GroupLayout.DEFAULT_SIZE, Short.MAX_VALUE) + .addGroup(layout.createSequentialGroup() + .addComponent(buttonReleasePsys) + .addPreferredGap(javax.swing.LayoutStyle.ComponentPlacement.UNRELATED) + .addComponent(buttonReleaseLocal))))) + .addContainerGap(373, Short.MAX_VALUE)) + ); + + layout.linkSize(javax.swing.SwingConstants.HORIZONTAL, new java.awt.Component[] {jLabel1, jLabel2, jLabel3}); + + layout.linkSize(javax.swing.SwingConstants.HORIZONTAL, new java.awt.Component[] {spinnerPuck, spinnerSample, spinnerSegment}); + + layout.setVerticalGroup( + layout.createParallelGroup(javax.swing.GroupLayout.Alignment.LEADING) + .addGroup(layout.createSequentialGroup() + .addGap(21, 21, 21) + .addComponent(buttonEnableAll) + .addGap(18, 18, 18) + .addGroup(layout.createParallelGroup(javax.swing.GroupLayout.Alignment.BASELINE) + .addComponent(buttonReleaseLocal) + .addComponent(buttonReleasePsys)) + .addGap(18, 18, 18) + .addComponent(jPanel1, javax.swing.GroupLayout.PREFERRED_SIZE, javax.swing.GroupLayout.DEFAULT_SIZE, javax.swing.GroupLayout.PREFERRED_SIZE) + .addGap(18, 18, 18) + .addGroup(layout.createParallelGroup(javax.swing.GroupLayout.Alignment.BASELINE) + .addComponent(buttonMount) + .addComponent(jLabel1) + .addComponent(spinnerSegment, 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(jLabel2) + .addComponent(spinnerPuck, javax.swing.GroupLayout.PREFERRED_SIZE, javax.swing.GroupLayout.DEFAULT_SIZE, javax.swing.GroupLayout.PREFERRED_SIZE) + .addComponent(checkFirst)) + .addPreferredGap(javax.swing.LayoutStyle.ComponentPlacement.RELATED) + .addGroup(layout.createParallelGroup(javax.swing.GroupLayout.Alignment.BASELINE) + .addComponent(jLabel3) + .addComponent(spinnerSample, javax.swing.GroupLayout.PREFERRED_SIZE, javax.swing.GroupLayout.DEFAULT_SIZE, javax.swing.GroupLayout.PREFERRED_SIZE)) + .addGap(24, 24, 24)) + ); + }// //GEN-END:initComponents + + private void buttonEnableAllActionPerformed(java.awt.event.ActionEvent evt) {//GEN-FIRST:event_buttonEnableAllActionPerformed + execute("enable_power()"); + }//GEN-LAST:event_buttonEnableAllActionPerformed + + private void buttonMountActionPerformed(java.awt.event.ActionEvent evt) {//GEN-FIRST:event_buttonMountActionPerformed + String cmd = checkFirst.isSelected() ? "firstmount" : "mount"; + execute("robot.start_task('" + cmd + "'," + spinnerSegment.getValue() + "," + spinnerPuck.getValue() + "," + spinnerSample.getValue() + ")"); + checkFirst.setSelected(false); + + }//GEN-LAST:event_buttonMountActionPerformed + + private void buttonEnableActionPerformed(java.awt.event.ActionEvent evt) {//GEN-FIRST:event_buttonEnableActionPerformed + execute("robot.enable()"); + }//GEN-LAST:event_buttonEnableActionPerformed + + private void buttonDisableActionPerformed(java.awt.event.ActionEvent evt) {//GEN-FIRST:event_buttonDisableActionPerformed + execute("robot.disable()"); + }//GEN-LAST:event_buttonDisableActionPerformed + + private void buttonReleaseLocalActionPerformed(java.awt.event.ActionEvent evt) {//GEN-FIRST:event_buttonReleaseLocalActionPerformed + execute("release_local()"); + }//GEN-LAST:event_buttonReleaseLocalActionPerformed + + private void buttonReleasePsysActionPerformed(java.awt.event.ActionEvent evt) {//GEN-FIRST:event_buttonReleasePsysActionPerformed + execute("release_psys()"); + }//GEN-LAST:event_buttonReleasePsysActionPerformed + + // Variables declaration - do not modify//GEN-BEGIN:variables + private javax.swing.JButton buttonDisable; + private javax.swing.JButton buttonEnable; + private javax.swing.JButton buttonEnableAll; + private javax.swing.JButton buttonMount; + private javax.swing.JButton buttonReleaseLocal; + private javax.swing.JButton buttonReleasePsys; + private javax.swing.JCheckBox checkFirst; + private javax.swing.JLabel jLabel1; + private javax.swing.JLabel jLabel2; + private javax.swing.JLabel jLabel3; + private javax.swing.JPanel jPanel1; + private javax.swing.JSpinner spinnerPuck; + private javax.swing.JSpinner spinnerSample; + private javax.swing.JSpinner spinnerSegment; + // End of variables declaration//GEN-END:variables +} diff --git a/plugins/MXSC-1.9.0.jar b/plugins/MXSC-1.9.0.jar index ea9f5b2..e128542 100644 Binary files a/plugins/MXSC-1.9.0.jar and b/plugins/MXSC-1.9.0.jar differ diff --git a/script/calibration/BinarySearchXZ.py b/script/calibration/BinarySearchYZ.py similarity index 60% rename from script/calibration/BinarySearchXZ.py rename to script/calibration/BinarySearchYZ.py index c3c5106..42db379 100644 --- a/script/calibration/BinarySearchXZ.py +++ b/script/calibration/BinarySearchYZ.py @@ -15,15 +15,15 @@ move_to_laser() robot.set_motors_enabled(True) -current_x = robot_x.getPosition() +current_y = robot_y.getPosition() current_z = robot_z.getPosition() -r = bsearch([robot_x, robot_z], laser_distance,[RANGE[0], RANGE[0]], [RANGE[1], RANGE[1]], [STEP_SIZE,STEP_SIZE], relative = True, maximum=True, strategy = STRATEGY, latency = LATENCY, title = "Binary Search XZ") +r = bsearch([robot_y, robot_z], laser_distance,[RANGE[0], RANGE[0]], [RANGE[1], RANGE[1]], [STEP_SIZE,STEP_SIZE], relative = True, maximum=True, strategy = STRATEGY, latency = LATENCY, title = "Binary Search YZ") print r.print() -opt_x, opt_z= r.getOptimalPosition() -offset_x, offset_z = opt_x - current_x, opt_z - current_z +opt_y, opt_z= r.getOptimalPosition() +offset_y, offset_z = opt_y - current_y, opt_z - current_z -print "offset_x: ", offset_x, " offset_z: ", offset_z \ No newline at end of file +print "offset_y: ", offset_y, " offset_z: ", offset_z \ No newline at end of file diff --git a/script/calibration/ScanRZ.py b/script/calibration/ScanRZ.py index acb04c5..9cd8de8 100644 --- a/script/calibration/ScanRZ.py +++ b/script/calibration/ScanRZ.py @@ -1,9 +1,13 @@ import plotutils from mathutils import fit_gaussian, Gaussian -RANGE = [-20.0,20.0] -STEP = 1.0 +RANGE = [180.0,-180.0] +STEP = 5.0 LATENCY = 0.005 +RELATIVE = False + + +robot.set_tool(TOOL_DEFAULT) robot.enable() move_to_laser() @@ -13,8 +17,9 @@ move_to_laser() -robot.set_motors_enabled(True) -ret = lscan(robot_rz, ue.readable, RANGE[0], RANGE[1], STEP, latency = LATENCY, relative = True, range = "auto") + +robot.set_joint_motors_enabled(True) +ret = lscan(robot_rz, ue.readable, RANGE[0], RANGE[1], STEP, latency = LATENCY, relative = RELATIVE, range = "auto", title = "Scan2") diff --git a/script/calibration/ScanX.py b/script/calibration/ScanX.py deleted file mode 100644 index 71eecb6..0000000 --- a/script/calibration/ScanX.py +++ /dev/null @@ -1,37 +0,0 @@ -import plotutils -from mathutils import fit_gaussian, Gaussian - -robot.enable() -move_to_laser() - - -RANGE = [-1.5, 1.5] -STEP = 0.025 -Z_OFFSET = -1.0 -LATENCY = 0.005 - -robot.set_motors_enabled(True) -current_positon = robot_x.getPosition() -robot_z.moveRel(Z_OFFSET) -ret = lscan(robot_x, ue.readable, RANGE[0], RANGE[1], STEP, latency = LATENCY, relative = True) - -d = ret.getReadable(0) - -first_index = -1 -last_index = -1 -for i in range(len(d)): - if not math.isnan(d[i]): - if first_index<0: - first_index = i - last_index = i - -if first_index == -1 or last_index < first_index: - raise Exception("Invalid X detection") - - -center_index = (first_index + last_index)/2 -center_positon = ret.getPositions(0)[center_index] -center_offset = center_positon-current_positon - - -print "X offset = ", center_offset \ No newline at end of file diff --git a/script/calibration/ScanY.py b/script/calibration/ScanY.py new file mode 100644 index 0000000..7e25fc9 --- /dev/null +++ b/script/calibration/ScanY.py @@ -0,0 +1,81 @@ +import plotutils +from mathutils import fit_gaussian, Gaussian + + +d = robot.get_distance_to_pnt("pLaser") +if d<0: + raise Exception ("Error calculating distance to laser: " + str(d)) + +if d>20: + raise Exception ("Should be near the laser position to perform the scan") + +RANGE = [-1.5, 1.5] +STEP = 0.02 +Z_OFFSET = 0 #-1.0 +LATENCY = 0.025 +BORDER_SIZE = 0.15 + +robot.enable() +robot.set_motors_enabled(True) +current_positon = robot_y.getPosition() +robot_z.moveRel(Z_OFFSET) + +robot.setPolling(25) +try: + ret = lscan(robot_y, ue.readable, RANGE[0], RANGE[1], STEP, latency = LATENCY, relative = True) +finally: + robot.setPolling(DEFAULT_ROBOT_POLLING) + +d = ret.getReadable(0) + +first_index = -1 +last_index = -1 +for i in range(len(d)): + if not math.isnan(d[i]): + if first_index<0: + first_index = i + last_index = i + +if first_index == -1 or last_index < first_index: + raise Exception("Invalid X detection") + + +remove = int(max(BORDER_SIZE, STEP) / STEP) + +_range = [first_index+remove, last_index-remove] +if _range[1] <= _range[0]: + raise Exception("invalid range: " + str(_range)) + + +center_index = int((_range[0] + _range[1])/2) +center_positon = ret.getPositions(0)[center_index] + + + + +y = ret.getReadable(0)[_range[0] : _range[1]] +x = ret.getPositions(0)[_range[0]: _range[1]] +#x = enforce_monotonic(x) +#(normalization, mean_val, sigma) = fit_gaussian([-v for v in y], x) + + +closest_y = x[y.indexOf(min(y))] +closest_x = y[y.indexOf(min(y))] + + + +if closest_y is None or closest_y <= ret.getPositions(0)[first_index] or closest_y >= ret.getPositions(0)[last_index]: + raise Exception("Invalid Fit") + + + +center_offset = center_positon-closest_y +#center_offset = current_positon-closest_y + + + +p=get_plots()[0] +p.addMarker(closest_y, p.AxisId.X, str(closest_y), Color.GREEN) + +robot.set_motors_enabled(True) +robot_y.move(closest_y) diff --git a/script/calibration/ScanXZ.py b/script/calibration/ScanYZ.py similarity index 78% rename from script/calibration/ScanXZ.py rename to script/calibration/ScanYZ.py index e5df77d..bee63e6 100644 --- a/script/calibration/ScanXZ.py +++ b/script/calibration/ScanYZ.py @@ -21,24 +21,24 @@ STEP_SIZE = 0.1 robot.enable() move_to_laser() -step_x = STEP_SIZE +step_y = STEP_SIZE step_z = STEP_SIZE -range_x = [RANGE[0], RANGE[1]] +range_y = [RANGE[0], RANGE[1]] range_z = [RANGE[0], RANGE[1]] robot.set_motors_enabled(True) -current_x = robot_x.getPosition() +current_y = robot_y.getPosition() current_z = robot_z.getPosition() -print "Current pos x,z" , current_x, ",", current_z -ret = ascan([robot_x, robot_z], ue.readable, [range_x[0], range_z[0]], [range_x[1], range_z[1]], [step_x,step_z], latency = LATENCY, relative = True , zigzag=False, title = "Scan XY") +print "Current pos y,z" , current_y, ",", current_z +ret = ascan([robot_y, robot_z], ue.readable, [range_y[0], range_z[0]], [range_y[1], range_z[1]], [step_y,step_z], latency = LATENCY, relative = True , zigzag=False, title = "Scan XY") data = ret.getData(0)[0] #plot(Convert.transpose(data), title="Data") integ = [] for x in data: integ.append(sum( [ (0.0 if (math.isnan(y)) else y) for y in x])) -xdata= frange(range_x[0], range_x[1], step_x , False, True) +xdata= frange(range_y[0], range_y[1], step_y , False, True) p = plot(integ, title = "Fit", xdata=xdata)[0] @@ -49,18 +49,18 @@ try: except: raise Exception("Invalid Fit") gaussian = Gaussian(normalization, mean_val, sigma) -xdata= frange(range_x[0], range_x[1], step_x/100.0 , False, True) +xdata= frange(range_y[0], range_y[1], step_y/100.0 , False, True) plot_function(p, gaussian, "Fit", xdata, show_points = False, show_lines = True, color = Color.BLUE) #So if abs(mean_val - max_x) > 1.0: - raise Exception("Invalid X detection") -x_offset = mean_val -center_x = current_x + x_offset + raise Exception("Invalid Y detection") +y_offset = mean_val +center_y = current_y + y_offset -print "X offset = ", x_offset +print "Y offset = ", y_offset -robot_x.move(center_x) +robot_y.move(center_y) if SINGLE_PASS: z_scan_data = data[max_x_index] else: diff --git a/script/calibration/ToolCalibration.py b/script/calibration/ToolCalibration.py index 9b7fb31..602403b 100644 --- a/script/calibration/ToolCalibration.py +++ b/script/calibration/ToolCalibration.py @@ -15,17 +15,17 @@ move_to_laser() robot.align() -run("calibration/ScanXZ") +run("calibration/ScanYZ") robot.set_motors_enabled(True) -first_x = robot_x.take() +first_y = robot_y.take() first_z = robot_z.take() first_y = ue.take() first_j6 = robot_j6.take() if first_y is None: - raise Exception("Invalid XZ scan values in first scan") + raise Exception("Invalid YZ scan values in first scan") robot.set_joint_motors_enabled(True) @@ -36,11 +36,11 @@ else: robot.set_motors_enabled(True) -run("calibration/ScanXZ") +run("calibration/ScanYZ") robot.set_motors_enabled(True) -second_x = robot_x.take() +second_y = robot_y.take() second_z = robot_z.take() second_y = ue.take() second_j6 = robot_j6.take() @@ -51,10 +51,10 @@ if second_y is None: #Updates the tool xoff = (first_x - second_x)/2 yoff = (first_y - second_y)/2 -robot.get_tool_trsf(robot.tool) -t[0]=xoff #TODO: Why signal? -t[1]=-yoff #TODO: Why signal? -robot.set_tool_trsf(t, robot.tool) +t=robot.get_tool_trsf(TOOL_DEFAULT) +t[0]=xoff +t[1]=-yoff +robot.set_tool_trsf(t, TOOL_DEFAULT) diff --git a/script/calibration/ToolCalibration2.py b/script/calibration/ToolCalibration2.py new file mode 100644 index 0000000..d146173 --- /dev/null +++ b/script/calibration/ToolCalibration2.py @@ -0,0 +1,75 @@ +import plotutils +from mathutils import fit_gaussian, Gaussian + + +robot.assert_tool(TOOL_CALIBRATION) +robot.enable() +robot.set_motors_enabled(True) +robot.set_joint_motors_enabled(True) +move_to_laser() + + +initial_pos = robot.get_cartesian_pos() + +robot.enable() +move_to_laser() + +#robot.align() + + +run("calibration/ScanY") + +pos1 = robot.get_cartesian_pos() +x1, y1 = closest_x, closest_y + +print "Closest 1: ", [x1, y1] +print "Position 1: ", pos1 + + +pj6 = robot_j6.position +if pj6>0: + robot_j6.move(pj6 - 180.0) +else: + robot_j6.move(pj6 + 180.0) + + +run("calibration/ScanY") + + +x2, y2 = closest_x, closest_y + +print "Closest 2: ", [x2, y2] + + + +off_x = x1 - x2 + +#robot.set_motors_enabled(True) +#robot_x.moveRel(off_x, -1) + +#For composing cannot use tcp_p, need another auxiliary point. tcp_t is also destroyed. +robot.set_pnt(robot.get_cartesian_pos(), "pTemp") +robot.set_trsf([off_x, 0,0,0,0,0]) +c=robot.compose("pTemp", "fTable", "tcp_t" ) +robot.set_pnt(c, "pTemp") +robot.movel("pTemp", TOOL_CALIBRATION, DESC_SCAN, sync=True) + + +pos2 = robot.get_cartesian_pos() +print pos2 + +print "Position 2: ", pos2 + + + +#Updates the tool +xoff = (pos2[0]-pos1[0])/2 +yoff = (pos2[1]-pos1[1])/2 + +print "Offset: ", [xoff, yoff] + +t=robot.get_tool_trsf(TOOL_DEFAULT) +t[0]=-xoff +t[1]=yoff +robot.set_tool_trsf(t, TOOL_DEFAULT) + diff --git a/script/devices/Hexiposi.py b/script/devices/Hexiposi.py index 5374af4..fdb4567 100644 --- a/script/devices/Hexiposi.py +++ b/script/devices/Hexiposi.py @@ -4,15 +4,17 @@ import json class Hexiposi(DiscretePositionerBase): def __init__(self, name, url): - DiscretePositionerBase.__init__(self, name, ["1","2","3","4","5","6"]) + DiscretePositionerBase.__init__(self, name, ["A","B","C","D","E","F"]) if not url.startswith("http://"): url = "http://" + url if not url.endswith("/"): url = url + "/" - self.url = url - self.setState(State.Ready) - self.val = self.doReadReadback() + self.url = url + def doInitialize(self): + super(Hexiposi, self).doInitialize() + self.val = self.doReadReadback() + def get_response(self, response): if (response.status_code!=200): raise Exception (response.text) @@ -28,16 +30,15 @@ class Hexiposi(DiscretePositionerBase): self.pos = self.status["position"] self.moving = self.status["moving"] self.offset = self.status["offset"] - self.dpos = self.status["discretePosition"] - if self.dpos == 1: self.rback = 1 - elif self.dpos == 2: self.rback = 2 - elif self.dpos == 4: self.rback = 3 - elif self.dpos == 8: self.rback = 4 - elif self.dpos == 16: self.rback = 5 - elif self.dpos == 32: self.rback = 6 - else: self.rback = None - self.rbackstr = self.UNKNOWN_POSITION if ((self.rback is None) or (self.homed==False)) else str(self.rback) - + self.dpos = self.status["discretePosition"] + if (self.homed==False): self.rback = self.UNKNOWN_POSITION + elif self.dpos == 1: self.rback = "A" + elif self.dpos == 2: self.rback = "B" + elif self.dpos == 4: self.rback = "C" + elif self.dpos == 8: self.rback = "D" + elif self.dpos == 16: self.rback = "E" + elif self.dpos == 32: self.rback = "F" + else: self.rback = self.UNKNOWN_POSITION return self.status def move_pos(self, pos): @@ -65,10 +66,12 @@ class Hexiposi(DiscretePositionerBase): def doReadReadback(self): self.get_status() - return self.rbackstr + return self.rback def doWrite(self, val): - val = int(val) + val = ord(val) - ord('A') +1 + if val<1 or val>6: + raise Exception("Invalid value: " + str(val)) moving = val != self.val self.val = val self.move_pos(self.val) diff --git a/script/devices/RobotMotors.py b/script/devices/RobotMotors.py index 1057ef1..07ba862 100644 --- a/script/devices/RobotMotors.py +++ b/script/devices/RobotMotors.py @@ -21,7 +21,7 @@ class RobotCartesianMotor (PositionerBase): def doWrite(self, value): if self.robot.cartesian_destination is not None: - print "Move " + ROBOT_MOTORS[self.index] + " to " + str(value) + #print "Move " + ROBOT_MOTORS[self.index] + " to " + str(value) self.robot.cartesian_destination[self.index] = float(value) self.robot.set_pnt(robot.cartesian_destination , "tcp_p") self.robot.movel("tcp_p", self.robot.tool , DESC_SCAN) @@ -49,7 +49,7 @@ class RobotJointMotor (PositionerBase): return self.setpoint def doWrite(self, value): - print "Move " + ROBOT_JOINT_MOTORS[self.index] + " to " + str(value) + #print "Move " + ROBOT_JOINT_MOTORS[self.index] + " to " + str(value) self.setpoint = value joint = self.robot.herej() joint[self.index] = value diff --git a/script/devices/RobotSC.py b/script/devices/RobotSC.py index 95bd16d..b50c64a 100644 --- a/script/devices/RobotSC.py +++ b/script/devices/RobotSC.py @@ -8,16 +8,21 @@ DESC_SCAN = "mScan" DESC_DEFAULT = DESC_FAST +DEFAULT_ROBOT_POLLING = 500 + + run("devices/RobotTCP") -simulation = True +simulation = False + joint_forces = False class RobotSC(RobotTCP): def __init__(self, name, server, timeout = 1000, retries = 1): - RobotTCP.__init__(self, name, server, timeout, retries) + RobotTCP.__init__(self, name, server, timeout, retries) + self.setPolling(DEFAULT_ROBOT_POLLING) def mount(self, puck, sample): return self.execute('mount',segment, puck, sample) @@ -66,13 +71,6 @@ if simulation: else: add_device(RobotSC("robot", "129.129.110.100:1000"), force = True) -robot.high_level_tasks = ["mount", "firstmount"] -robot.set_tool(TOOL_CALIBRATION) -robot.setPolling(500) - -robot.set_motors_enabled(True) -robot.set_joint_motors_enabled(True) - #robot.set_monitor_speed(20) diff --git a/script/devices/RobotTCP.py b/script/devices/RobotTCP.py index bbd7e28..bcf7a53 100644 --- a/script/devices/RobotTCP.py +++ b/script/devices/RobotTCP.py @@ -368,7 +368,7 @@ class RobotTCP(TcpDevice, Stoppable): def distance_p(self, pnt1, pnt2): return self.eval_float("distance(" + pnt1 + ", " + pnt2 + ")") - def compose(self, pnt, frame, trsf): + def compose(self, pnt, frame = FRAME_DEFAULT, trsf = "tcp_t"): return self.eval_pnt("compose(" + pnt + ", " + frame + ", " + trsf + ")") def here(self, tool, frame): diff --git a/script/imgproc/CameraCalibration.py b/script/imgproc/CameraCalibration.py new file mode 100644 index 0000000..02e7d9d --- /dev/null +++ b/script/imgproc/CameraCalibration.py @@ -0,0 +1,71 @@ +import ch.psi.pshell.device.Camera as Camera +import ch.psi.pshell.imaging.RendererMode as RendererMode +from ch.psi.pshell.imaging.Overlays import * + +""" +img.camera.setColorMode(Camera.ColorMode.Mono) +img.camera.setDataType(Camera.DataType.UInt8) +img.camera.setGrabMode(Camera.GrabMode.Continuous) +img.camera.setTriggerMode(Camera.TriggerMode.Fixed_Rate) +img.camera.setExposure(50.00) +img.camera.setAcquirePeriod(200.00) +img.camera.setGain(0.0) +img.config.rotationCrop=True + + +""" +#img.camera.setROI(200, 0,1200,1200) + +img.camera.setROI(0, 0,1600,1200) +img.config.rotation=0 +img.config.roiX,img.config.roiY, img.config.roiWidth,img.config.roiHeight =0,0,-1,-1 +img.config.setCalibration(None) +img.camera.stop() +img.camera.start() + +#img.camera.setROI(300, 200,1000,1000) +#img.config.rotation=17 +#img.config.roiX,img.config.roiY, img.config.roiWidth,img.config.roiHeight = 50,50,900,900 + +p = show_panel(img) +p.setMode(RendererMode.Fit) +ov_text = Text(Pen(java.awt.Color.GREEN.darker()), "", java.awt.Font("Verdana", java.awt.Font.PLAIN, 24), java.awt.Point(20,20)) +ov_text.setFixed(True) + + + +p.addOverlay(ov_text) +try: + ov_text.update("Click on upper reference...") + p1 = p.waitClick(60000) + print p1 + ov_text.update("Click on left reference...") + p2 = p.waitClick(60000) + print p2 + ov_text.update("Click on right reference...") + p3 = p.waitClick(60000) + print p3 + + x, y, z = p1.x+p1.y*1j, p2.x+p2.y*1j, p3.x+p3.y*1j + w = z-x + w /= y-x + c = (x-y)*(w-abs(w)**2)/2j/w.imag-x + cx, cy, r = -c.real, -c.imag, abs(c+x) + a = math.degrees(math.atan((cx-p1.x)/(p1.y-cy))) + + print cx, cy, r, a + + #img.camera.setROI(int((1600-cx)/2),int((1200-cy)/2),1000,1000) + img.camera.setROI(int(cx-r),int(cy-r),int(2*r),int(2*r)) + img.config.rotation=-a + #remove rotation border + d=int(r/11) + img.config.roiX,img.config.roiY, img.config.roiWidth,img.config.roiHeight =d,d, int(2*r-2*d), int(2*r-2*d) + #img.config.setCalibration(None) + img.camera.stop() + img.camera.start() + +finally: + p.removeOverlay(ov_text) + + \ No newline at end of file diff --git a/script/local.py b/script/local.py index 5d8ea73..61f6a15 100644 --- a/script/local.py +++ b/script/local.py @@ -37,20 +37,47 @@ run("tools/Math") # Device initialization ################################################################################################### -import ch.psi.pshell.device.Camera as Camera -img.camera.setColorMode(Camera.ColorMode.Mono) -img.camera.setDataType(Camera.DataType.UInt8) -img.camera.setGrabMode(Camera.GrabMode.Continuous) -img.camera.setTriggerMode(Camera.TriggerMode.Fixed_Rate) -img.camera.setExposure(50.00) -img.camera.setAcquirePeriod(200.00) -img.camera.setGain(0.0) +try: + release_local_safety.write(False) + release_psys_safety.write(False) +except: + print >> sys.stderr, traceback.format_exc() +try: + hexiposi.polling=500 +except: + print >> sys.stderr, traceback.format_exc() -release_local_safety.write(False) -release_psys_safety.write(False) +try: + robot.setPolling(DEFAULT_ROBOT_POLLING) + robot.high_level_tasks = ["mount", "firstmount"] + robot.set_tool(TOOL_CALIBRATION) + robot.set_motors_enabled(True) + robot.set_joint_motors_enabled(True) +except: + print >> sys.stderr, traceback.format_exc() + +try: + import ch.psi.pshell.device.Camera as Camera + img.camera.setColorMode(Camera.ColorMode.Mono) + img.camera.setDataType(Camera.DataType.UInt8) + img.camera.setGrabMode(Camera.GrabMode.Continuous) + img.camera.setTriggerMode(Camera.TriggerMode.Fixed_Rate) + img.camera.setExposure(50.00) + img.camera.setAcquirePeriod(200.00) + img.camera.setGain(0.0) + #img.camera.setROI(200, 0,1200,1200) + img.camera.setROI(300, 200,1000,1000) + + img.config.rotation=17 + img.config.rotationCrop=True + img.config.roiX,img.config.roiY, img.config.roiWidth,img.config.roiHeight = 50,50,900,900 + + img.camera.stop() + img.camera.start() +except: + print >> sys.stderr, traceback.format_exc() -hexiposi.polling=500 ################################################################################################### diff --git a/script/tools/Math.py b/script/tools/Math.py index dc88540..c76ce33 100644 --- a/script/tools/Math.py +++ b/script/tools/Math.py @@ -57,4 +57,12 @@ def fit(ydata, xdata = None, draw_plot = True): if draw_plot: p.addMarker(max_x, None, "Max="+str(round(max_x,4)), Color.GRAY) #print "Invalid gaussian fit: " + str(mean) - return (None, None, None) \ No newline at end of file + return (None, None, None) + + +def enforce_monotonic(x): + epsilon = 1e-8 + for i in range(len(x)-1): + if x[i+1]<=x[i]: + x[i+1] = x[i]+ epsilon + return x \ No newline at end of file