From 6fd694dc665ed39f3be2a56280cac074d9d46cf2 Mon Sep 17 00:00:00 2001
From: Joacim Breiler <joacim@breiler.com>
Date: Thu, 17 Aug 2023 19:28:17 +0200
Subject: [PATCH] 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.
---
 .../surfacescanner/SurfaceScanner.java        | 63 +++++++++++++++----
 1 file changed, 51 insertions(+), 12 deletions(-)

diff --git a/ugs-platform/ugs-platform-surfacescanner/src/main/java/com/willwinder/ugs/platform/surfacescanner/SurfaceScanner.java b/ugs-platform/ugs-platform-surfacescanner/src/main/java/com/willwinder/ugs/platform/surfacescanner/SurfaceScanner.java
index 3f6357d305..c61e77f9da 100644
--- a/ugs-platform/ugs-platform-surfacescanner/src/main/java/com/willwinder/ugs/platform/surfacescanner/SurfaceScanner.java
+++ b/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;
     }