From 5e45206aea04cbf7d492d3de7290dca1bc6a9cd3 Mon Sep 17 00:00:00 2001
From: Jim Morris <morris@wolfman.com>
Date: Sat, 2 Aug 2014 17:35:13 -0700
Subject: [PATCH] make the z adjustment not affect requested target just actual
 target

---
 src/modules/robot/Robot.cpp                     | 15 +++++++++------
 src/modules/tools/zprobe/ThreePointStrategy.cpp |  1 +
 2 files changed, 10 insertions(+), 6 deletions(-)

diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp
index 6e707a74..845b4f0f 100644
--- a/src/modules/robot/Robot.cpp
+++ b/src/modules/robot/Robot.cpp
@@ -578,16 +578,19 @@ void Robot::append_milestone( float target[], float rate_mm_s )
     float deltas[3];
     float unit_vec[3];
     float actuator_pos[3];
+    float adj_target[3]; // adjust target for bed leveling
     float millimeters_of_travel;
 
+    memcpy(adj_target, target, sizeof(adj_target));
+
     // check function pointer and call if set to adjust Z for bed leveling
     if(adjustZfnc) {
-        target[Z_AXIS] += adjustZfnc(target[X_AXIS], target[Y_AXIS]);
+        adj_target[Z_AXIS] += adjustZfnc(target[X_AXIS], target[Y_AXIS]);
     }
 
-    // find distance moved by each axis
+    // find distance moved by each axis, use actual adjusted target
     for (int axis = X_AXIS; axis <= Z_AXIS; axis++)
-        deltas[axis] = target[axis] - last_milestone[axis];
+        deltas[axis] = adj_target[axis] - last_milestone[axis];
 
     // Compute how long this move moves, so we can attach it to the block for later use
     millimeters_of_travel = sqrtf( powf( deltas[X_AXIS], 2 ) +  powf( deltas[Y_AXIS], 2 ) +  powf( deltas[Z_AXIS], 2 ) );
@@ -606,8 +609,8 @@ void Robot::append_milestone( float target[], float rate_mm_s )
         }
     }
 
-    // find actuator position given cartesian position
-    arm_solution->cartesian_to_actuator( target, actuator_pos );
+    // find actuator position given cartesian position, use actual adjusted target
+    arm_solution->cartesian_to_actuator( adj_target, actuator_pos );
 
     // check per-actuator speed limits
     for (int actuator = 0; actuator <= 2; actuator++) {
@@ -620,7 +623,7 @@ void Robot::append_milestone( float target[], float rate_mm_s )
     // Append the block to the planner
     THEKERNEL->planner->append_block( actuator_pos, rate_mm_s, millimeters_of_travel, unit_vec );
 
-    // Update the last_milestone to the current target for the next time we use last_milestone
+    // Update the last_milestone to the current target for the next time we use last_milestone, use the requested target not the adjusted one
     memcpy(this->last_milestone, target, sizeof(this->last_milestone)); // this->last_milestone[] = target[];
 
 }
diff --git a/src/modules/tools/zprobe/ThreePointStrategy.cpp b/src/modules/tools/zprobe/ThreePointStrategy.cpp
index dc87fb27..35cd6e92 100644
--- a/src/modules/tools/zprobe/ThreePointStrategy.cpp
+++ b/src/modules/tools/zprobe/ThreePointStrategy.cpp
@@ -124,6 +124,7 @@ bool ThreePointStrategy::handleGcode(Gcode *gcode)
             if(gcode->has_letter('Y')) y = gcode->get_value('Y');
             z= getZOffset(x, y);
             gcode->stream->printf("z= %f\n", z);
+            // tell robot to adjust z on each move
             THEKERNEL->robot->adjustZfnc= [this](float x, float y) { return this->plane->getz(x, y); };
             return true;
         }
-- 
2.20.1