CartGridStrategy: compensate Z scale based on target Z, not current Z

The strategy scales the grid compensation between dampening_start and height_limit,
but the current Z position is used to generate the scale. This can create a discontinuity
when switching layers or Z-hopping because the new position is scaled based on the
previous position-- it shouldn't use any information about where it _was_ to adjust
the new Z height.

diff --git a/src/modules/tools/zprobe/CartGridStrategy.cpp b/src/modules/tools/zprobe/CartGridStrategy.cpp
index 00f0290..35f27b1 100644 (file)
--- a/src/modules/tools/zprobe/CartGridStrategy.cpp
+++ b/src/modules/tools/zprobe/CartGridStrategy.cpp
@@ -583,18 +583,14 @@ void CartGridStrategy::doCompensation(float *target, bool inverse)
     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) {
+        if(target[Z_AXIS] <= this->height_limit) {
             // scale the offset as necessary:
-            if( current_z >= this->dampening_start) {
-                scale = ( 1- ( (current_z - this->dampening_start ) / this->damping_interval) );
+            if(target[Z_AXIS] >= this->dampening_start) {
+                scale = (1 - ((target[Z_AXIS] - this->dampening_start ) / this->damping_interval));
             } // else leave scale at 1.0;
         } else {
-            scale = 0.0; // if Z is higher than max, no compensation
+            return; // if Z is higher than max, no compensation
         }
     }