From d698c89971b3c2f5a19a44b400e6c08be110a975 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Mon, 23 May 2016 14:46:29 -0700
Subject: [PATCH] Fix bad movement in gcode_T when switching

---
 Marlin/Marlin_main.cpp | 34 +++++++++++++---------------------
 1 file changed, 13 insertions(+), 21 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index b730b5852..6ddd0d2ea 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -1272,6 +1272,7 @@ XYZ_CONSTS_FROM_CONFIG(signed char, home_dir, HOME_DIR);
  */
 static void update_software_endstops(AxisEnum axis) {
   float offs = home_offset[axis] + position_shift[axis];
+
   #if ENABLED(DUAL_X_CARRIAGE)
     if (axis == X_AXIS) {
       float dual_max_x = max(extruder_offset[X_AXIS][1], X2_MAX_POS);
@@ -1292,6 +1293,7 @@ static void update_software_endstops(AxisEnum axis) {
     sw_endstop_min[axis] = base_min_pos(axis) + offs;
     sw_endstop_max[axis] = base_max_pos(axis) + offs;
   }
+
 }
 
 /**
@@ -6472,24 +6474,29 @@ inline void gcode_T(uint8_t tmp_extruder) {
 
         #else // !AUTO_BED_LEVELING_FEATURE
 
-          // Offset extruder (only by XY)
-          for (int i=X_AXIS; i<=Y_AXIS; i++)
-            current_position[i] += extruder_offset[i][tmp_extruder] - extruder_offset[i][active_extruder];
+          // The newly-selected extruder is actually at...
+          for (int i=X_AXIS; i<=Y_AXIS; i++) {
+            float diff = extruder_offset[i][tmp_extruder] - extruder_offset[i][active_extruder];
+            current_position[i] += diff;
+            position_shift[i] += diff; // Offset the coordinate space
+            update_software_endstops((AxisEnum)i);
+          }
 
         #endif // !AUTO_BED_LEVELING_FEATURE
 
-        // Set the new active extruder and position
+        // Set the new active extruder
         active_extruder = tmp_extruder;
 
       #endif // !DUAL_X_CARRIAGE
 
+      // Tell the planner the new "current position"
       #if ENABLED(DELTA)
         sync_plan_position_delta();
       #else
         sync_plan_position();
       #endif
 
-      // Move to the old position
+      // Move to the "old position" (move the extruder into place)
       if (IsRunning()) prepare_move();
 
     } // (tmp_extruder != active_extruder)
@@ -7187,23 +7194,8 @@ void clamp_to_software_endstops(float target[3]) {
   if (min_software_endstops) {
     NOLESS(target[X_AXIS], sw_endstop_min[X_AXIS]);
     NOLESS(target[Y_AXIS], sw_endstop_min[Y_AXIS]);
-
-    float negative_z_offset = 0;
-    #if ENABLED(AUTO_BED_LEVELING_FEATURE)
-      if (zprobe_zoffset < 0) negative_z_offset += zprobe_zoffset;
-      if (home_offset[Z_AXIS] < 0) {
-        #if ENABLED(DEBUG_LEVELING_FEATURE)
-          if (DEBUGGING(LEVELING)) {
-            SERIAL_ECHOPAIR("> clamp_to_software_endstops > Add home_offset[Z_AXIS]:", home_offset[Z_AXIS]);
-            SERIAL_EOL;
-          }
-        #endif
-        negative_z_offset += home_offset[Z_AXIS];
-      }
-    #endif
-    NOLESS(target[Z_AXIS], sw_endstop_min[Z_AXIS] + negative_z_offset);
+    NOLESS(target[Z_AXIS], sw_endstop_min[Z_AXIS]);
   }
-
   if (max_software_endstops) {
     NOMORE(target[X_AXIS], sw_endstop_max[X_AXIS]);
     NOMORE(target[Y_AXIS], sw_endstop_max[Y_AXIS]);