ZGrid repair probe removal position

optimise code and repaired probe removal position

diff --git a/src/modules/tools/zprobe/ZGridStrategy.cpp b/src/modules/tools/zprobe/ZGridStrategy.cpp
index bd2e64d..5fd9a1c 100644 (file)
--- a/src/modules/tools/zprobe/ZGridStrategy.cpp
+++ b/src/modules/tools/zprobe/ZGridStrategy.cpp
@@ -515,17 +515,21 @@ bool ZGridStrategy::doProbing(StreamOutput *stream)  // probed calibration
 
         pindex = int(this->cal[X_AXIS]/this->bed_div_x + 0.25)*this->numCols + int(this->cal[Y_AXIS]/this->bed_div_y + 0.25);
 
-        if (probes == (probe_points-1) && this->wait_for_probe){  // Only move to removal position if probe confirmation was selected
-            this->cal[X_AXIS] = this->bed_x/2.0f;                 // Else machine will return to first probe position when done
-            this->cal[Y_AXIS] = this->bed_y/2.0f;
-            this->cal[Z_AXIS] = this->bed_z/2.0f;                 // Position head for probe removal
-        } else {
-            this->next_cal();                                     // to not cause damage to machine due to Z-offset
-        }
-        this->pData[pindex] = z ;                                 // save the offset
+        this->next_cal();                                        // Calculate next calibration position
+
+        this->pData[pindex] = z ;                                // save the offset
     }
 
-    stream->printf("\nCalibration done.  Please remove probe\n");
+    stream->printf("\nCalibration done.\n");
+    if (this->wait_for_probe) {                                  // Only do this it the config calls for probe removal position
+        this->cal[X_AXIS] = this->bed_x/2.0f;
+        this->cal[Y_AXIS] = this->bed_y/2.0f;
+        this->cal[Z_AXIS] = this->bed_z/2.0f;                    // Position head for probe removal
+        this->move(this->cal, slow_rate);
+
+        stream->printf("Please remove probe\n");
+
+    }
 
     // activate correction
     this->normalize_grid();
@@ -657,7 +661,7 @@ float ZGridStrategy::getZOffset(float X, float Y)
 
     int xIndex = (int)(floorf(xdiff)); // Get the current sector (X)
     int yIndex = (int)(floorf(ydiff)); // Get the current sector (Y)
-    
+
     // Index bounds limited to be inside the table
     if (xIndex < 0) xIndex = 0;
     else if (xIndex > (this->numRows - 2)) xIndex = this->numRows - 2;