Merge pull request #1334 from CapnBry/z-grid-dest

CartGridStrategy: Always calculate target Z scaling using target Z, not current Z

diff --git a/src/modules/tools/zprobe/CartGridStrategy.cpp b/src/modules/tools/zprobe/CartGridStrategy.cpp
index 00f0290..be756aa 100644 (file)
--- a/src/modules/tools/zprobe/CartGridStrategy.cpp
+++ b/src/modules/tools/zprobe/CartGridStrategy.cpp
@@ -554,6 +554,19 @@ bool CartGridStrategy::doProbe(Gcode *gc)
 void CartGridStrategy::doCompensation(float *target, bool inverse)
 {
     // Adjust print surface height by linear interpolation over the bed_level array.
+    // offset scale: 1 for default (use offset as is)
+    float scale = 1.0;
+    if (!isnan(this->damping_interval)) {
+        // if the height is below our compensation limit:
+        if(target[Z_AXIS] <= this->height_limit) {
+            // scale the offset as necessary:
+            if(target[Z_AXIS] >= this->dampening_start) {
+                scale = (1.0 - ((target[Z_AXIS] - this->dampening_start) / this->damping_interval));
+            } // else leave scale at 1.0;
+        } else {
+            return; // if Z is higher than max, no compensation
+        }
+    }
 
     // find min/maxes, and handle the case where size is negative (assuming this is possible? Legacy code supported this)
     float min_x = std::min(this->x_start, this->x_start + this->x_size);
@@ -579,24 +592,6 @@ void CartGridStrategy::doCompensation(float *target, bool inverse)
     float left = (1 - ratio_y) * z1 + ratio_y * z2;
     float right = (1 - ratio_y) * z3 + ratio_y * z4;
     float offset = (1 - ratio_x) * left + ratio_x * right;
-    // offset scale: 1 for default (use offset as is)
-    float scale = 1.0;
-
-    if (!isnan(this->damping_interval)) {
-        // first let's find out our 'world coordinate' positions for checking the limits:
-        Robot::wcs_t world_coordinates = THEROBOT->mcs2wcs(THEROBOT->get_axis_position());
-        float current_z = std::get<Z_AXIS>(world_coordinates); // no need to convert to mm, if machine is in inches; so is config!
-        // THEKERNEL->streams->printf("//DEBUG: Current Z: %f\n", current_z);
-        // if the height is below our compensation limit:
-        if(current_z <= this->height_limit) {
-            // scale the offset as necessary:
-            if( current_z >= this->dampening_start) {
-                scale = ( 1- ( (current_z - this->dampening_start ) / this->damping_interval) );
-            } // else leave scale at 1.0;
-        } else {
-            scale = 0.0; // if Z is higher than max, no compensation
-        }
-    }
 
     if (inverse) {
         target[Z_AXIS] -= offset * scale;