From e0be983d484481001561f465b9f6f181504b956e Mon Sep 17 00:00:00 2001
From: Jim Morris <morris@wolfman.com>
Date: Tue, 15 Dec 2015 14:44:43 -0800
Subject: [PATCH] zprobe moves must use G53 so they move in machine space

---
 src/modules/tools/zprobe/ThreePointStrategy.cpp | 12 ++++++------
 src/modules/tools/zprobe/ZGridStrategy.cpp      |  9 ++-------
 src/modules/tools/zprobe/ZProbe.cpp             |  3 ++-
 3 files changed, 10 insertions(+), 14 deletions(-)

diff --git a/src/modules/tools/zprobe/ThreePointStrategy.cpp b/src/modules/tools/zprobe/ThreePointStrategy.cpp
index 791c6246..8845167f 100644
--- a/src/modules/tools/zprobe/ThreePointStrategy.cpp
+++ b/src/modules/tools/zprobe/ThreePointStrategy.cpp
@@ -134,12 +134,12 @@ bool ThreePointStrategy::handleGcode(Gcode *gcode)
         } else if( gcode->g == 32 ) { // three point probe
             // first wait for an empty queue i.e. no moves left
             THEKERNEL->conveyor->wait_for_empty_queue();
-            if(!gcode->has_letter('K')) { // K will keep current compensation to test plane
-                // clear any existing plane and compensation
-                delete this->plane;
-                this->plane= nullptr;
-                setAdjustFunction(false);
-            }
+
+             // clear any existing plane and compensation
+            delete this->plane;
+            this->plane= nullptr;
+            setAdjustFunction(false);
+
             if(!doProbing(gcode->stream)) {
                 gcode->stream->printf("Probe failed to complete, probe not triggered or other error\n");
             } else {
diff --git a/src/modules/tools/zprobe/ZGridStrategy.cpp b/src/modules/tools/zprobe/ZGridStrategy.cpp
index 5fd9a1c1..13b679b1 100644
--- a/src/modules/tools/zprobe/ZGridStrategy.cpp
+++ b/src/modules/tools/zprobe/ZGridStrategy.cpp
@@ -584,15 +584,10 @@ void ZGridStrategy::homexyz()
 
 void ZGridStrategy::move(float *position, float feed)
 {
-    char cmd[64];
-
-    // Assemble Gcode to add onto the queue.  Also translate the position for non standard cartesian spaces (cal_offset)
-    snprintf(cmd, sizeof(cmd), "G0 X%1.3f Y%1.3f Z%1.3f F%1.1f", position[0] + this->cal_offset_x, position[1] + this->cal_offset_y, position[2], feed * 60); // use specified feedrate (mm/sec)
+    // translate the position for non standard cartesian spaces (cal_offset)
+    zprobe->coordinated_move(position[0] + this->cal_offset_x, position[1] + this->cal_offset_y, position[2], feed); // use specified feedrate (mm/sec)
 
     //THEKERNEL->streams->printf("DEBUG: move: %s cent: %i\n", cmd, this->center_zero);
-
-    Gcode gc(cmd, &(StreamOutput::NullStream));
-    THEKERNEL->robot->on_gcode_received(&gc); // send to robot directly
 }
 
 
diff --git a/src/modules/tools/zprobe/ZProbe.cpp b/src/modules/tools/zprobe/ZProbe.cpp
index f243cda2..64617889 100644
--- a/src/modules/tools/zprobe/ZProbe.cpp
+++ b/src/modules/tools/zprobe/ZProbe.cpp
@@ -414,13 +414,14 @@ void ZProbe::accelerate(int c)
 
 // issue a coordinated move directly to robot, and return when done
 // Only move the coordinates that are passed in as not nan
+// NOTE must use G53 to force move in machine coordiantes and ignore any WCS offsetts
 void ZProbe::coordinated_move(float x, float y, float z, float feedrate, bool relative)
 {
     char buf[32];
     char cmd[64];
 
     if(relative) strcpy(cmd, "G91 G0 ");
-    else strcpy(cmd, "G0 ");
+    else strcpy(cmd, "G53 G0 "); // G53 forces movement in machine coordinate system
 
     if(!isnan(x)) {
         int n = snprintf(buf, sizeof(buf), " X%1.3f", x);
-- 
2.20.1