Merge pull request openpnp#1066 from markmaker/feature/advanced-motio…

…n-control--update-2

Feature/Advanced Motion Control  - Update 2

src/main/java/org/openpnp/gui/components/CameraView.java

@@ -1615,9 +1615,10 @@ public void componentResized(ComponentEvent e) {
     private MouseWheelListener mouseWheelListener = new MouseWheelListener() {
         @Override
         public void mouseWheelMoved(MouseWheelEvent e) {
-            zoom -= e.getPreciseWheelRotation() * Math.max(zoomIncPerMouseWheelTick,
+            double zoomInc = Math.max(zoomIncPerMouseWheelTick,
                     // When best-scale is selected, we can only zoom by 1.0 or faster.
-                    renderingQuality == RenderingQuality.BestScale ? 1.0 : 0); 
+                    renderingQuality == RenderingQuality.BestScale ? 1.0 : 0);
+            zoom = (Math.round(zoom/zoomInc) - e.getPreciseWheelRotation()) * zoomInc; 
             zoom = Math.max(zoom, 1.0d);
             zoom = Math.min(zoom, 100d);
             calculateScalingData();

src/main/java/org/openpnp/machine/neoden4/NeoDen4Driver.java

@@ -385,7 +385,7 @@ public void setGlobalOffsets(ReferenceMachine machine, AxesLocation location)
     }
 
     @Override
-    public AxesLocation getMomentaryLocation(long timeout) throws Exception {
+    public AxesLocation getReportedLocation(long timeout) throws Exception {
         // TODO: if the driver can do it, please implement. 
         throw new Exception("Not supported in this driver");
     }

src/main/java/org/openpnp/machine/reference/HttpActuator.java

@@ -47,7 +47,7 @@ public HttpActuator() {}
     public void actuate(boolean on) throws Exception {
         Logger.debug("{}.actuate({})", getName(), on);
         if (isCoordinatedBeforeActuate()) {
-            coordinateWithMachine();
+            coordinateWithMachine(false);
         }
         // getDriver().actuate(this, on);
         URL obj = null;
@@ -84,7 +84,7 @@ else if (!this.on && on) {
         this.on = on;
 
         if (isCoordinatedAfterActuate()) {
-            coordinateWithMachine();
+            coordinateWithMachine(true);
         }
         getMachine().fireMachineHeadActivity(head);
     }

src/main/java/org/openpnp/machine/reference/ReferenceActuator.java

@@ -80,11 +80,11 @@ public void setIndex(int index) {
     public void actuate(boolean on) throws Exception {
         Logger.debug("{}.actuate({})", getName(), on);
         if (isCoordinatedBeforeActuate()) {
-            coordinateWithMachine();
+            coordinateWithMachine(false);
         }
         getDriver().actuate(this, on);
         if (isCoordinatedAfterActuate()) {
-            coordinateWithMachine();
+            coordinateWithMachine(true);
         }
         getMachine().fireMachineHeadActivity(head);
     }
@@ -98,11 +98,11 @@ public Location getCameraToolCalibratedOffset(Camera camera) {
     public void actuate(double value) throws Exception {
         Logger.debug("{}.actuate({})", getName(), value);
         if (isCoordinatedBeforeActuate()) {
-            coordinateWithMachine();
+            coordinateWithMachine(false);
         }
         getDriver().actuate(this, value);
         if (isCoordinatedAfterActuate()) {
-            coordinateWithMachine();
+            coordinateWithMachine(true);
         }
         getMachine().fireMachineHeadActivity(head);
     }
@@ -111,24 +111,24 @@ public void actuate(double value) throws Exception {
     public void actuate(String value) throws Exception {
         Logger.debug("{}.actuate({})", getName(), value);
         if (isCoordinatedBeforeActuate()) {
-            coordinateWithMachine();
+            coordinateWithMachine(false);
         }
         getDriver().actuate(this, value);
         if (isCoordinatedAfterActuate()) {
-            coordinateWithMachine();
+            coordinateWithMachine(true);
         }
         getMachine().fireMachineHeadActivity(head);
     }
 
     @Override
     public String read() throws Exception {
         if (isCoordinatedBeforeRead()) {
-            coordinateWithMachine();
+            coordinateWithMachine(false);
         }
         String value = getDriver().actuatorRead(this);
         Logger.debug("{}.read(): {}", getName(), value);
         if (isCoordinatedAfterActuate()) {
-            coordinateWithMachine();
+            coordinateWithMachine(true);
         }
         getMachine().fireMachineHeadActivity(head);
         return value;
@@ -137,7 +137,7 @@ public String read() throws Exception {
     @Override
     public String read(double parameter) throws Exception {
         if (isCoordinatedBeforeRead()) {
-            coordinateWithMachine();
+            coordinateWithMachine(false);
         }
         String value = getDriver().actuatorRead(this, parameter);
         Logger.debug("{}.readWithDouble({}): {}", getName(), parameter, value);

src/main/java/org/openpnp/machine/reference/ScriptActuator.java

@@ -26,26 +26,26 @@ private void execute(Map<String, Object> globals) throws Exception {
     @Override
     public void actuate(boolean on) throws Exception {
         if (isCoordinatedBeforeActuate()) {
-            coordinateWithMachine();
+            coordinateWithMachine(false);
         }
         Map<String, Object> globals = new HashMap<>();
         globals.put("actuateBoolean", on);
         this.execute(globals);
         if (isCoordinatedAfterActuate()) {
-            coordinateWithMachine();
+            coordinateWithMachine(true);
         }
     }
 
     @Override
     public void actuate(double value) throws Exception {
         if (isCoordinatedBeforeActuate()) {
-            coordinateWithMachine();
+            coordinateWithMachine(false);
         }
         Map<String, Object> globals = new HashMap<>();
         globals.put("actuateDouble", value);
         this.execute(globals);
         if (isCoordinatedAfterActuate()) {
-            coordinateWithMachine();
+            coordinateWithMachine(true);
         }
     }
 

src/main/java/org/openpnp/machine/reference/axis/wizards/ReferenceControllerAxisConfigurationWizard.java

@@ -544,6 +544,17 @@ protected void adaptDialog() {
         btnCaptureSoftLimitHigh.setVisible(!showRotationSettings);
         btnPositionSoftLimitLow.setVisible(!showRotationSettings);
         btnPositionSoftLimitHigh.setVisible(!showRotationSettings);
+
+        lblSafeZoneLow.setVisible(!showRotationSettings);
+        lblSafeZoneHigh.setVisible(!showRotationSettings);
+        safeZoneLow.setVisible(!showRotationSettings);
+        safeZoneHigh.setVisible(!showRotationSettings);
+        safeZoneLowEnabled.setVisible(!showRotationSettings);
+        safeZoneHighEnabled.setVisible(!showRotationSettings);
+        btnCaptureSafeZoneLow.setVisible(!showRotationSettings);
+        btnCaptureSafeZoneHigh.setVisible(!showRotationSettings);
+        btnPositionSafeZoneLow.setVisible(!showRotationSettings);
+        btnPositionSafeZoneHigh.setVisible(!showRotationSettings);
     }
 
     @Override

src/main/java/org/openpnp/machine/reference/driver/GcodeAsyncDriver.java

@@ -297,8 +297,8 @@ public void sendCommand(String command, long timeout) throws Exception {
     public void waitForCompletion(ReferenceHeadMountable hm, 
             CompletionType completionType) throws Exception {
         waitedForCommands = true;
-        if (completionType != CompletionType.WaitForStillstandIndefinitely 
-                && !isMotionPending()) {
+        if (!(completionType.isUnconditionalCoordination() 
+                || isMotionPending())) {
             return;
         }
         // Issue the M400 in the super class.
@@ -307,7 +307,7 @@ public void waitForCompletion(ReferenceHeadMountable hm,
         if (completionType.isWaitingForDrivers()) {
             // Explicitly wait for the controller's acknowledgment here. 
             // This is signaled with a position report.
-            getMomentaryLocation(
+            getReportedLocation(
                 completionType == CompletionType.WaitForStillstandIndefinitely ?
                 -1 : getTimeoutAtMachineSpeed());
         }

src/main/java/org/openpnp/machine/reference/driver/GcodeDriver.java

@@ -278,7 +278,7 @@ public String toString() {
 
     protected AtomicLong receivedConfirmations = new AtomicLong();
     protected Line errorResponse;
-    private AxesLocation lastMomentaryLocation;
+    private AxesLocation lastReportedLocation;
     private double lastMomentaryTime;
     private boolean motionPending;
 
@@ -483,7 +483,7 @@ public void setGlobalOffsets(ReferenceMachine machine, AxesLocation axesLocation
 
 
     @Override
-    public AxesLocation getMomentaryLocation(long timeout) throws Exception {
+    public AxesLocation getReportedLocation(long timeout) throws Exception {
         String command = getCommand(null, CommandType.GET_POSITION_COMMAND);
         if (command == null) {
             throw new Exception(getName()+" configuration error: missing GET_POSITION_COMMAND.");
@@ -493,16 +493,16 @@ public AxesLocation getMomentaryLocation(long timeout) throws Exception {
         }
 
         // Reset the last position report.
-        lastMomentaryLocation = null;
+        lastReportedLocation = null;
         sendGcode(command, -1);
         // Blocking queue?
         long t1 = System.currentTimeMillis() + ((timeout == -1) ?
                 infinityTimeoutMilliseconds
                 : timeout);
         do { 
-            if (lastMomentaryLocation != null) {
-                Logger.trace("Got lastMomentaryLocation");
-                return lastMomentaryLocation;
+            if (lastReportedLocation != null) {
+                Logger.trace("Got lastReportedLocation");
+                return lastReportedLocation;
             }
             // TODO: sync with response queue? How?
             Thread.yield();
@@ -676,7 +676,8 @@ public boolean isMotionPending() {
     @Override
     public void waitForCompletion(ReferenceHeadMountable hm, 
             CompletionType completionType) throws Exception {
-        if (!motionPending) {
+        if (!(completionType.isUnconditionalCoordination() 
+                || isMotionPending())) {
             return;
         }
         String command = getCommand(hm, CommandType.MOVE_TO_COMPLETE_COMMAND);
@@ -1127,12 +1128,12 @@ protected boolean processPositionReport(Line line) {
             }
         }
         // Store the latest momentary position.
-        lastMomentaryLocation = position;
+        lastReportedLocation = position;
         lastMomentaryTime = line.getTransmissionTime();
 
         if (motionPending) {
-            Logger.warn("Position report cannot be processed when motion might still be pending. Waiting for completion missing/check Machine Coordination on Actuators.", 
-                    lastMomentaryLocation);
+            Logger.warn("Position report cannot be processed when motion might still be pending. Missing Machine Coordination on Actuators?", 
+                    lastReportedLocation);
         }
         else {
             // Store the actual driver location. This is used to re-sync OpenPnP to the actual controller 

src/main/java/org/openpnp/machine/reference/driver/NullDriver.java

@@ -138,7 +138,7 @@ public void waitForCompletion(ReferenceHeadMountable hm, CompletionType completi
     }
 
     @Override
-    public AxesLocation getMomentaryLocation(long timeout) throws Exception {
+    public AxesLocation getReportedLocation(long timeout) throws Exception {
         ReferenceMachine machine = (ReferenceMachine) Configuration.get().getMachine();
         double now = NanosecondTime.getRuntimeSeconds();
         Motion motion = machine.getMotionPlanner()

src/main/java/org/openpnp/machine/reference/driver/ReferenceAdvancedMotionPlanner.java

@@ -91,6 +91,7 @@ public class ReferenceAdvancedMotionPlanner extends AbstractMotionPlanner {
     private Double recordingMoveTimePlanned;
     private boolean interpolationFailed;
     private boolean recordingInterpolationFailed;
+    private AxesLocation recordingLocation0;
 
     public boolean isAllowContinuousMotion() {
         return allowContinuousMotion;
@@ -367,13 +368,14 @@ protected boolean rearrangeGraph() {
     protected void recordDiagnostics(Motion plannedMotion, MoveToCommand moveToCommand, Driver driver) {
         super.recordDiagnostics(plannedMotion, moveToCommand, driver);
         if (diagnosticsEnabled) {
-            final double tick = 1e-9;
+            final double tick = 1e-8;
             final double dt = Math.min(0.001, plannedMotion.getTime()/1000 + 1e-6); // 1ms or 1/1000 of whole motion 
             if (recordingMotionGraph == null) {
                 startNewMotionGraph();
                 recordingT0 = plannedMotion.getPlannedTime0();
                 recordingT = 0;
                 recordingInterpolationFailed = false;
+                recordingLocation0 = plannedMotion.getLocation0();
             }
             AxesLocation segment = moveToCommand.getLocation0().motionSegmentTo(moveToCommand.getMovedAxesLocation());
 
@@ -385,6 +387,7 @@ protected void recordDiagnostics(Motion plannedMotion, MoveToCommand moveToComma
 
             Double timeStart = moveToCommand.getTimeStart();
             Double d = moveToCommand.getTimeDuration(); 
+            AxesLocation recordingLocation1 = recordingLocation0.put(moveToCommand.getMovedAxesLocation());
             for (ControllerAxis axis : plannedMotion.getLocation1().getAxes(driver)) {
                 MotionProfile profile = plannedMotion.getAxesProfiles()[plannedMotion.getAxisIndex(axis)];
                 if (recordingMotionGraph.getRow(axis.getName(), "s'").size() > 0 
@@ -402,33 +405,42 @@ protected void recordDiagnostics(Motion plannedMotion, MoveToCommand moveToComma
                             Double a = moveToCommand.getAccelerationPerSecond2();
                             if (v0 != null && v1 != null && a != null) {
                                 // Approximated constant acceleration move. Reconstruct Profile.
-                                double s0 = moveToCommand.getLocation0().getCoordinate(axis);
-                                double s1 = moveToCommand.getLocation1().getCoordinate(axis);
+                                double s0 = recordingLocation0.getCoordinate(axis);
+                                double s1 = recordingLocation1.getCoordinate(axis);
                                 double factor = (s1 - s0)*factorRS274NGC;
                                 v0 *= factor;
                                 v1 *= factor;
                                 a *= factor;
-                                sRow.recordDataPoint(t, s0);
+                                if (t == 0) {
+                                    sRow.recordDataPoint(t, s0);
+                                }
                                 sRow.recordDataPoint(t+d, s1);
                                 if (v != null && plannedMotion.getTime() == moveToCommand.getTimeDuration()) { 
                                     // Single trapezoidal move
                                     v *= factor;
                                     double t0 = (v-v0)/a;
                                     double t1 = (v-v1)/a;
                                     double tMid = d-t0-t1;
-                                    if ((t0 > tick || t1 > tick) && tMid > -tick*2) {
+                                    if (tMid < tick) {
+                                        if (t0 > t1) {
+                                            t0 = d-t1-tick;
+                                        } else {
+                                            t1 = d-t0-tick;
+                                        }
+                                    }
+                                    if ((t0 > 2*tick || t1 > 2*tick) && tMid > -tick*2) {
                                         // Valid Trapezoidal.
                                         vRow.recordDataPoint(t+t0, v);
                                         vRow.recordDataPoint(t+d-t1, v);
-                                        if (t0 > tick) {
+                                        if (t0 > tick*2) {
                                             aRow.recordDataPoint(t, a);
                                             aRow.recordDataPoint(t+t0-tick, a);
                                         }
                                         if (tMid > tick*4) {
                                             aRow.recordDataPoint(t+t0, 0);
                                             aRow.recordDataPoint(t+d-t1, 0);
                                         }
-                                        if (t1 > tick) {
+                                        if (t1 > tick*2) {
                                             aRow.recordDataPoint(t+d-t1+tick, -a);
                                             aRow.recordDataPoint(t+d, -a);
                                         }
@@ -466,7 +478,7 @@ else if (d > 0) {
                         }
                         else if (timeStart == 0 && driver.getMotionControlType().isUnpredictable()) {
                             // No approximation possible due to driver setting. Just connect s and show limits to illustrate.
-                            d = plannedMotion.getTime();
+                            d = plannedMotion.getTime()-tick;
                             sRow.recordDataPoint(t, profile.getMomentaryLocation(0));
                             sRow.recordDataPoint(t+d, profile.getMomentaryLocation(d));
                             if (vRow.size() == 0) {
@@ -515,6 +527,7 @@ else if (timeStart == 0 && driver.getMotionControlType().isUnpredictable()) {
             if (moveToCommand.getTimeDuration() != null) {
                 recordingT += moveToCommand.getTimeDuration();
             }
+            recordingLocation0 = recordingLocation1; 
         }
     }
 

src/main/java/org/openpnp/model/Motion.java

@@ -984,7 +984,7 @@ else if (!driver.getMotionControlType().isInterpolated()) {
             double t2 = i*dt;
             boolean special = (i == numSteps);
             // Snap to a any special interval.
-            while (!motionIntervals.isEmpty() && motionIntervals.first() < t2+dt) {
+            while (!motionIntervals.isEmpty() && motionIntervals.first() <= t2+dt*.5) {
                 t2 = motionIntervals.first();
                 motionIntervals.remove(t2);
                 special = true;

src/main/java/org/openpnp/spi/Driver.java

@@ -93,7 +93,7 @@ public interface Driver extends Identifiable, Named, Closeable, WizardConfigurab
      * @return
      * @throws Exception
      */
-    public AxesLocation getMomentaryLocation(long timeout) throws Exception;
+    public AxesLocation getReportedLocation(long timeout) throws Exception;
 
     /**
      * @return true if a motion is still assumed to be pending, i.e. waitForCompletion() has not yet been called.  

src/main/java/org/openpnp/spi/MotionPlanner.java

@@ -142,18 +142,30 @@ public enum CompletionType {
          */
         WaitForStillstand,
 
+
+        /**
+         * Like WaitForStillStand but the wait will also be done, if no motion is thought to be pending (used when it is though 
+         * that the machine might have moved through custom Actuator Gcode (Contract Z Probing is one example). 
+         */
+        WaitForUnconditionalCoordination,
+
         /**
-         * Wait forever.
+         * Like WaitForFullCoordination but wait "forever".
          */
         WaitForStillstandIndefinitely;
 
         public boolean isEnforcingStillstand() {
-            return isWaitingForDrivers() || this == CommandStillstand;
+            return this.ordinal() >= CommandStillstand.ordinal();
         }
 
         public boolean isWaitingForDrivers() {
-            return this == WaitForStillstand || this == WaitForStillstandIndefinitely;
+            return this.ordinal() >= WaitForStillstand.ordinal();
         }
+
+        public boolean isUnconditionalCoordination() {
+            return this.ordinal() >= WaitForUnconditionalCoordination.ordinal();
+        }
+
     }
 
     /**