Added safe positioning when starting an autoleveler job (#2282)

When starting the autoleveler it will make sure to heighten the probe to the max probe height before moving to the XY start position. After the job is finished it will go to the max height and go back to start position.

Also fixed bug where if the controller was reporting coordinates in inches the autoleveling wouldn't work.

ugs-platform/ugs-platform-surfacescanner/src/main/java/com/willwinder/ugs/platform/surfacescanner/SurfaceScanner.java

@@ -111,15 +111,14 @@ public void handleEvent(ProbeEvent evt) {
                 new Object[]{probePosition.getX(), probePosition.getY(), probePosition.getZ()});
         probeEvent(probePosition);
 
-        try {
+        if (pendingPositions.isEmpty()) {
+            // The probing is done!
+            moveToSafeStartPoint(probePosition);
+        } else {
             double retractedZ = retract(probePosition.getZ());
-            if (!pendingPositions.isEmpty()) {
-                probeNextPoint(retractedZ);
-            }
-        } catch (Exception e) {
-            throw new RuntimeException(e);
+            probeNextPoint(retractedZ);
         }
-    }
+}
 
     private Units getPreferredUnits() {
         return this.backend.getSettings().getPreferredUnits();
@@ -166,7 +165,7 @@ public void probeEvent(final Position p) {
         Position expectedProbePosition = pendingPositions.pop();
         Position probedPosition = p.getPositionIn(expectedProbePosition.getUnits());
 
-        // The position reported from controller might lack some precision on the X/Y position.
+        // The position reported from the controller might lack some precision on the X/Y position.
         // We therefore need to lower the precision when checking the probed X/Y axes
         double delta = expectedProbePosition.getUnits() == Units.MM ? 0.01 : 0.001;
         if (!isEqual(probedPosition.getX(), expectedProbePosition.getX(), delta) || !isEqual(probedPosition.getY(), expectedProbePosition.getY(), delta)) {
@@ -188,10 +187,44 @@ public void scan() {
         isScanning.set(true);
         Position work = backend.getWorkPosition();
         Position machine = backend.getMachinePosition();
+        machineWorkOffset = new Position(work.getUnits());
         machineWorkOffset.x = work.x - machine.x;
         machineWorkOffset.y = work.y - machine.y;
         machineWorkOffset.z = work.z - machine.z;
-        probeNextPoint(work.getZ());
+
+        moveToSafeStartPoint(work);
+        probeNextPoint(maxXYZ.getZ());
+    }
+
+    private void moveToSafeStartPoint(Position currentPosition) {
+        try {
+            // Move up if below probe area
+            double safetyHeight = (UnitUtils.scaleUnits(Units.MM, maxXYZ.getUnits()) * backend.getSettings().getSafetyHeight()) + maxXYZ.getZ();
+            if (currentPosition.getPositionIn(maxXYZ.getUnits()).getZ() < safetyHeight) {
+                PartialPosition safeHeightPos = PartialPosition.builder(maxXYZ.getUnits()).setZ(safetyHeight).build();
+                String cmd = GcodeUtils.generateMoveCommand(
+                        "G90G0", getProbeScanFeedRate(), safeHeightPos);
+                logger.log(Level.INFO, "Move up to safe height {0}", new Object[]{safeHeightPos});
+                backend.sendGcodeCommand(true, cmd);
+            }
+
+            // Move to the XY start position
+            PartialPosition startPos = PartialPosition.builder(minXYZ).clearZ().build();
+            String cmd = GcodeUtils.generateMoveCommand(
+                    "G90G0", getProbeScanFeedRate(), startPos);
+            logger.log(Level.INFO, "Move to start position {0}", new Object[]{startPos});
+            backend.sendGcodeCommand(true, cmd);
+
+            // Move to the Y start position
+            PartialPosition startHeight = PartialPosition.builder(maxXYZ.getUnits()).setZ(maxXYZ.getZ()).build();
+            cmd = GcodeUtils.generateMoveCommand(
+                    "G90G0", getProbeScanFeedRate(), startHeight);
+            logger.log(Level.INFO, "Move to start height {0}", new Object[]{startHeight});
+            backend.sendGcodeCommand(true, cmd);
+        } catch (Exception e) {
+            reset();
+            throw new RuntimeException(e);
+        }
     }
 
     public Optional<Position> getNextProbePoint() {
@@ -227,7 +260,7 @@ private double getProbeScanFeedRate() {
         return settings.getProbeScanFeedRate() * UnitUtils.scaleUnits(Units.MM, getPreferredUnits());
     }
 
-    private double retract(Double zLast) throws Exception {
+    private double retract(Double zLast) {
         double zRetract = settings.getZRetract() * maxXYZ.getZ();
         if (zRetract <= 0) {
             zRetract = maxXYZ.getZ() - minXYZ.getZ();
@@ -240,8 +273,14 @@ private double retract(Double zLast) throws Exception {
                 "G90G0",
                 getProbeScanFeedRate(),
                 safeZ);
-        logger.log(Level.INFO, "Retract to {0} {1}", new Object[]{safeZ, retractCommand});
-        backend.sendGcodeCommand(true, retractCommand);
+
+        try {
+            logger.log(Level.INFO, "Retract to {0} {1}", new Object[]{safeZ, retractCommand});
+            backend.sendGcodeCommand(true, retractCommand);
+        } catch (Exception e) {
+            reset();
+            throw new RuntimeException(e);
+        }
         return zBackoff;
     }