From b84bd559839ca453f1a85403a299a7c8939b5845 Mon Sep 17 00:00:00 2001
From: Jim Morris <morris@wolfman.com>
Date: Fri, 7 Oct 2016 12:13:28 -0700
Subject: [PATCH] move where compensation transform is disabled so axis reset
 gets it right and Z does not move during home

---
 src/modules/robot/Robot.cpp             |  6 +++---
 src/modules/tools/endstops/Endstops.cpp | 22 ++++++++++++++--------
 2 files changed, 17 insertions(+), 11 deletions(-)

diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp
index c7281ded..15925530 100644
--- a/src/modules/robot/Robot.cpp
+++ b/src/modules/robot/Robot.cpp
@@ -973,9 +973,9 @@ bool Robot::append_milestone(const float target[], float rate_mm_s)
     }
 
     bool move= false;
-    float sos= 0; // sun of squares for just XYZ
+    float sos= 0; // sum of squares for just XYZ
 
-    // find distance moved by each axis, use transformed target from the current machine position
+    // find distance moved by each axis, use transformed target from the current compensated machine position
     for (size_t i = 0; i < n_motors; i++) {
         deltas[i] = transformed_target[i] - compensated_machine_position[i];
         if(deltas[i] == 0) continue;
@@ -1082,7 +1082,7 @@ bool Robot::append_milestone(const float target[], float rate_mm_s)
     // Append the block to the planner
     // NOTE that distance here should be either the distance travelled by the XYZ axis, or the E mm travel if a solo E move
     if(THEKERNEL->planner->append_block( actuator_pos, n_motors, rate_mm_s, distance, auxilliary_move ? nullptr : unit_vec, acceleration, s_value, is_g123)) {
-        // this is the machine position
+        // this is the new compensated machine position
         memcpy(this->compensated_machine_position, transformed_target, n_motors*sizeof(float));
         return true;
     }
diff --git a/src/modules/tools/endstops/Endstops.cpp b/src/modules/tools/endstops/Endstops.cpp
index 7fd4741e..a0c79d6d 100644
--- a/src/modules/tools/endstops/Endstops.cpp
+++ b/src/modules/tools/endstops/Endstops.cpp
@@ -537,6 +537,10 @@ uint32_t Endstops::read_endstops(uint32_t dummy)
     for(auto& e : homing_axis) { // check all axis homing endstops
         if(e.pin_info == nullptr) continue; // ignore if not a homing endstop
         int m= e.axis_index;
+
+        // for corexy homing in X or Y we must only check the associated endstop, works as we only home one axis at a time for corexy
+        if(is_corexy && (m == X_AXIS || m == Y_AXIS) && !axis_to_home[m]) continue;
+
         if(STEPPER[m]->is_moving()) {
             // if it is moving then we check the associated endstop, and debounce it
             if(e.pin_info->pin.get()) {
@@ -545,7 +549,7 @@ uint32_t Endstops::read_endstops(uint32_t dummy)
 
                 } else {
                     if(is_corexy && (m == X_AXIS || m == Y_AXIS)) {
-                        // corexy when moving in X or Y we need to stop both the X and Y motors, and corexy always homes one axis at a time
+                        // corexy when moving in X or Y we need to stop both the X and Y motors
                         STEPPER[X_AXIS]->stop_moving();
                         STEPPER[Y_AXIS]->stop_moving();
 
@@ -599,10 +603,6 @@ void Endstops::home(axis_bitmap_t a)
        e->debounce= 0;
     }
 
-    // turn off any compensation transform so Z does not move as XY home
-    auto savect= THEROBOT->compensationTransform;
-    THEROBOT->compensationTransform= nullptr;
-
     this->axis_to_home= a;
 
     // Start moving the axes to the origin
@@ -685,9 +685,6 @@ void Endstops::home(axis_bitmap_t a)
 
     THEROBOT->disable_segmentation= false;
 
-    // restore compensationTransform
-    THEROBOT->compensationTransform= savect;
-
     this->status = NOT_HOMING;
 }
 
@@ -696,6 +693,10 @@ void Endstops::process_home_command(Gcode* gcode)
     // First wait for the queue to be empty
     THECONVEYOR->wait_for_idle();
 
+    // turn off any compensation transform so Z does not move as XY home
+    auto savect= THEROBOT->compensationTransform;
+    THEROBOT->compensationTransform= nullptr;
+
     // deltas always home Z axis only, which moves all three actuators
     bool home_in_z = this->is_delta || this->is_rdelta;
 
@@ -748,6 +749,8 @@ void Endstops::process_home_command(Gcode* gcode)
                 bs.set(p.axis_index);
                 home(bs);
             }
+            // check if on_halt (eg kill)
+            if(THEKERNEL->is_halted()) break;
         }
 
     } else {
@@ -755,6 +758,9 @@ void Endstops::process_home_command(Gcode* gcode)
         home(haxis);
     }
 
+    // restore compensationTransform
+    THEROBOT->compensationTransform= savect;
+
     // check if on_halt (eg kill)
     if(THEKERNEL->is_halted()) {
         if(!THEKERNEL->is_grbl_mode()) {
-- 
2.20.1