From 571652c49b68988c728f8ef2d9856d7aa50d68ae Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Tue, 31 Mar 2015 01:41:24 -0700
Subject: [PATCH 1/9] Fix LCD click

- Make the default LCD click sound more pleasant
- Fix warnings about bed temperature runaway
---
 Marlin/temperature.cpp | 27 ++++++++++++++++-----------
 Marlin/ultralcd.cpp    |  6 +++---
 2 files changed, 19 insertions(+), 14 deletions(-)

diff --git a/Marlin/temperature.cpp b/Marlin/temperature.cpp
index d0720c5ec2..884de94014 100644
--- a/Marlin/temperature.cpp
+++ b/Marlin/temperature.cpp
@@ -83,15 +83,20 @@ unsigned char soft_pwm_bed;
 #ifdef FILAMENT_SENSOR
   int current_raw_filwidth = 0;  //Holds measured filament diameter - one extruder only
 #endif  
-#if defined (THERMAL_RUNAWAY_PROTECTION_PERIOD) && THERMAL_RUNAWAY_PROTECTION_PERIOD > 0
-void thermal_runaway_protection(int *state, unsigned long *timer, float temperature, float target_temperature, int heater_id, int period_seconds, int hysteresis_degc);
-static int thermal_runaway_state_machine[4]; // = {0,0,0,0};
-static unsigned long thermal_runaway_timer[4]; // = {0,0,0,0};
-static bool thermal_runaway = false;
-#if TEMP_SENSOR_BED != 0
-  static int thermal_runaway_bed_state_machine;
-  static unsigned long thermal_runaway_bed_timer;
-#endif
+
+#define HAS_HEATER_THERMAL_PROTECTION (defined(THERMAL_RUNAWAY_PROTECTION_PERIOD) && THERMAL_RUNAWAY_PROTECTION_PERIOD > 0)
+#define HAS_BED_THERMAL_PROTECTION (defined(THERMAL_RUNAWAY_PROTECTION_BED_PERIOD) && THERMAL_RUNAWAY_PROTECTION_BED_PERIOD > 0 && TEMP_SENSOR_BED != 0)
+#if HAS_HEATER_THERMAL_PROTECTION || HAS_BED_THERMAL_PROTECTION
+  static bool thermal_runaway = false;
+  void thermal_runaway_protection(int *state, unsigned long *timer, float temperature, float target_temperature, int heater_id, int period_seconds, int hysteresis_degc);
+  #if HAS_HEATER_THERMAL_PROTECTION
+    static int thermal_runaway_state_machine[4]; // = {0,0,0,0};
+    static unsigned long thermal_runaway_timer[4]; // = {0,0,0,0};
+  #endif
+  #if HAS_BED_THERMAL_PROTECTION
+    static int thermal_runaway_bed_state_machine;
+    static unsigned long thermal_runaway_bed_timer;
+  #endif
 #endif
 
 //===========================================================================
@@ -650,7 +655,7 @@ void manage_heater() {
 
   #if TEMP_SENSOR_BED != 0
   
-    #if defined(THERMAL_RUNAWAY_PROTECTION_BED_PERIOD) && THERMAL_RUNAWAY_PROTECTION_BED_PERIOD > 0
+    #if HAS_BED_THERMAL_PROTECTION
       thermal_runaway_protection(&thermal_runaway_bed_state_machine, &thermal_runaway_bed_timer, current_temperature_bed, target_temperature_bed, 9, THERMAL_RUNAWAY_PROTECTION_BED_PERIOD, THERMAL_RUNAWAY_PROTECTION_BED_HYSTERESIS);
     #endif
 
@@ -1008,7 +1013,7 @@ void setWatch() {
   #endif 
 }
 
-#if defined(THERMAL_RUNAWAY_PROTECTION_PERIOD) && THERMAL_RUNAWAY_PROTECTION_PERIOD > 0
+#if HAS_HEATER_THERMAL_PROTECTION || HAS_BED_THERMAL_PROTECTION
 void thermal_runaway_protection(int *state, unsigned long *timer, float temperature, float target_temperature, int heater_id, int period_seconds, int hysteresis_degc)
 {
 /*
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index dc0ba4fbba..b00cfea1d4 100644
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -136,7 +136,6 @@ static void lcd_status_screen();
     if (encoderLine < currentMenuViewOffset) currentMenuViewOffset = encoderLine; \
     uint8_t _lineNr = currentMenuViewOffset, _menuItemNr; \
     bool wasClicked = LCD_CLICKED, itemSelected; \
-    if (wasClicked) lcd_quick_feedback(); \
     for (uint8_t _drawLineNr = 0; _drawLineNr < LCD_HEIGHT; _drawLineNr++, _lineNr++) { \
       _menuItemNr = 0;
 
@@ -167,6 +166,7 @@ static void lcd_status_screen();
       if (lcdDrawUpdate) \
         lcd_implementation_drawmenu_ ## type(itemSelected, _drawLineNr, PSTR(label), ## args); \
       if (wasClicked && itemSelected) { \
+        lcd_quick_feedback(); \
         menu_action_ ## type(args); \
         return; \
       } \
@@ -1155,10 +1155,10 @@ static void lcd_quick_feedback() {
   #elif defined(BEEPER) && BEEPER > -1
     SET_OUTPUT(BEEPER);
     #ifndef LCD_FEEDBACK_FREQUENCY_HZ
-      #define LCD_FEEDBACK_FREQUENCY_HZ 500
+      #define LCD_FEEDBACK_FREQUENCY_HZ 5000
     #endif
     #ifndef LCD_FEEDBACK_FREQUENCY_DURATION_MS
-      #define LCD_FEEDBACK_FREQUENCY_DURATION_MS 50
+      #define LCD_FEEDBACK_FREQUENCY_DURATION_MS 2
     #endif
     const unsigned int delay = 1000000 / LCD_FEEDBACK_FREQUENCY_HZ / 2;
     int i = LCD_FEEDBACK_FREQUENCY_DURATION_MS * LCD_FEEDBACK_FREQUENCY_HZ / 1000;

From bb6174524f5f2382cf77ccec276d2ce7e1eb848e Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Tue, 31 Mar 2015 02:08:57 -0700
Subject: [PATCH 2/9] Fix EXTRUDER_OFFSET compiler error

- Addressing issue #1741
---
 Marlin/Marlin_main.cpp | 14 ++++++++++----
 1 file changed, 10 insertions(+), 4 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 86138fb8f9..caeb628b2d 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -229,18 +229,17 @@ bool axis_known_position[3] = { false };
 // Extruder offset
 #if EXTRUDERS > 1
   #ifndef EXTRUDER_OFFSET_X
-    #define EXTRUDER_OFFSET_X 0
+    #define EXTRUDER_OFFSET_X { 0 }
   #endif
   #ifndef EXTRUDER_OFFSET_Y
-    #define EXTRUDER_OFFSET_Y 0
+    #define EXTRUDER_OFFSET_Y { 0 }
   #endif
   #ifndef DUAL_X_CARRIAGE
     #define NUM_EXTRUDER_OFFSETS 2 // only in XY plane
   #else
     #define NUM_EXTRUDER_OFFSETS 3 // supports offsets in XYZ plane
   #endif
-  #define _EXY { EXTRUDER_OFFSET_X, EXTRUDER_OFFSET_Y }
-  float extruder_offset[EXTRUDERS][NUM_EXTRUDER_OFFSETS] = ARRAY_BY_EXTRUDERS(_EXY, _EXY, _EXY, _EXY);
+  float extruder_offset[EXTRUDERS][NUM_EXTRUDER_OFFSETS];
 #endif
 
 uint8_t active_extruder = 0;
@@ -569,6 +568,13 @@ void servo_init()
 
 void setup()
 {
+  #if EXTRUDERS > 1
+    float offset[3][EXTRUDERS] = { EXTRUDER_OFFSET_X, EXTRUDER_OFFSET_Y, ARRAY_BY_EXTRUDERS(0,0,0,0) };
+    for (int e=0; e<EXTRUDERS; e++)
+      for (int i=0; i<NUM_EXTRUDER_OFFSETS; i++)
+        extruder_offset[e][i] = offset[i,e];
+  #endif
+
   setup_killpin();
   setup_filrunoutpin();
   setup_powerhold();

From 28792e573112e6b4daf3aea3e926a716fa027a56 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Tue, 31 Mar 2015 02:10:49 -0700
Subject: [PATCH 3/9] Fix typo in assignment

---
 Marlin/Marlin_main.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index caeb628b2d..5c7ff37f37 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -572,7 +572,7 @@ void setup()
     float offset[3][EXTRUDERS] = { EXTRUDER_OFFSET_X, EXTRUDER_OFFSET_Y, ARRAY_BY_EXTRUDERS(0,0,0,0) };
     for (int e=0; e<EXTRUDERS; e++)
       for (int i=0; i<NUM_EXTRUDER_OFFSETS; i++)
-        extruder_offset[e][i] = offset[i,e];
+        extruder_offset[e][i] = offset[i][e];
   #endif
 
   setup_killpin();

From 2a45922afef441d1a32f6ba6655d49785212ee5e Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Tue, 31 Mar 2015 02:49:47 -0700
Subject: [PATCH 4/9] Restore extruder_offset, init more simply

---
 Marlin/Configuration.h | 12 ++++-----
 Marlin/Marlin_main.cpp | 56 +++++++++++++++++++-----------------------
 2 files changed, 31 insertions(+), 37 deletions(-)

diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index bd1bff6d22..f62509e4a6 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -72,6 +72,12 @@ Here are some standard links for getting your machine calibrated:
 // This defines the number of extruders
 #define EXTRUDERS 1
 
+// Offset of the extruders (uncomment if using more than one and relying on firmware to position when changing).
+// The offset has to be X=0, Y=0 for the extruder 0 hotend (default extruder).
+// For the other hotends it is their distance from the extruder 0 hotend.
+#define EXTRUDER_OFFSET_X {0.0, 20.00} // (in mm) for each extruder, offset of the hotend on the X axis
+#define EXTRUDER_OFFSET_Y {0.0, 5.00}  // (in mm) for each extruder, offset of the hotend on the Y axis
+
 //// The following define selects which power supply you have. Please choose the one that matches your setup
 // 1 = ATX
 // 2 = X-Box 360 203Watts (the blue wire connected to PS_ON and the red wire to VCC)
@@ -516,12 +522,6 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 #define DEFAULT_RETRACT_ACCELERATION  3000   // E acceleration in mm/s^2 for retracts
 #define DEFAULT_TRAVEL_ACCELERATION   3000    // X, Y, Z acceleration in mm/s^2 for travel (non printing) moves
 
-// Offset of the extruders (uncomment if using more than one and relying on firmware to position when changing).
-// The offset has to be X=0, Y=0 for the extruder 0 hotend (default extruder).
-// For the other hotends it is their distance from the extruder 0 hotend.
-// #define EXTRUDER_OFFSET_X {0.0, 20.00} // (in mm) for each extruder, offset of the hotend on the X axis
-// #define EXTRUDER_OFFSET_Y {0.0, 5.00}  // (in mm) for each extruder, offset of the hotend on the Y axis
-
 // The speed change that does not require acceleration (i.e. the software might assume it can be done instantaneously)
 #define DEFAULT_XYJERK                20.0    // (mm/sec)
 #define DEFAULT_ZJERK                 0.4     // (mm/sec)
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 5c7ff37f37..6b3589d7cc 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -226,7 +226,7 @@ float min_pos[3] = { X_MIN_POS, Y_MIN_POS, Z_MIN_POS };
 float max_pos[3] = { X_MAX_POS, Y_MAX_POS, Z_MAX_POS };
 bool axis_known_position[3] = { false };
 
-// Extruder offset
+// Extruder offsets
 #if EXTRUDERS > 1
   #ifndef EXTRUDER_OFFSET_X
     #define EXTRUDER_OFFSET_X { 0 }
@@ -234,12 +234,13 @@ bool axis_known_position[3] = { false };
   #ifndef EXTRUDER_OFFSET_Y
     #define EXTRUDER_OFFSET_Y { 0 }
   #endif
-  #ifndef DUAL_X_CARRIAGE
-    #define NUM_EXTRUDER_OFFSETS 2 // only in XY plane
-  #else
-    #define NUM_EXTRUDER_OFFSETS 3 // supports offsets in XYZ plane
-  #endif
-  float extruder_offset[EXTRUDERS][NUM_EXTRUDER_OFFSETS];
+  float extruder_offset[][EXTRUDERS] = {
+    EXTRUDER_OFFSET_X,
+    EXTRUDER_OFFSET_Y
+    #ifdef DUAL_X_CARRIAGE
+      , { 0 } // supports offsets in XYZ plane
+    #endif
+  };
 #endif
 
 uint8_t active_extruder = 0;
@@ -568,13 +569,6 @@ void servo_init()
 
 void setup()
 {
-  #if EXTRUDERS > 1
-    float offset[3][EXTRUDERS] = { EXTRUDER_OFFSET_X, EXTRUDER_OFFSET_Y, ARRAY_BY_EXTRUDERS(0,0,0,0) };
-    for (int e=0; e<EXTRUDERS; e++)
-      for (int i=0; i<NUM_EXTRUDER_OFFSETS; i++)
-        extruder_offset[e][i] = offset[i][e];
-  #endif
-
   setup_killpin();
   setup_filrunoutpin();
   setup_powerhold();
@@ -941,7 +935,7 @@ XYZ_CONSTS_FROM_CONFIG(signed char, home_dir,  HOME_DIR);
       // second X-carriage offset when homed - otherwise X2_HOME_POS is used.
       // This allow soft recalibration of the second extruder offset position without firmware reflash
       // (through the M218 command).
-      return (extruder_offset[1][X_AXIS] > 0) ? extruder_offset[1][X_AXIS] : X2_HOME_POS;
+      return (extruder_offset[X_AXIS][1] > 0) ? extruder_offset[X_AXIS][1] : X2_HOME_POS;
   }
 
   static int x_home_dir(int extruder) {
@@ -965,14 +959,14 @@ static void axis_is_at_home(int axis) {
       if (active_extruder != 0) {
         current_position[X_AXIS] = x_home_pos(active_extruder);
                  min_pos[X_AXIS] = X2_MIN_POS;
-                 max_pos[X_AXIS] = max(extruder_offset[1][X_AXIS], X2_MAX_POS);
+                 max_pos[X_AXIS] = max(extruder_offset[X_AXIS][1], X2_MAX_POS);
         return;
       }
       else if (dual_x_carriage_mode == DXC_DUPLICATION_MODE) {
         float xoff = home_offset[X_AXIS];
         current_position[X_AXIS] = base_home_pos(X_AXIS) + xoff;
                  min_pos[X_AXIS] = base_min_pos(X_AXIS) + xoff;
-                 max_pos[X_AXIS] = min(base_max_pos(X_AXIS) + xoff, max(extruder_offset[1][X_AXIS], X2_MAX_POS) - duplicate_extruder_x_offset);
+                 max_pos[X_AXIS] = min(base_max_pos(X_AXIS) + xoff, max(extruder_offset[X_AXIS][1], X2_MAX_POS) - duplicate_extruder_x_offset);
         return;
       }
     }
@@ -3786,23 +3780,23 @@ inline void gcode_M206() {
   inline void gcode_M218() {
     if (setTargetedHotend(218)) return;
 
-    if (code_seen('X')) extruder_offset[tmp_extruder][X_AXIS] = code_value();
-    if (code_seen('Y')) extruder_offset[tmp_extruder][Y_AXIS] = code_value();
+    if (code_seen('X')) extruder_offset[X_AXIS][tmp_extruder] = code_value();
+    if (code_seen('Y')) extruder_offset[Y_AXIS][tmp_extruder] = code_value();
 
     #ifdef DUAL_X_CARRIAGE
-      if (code_seen('Z')) extruder_offset[tmp_extruder][Z_AXIS] = code_value();
+      if (code_seen('Z')) extruder_offset[Z_AXIS][tmp_extruder] = code_value();
     #endif
 
     SERIAL_ECHO_START;
     SERIAL_ECHOPGM(MSG_HOTEND_OFFSET);
     for (tmp_extruder = 0; tmp_extruder < EXTRUDERS; tmp_extruder++) {
       SERIAL_ECHO(" ");
-      SERIAL_ECHO(extruder_offset[tmp_extruder][X_AXIS]);
+      SERIAL_ECHO(extruder_offset[X_AXIS][tmp_extruder]);
       SERIAL_ECHO(",");
-      SERIAL_ECHO(extruder_offset[tmp_extruder][Y_AXIS]);
+      SERIAL_ECHO(extruder_offset[Y_AXIS][tmp_extruder]);
       #ifdef DUAL_X_CARRIAGE
         SERIAL_ECHO(",");
-        SERIAL_ECHO(extruder_offset[tmp_extruder][Z_AXIS]);
+        SERIAL_ECHO(extruder_offset[Z_AXIS][tmp_extruder]);
       #endif
     }
     SERIAL_EOL;
@@ -4493,13 +4487,13 @@ inline void gcode_M503() {
         SERIAL_ECHO_START;
         SERIAL_ECHOPGM(MSG_HOTEND_OFFSET);
         SERIAL_ECHO(" ");
-        SERIAL_ECHO(extruder_offset[0][X_AXIS]);
+        SERIAL_ECHO(extruder_offset[X_AXIS][0]);
         SERIAL_ECHO(",");
-        SERIAL_ECHO(extruder_offset[0][Y_AXIS]);
+        SERIAL_ECHO(extruder_offset[Y_AXIS][0]);
         SERIAL_ECHO(" ");
         SERIAL_ECHO(duplicate_extruder_x_offset);
         SERIAL_ECHO(",");
-        SERIAL_ECHOLN(extruder_offset[1][Y_AXIS]);
+        SERIAL_ECHOLN(extruder_offset[Y_AXIS][1]);
         break;
       case DXC_FULL_CONTROL_MODE:
       case DXC_AUTO_PARK_MODE:
@@ -4634,11 +4628,11 @@ inline void gcode_T() {
 
           // apply Y & Z extruder offset (x offset is already used in determining home pos)
           current_position[Y_AXIS] = current_position[Y_AXIS] -
-                       extruder_offset[active_extruder][Y_AXIS] +
-                       extruder_offset[tmp_extruder][Y_AXIS];
+                       extruder_offset[Y_AXIS][active_extruder] +
+                       extruder_offset[Y_AXIS][tmp_extruder];
           current_position[Z_AXIS] = current_position[Z_AXIS] -
-                       extruder_offset[active_extruder][Z_AXIS] +
-                       extruder_offset[tmp_extruder][Z_AXIS];
+                       extruder_offset[Z_AXIS][active_extruder] +
+                       extruder_offset[Z_AXIS][tmp_extruder];
 
           active_extruder = tmp_extruder;
 
@@ -4668,7 +4662,7 @@ inline void gcode_T() {
         #else // !DUAL_X_CARRIAGE
           // Offset extruder (only by XY)
           for (int i=X_AXIS; i<=Y_AXIS; i++)
-            current_position[i] += extruder_offset[tmp_extruder][i] - extruder_offset[active_extruder][i];
+            current_position[i] += extruder_offset[i][tmp_extruder] - extruder_offset[i][active_extruder];
           // Set the new active extruder and position
           active_extruder = tmp_extruder;
         #endif // !DUAL_X_CARRIAGE

From 935592cf6a8793e9baaed0d584b5aad84c1a3ff5 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Tue, 31 Mar 2015 02:50:52 -0700
Subject: [PATCH 5/9] offsets off by default

---
 Marlin/Configuration.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index f62509e4a6..5412cbbea3 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -75,8 +75,8 @@ Here are some standard links for getting your machine calibrated:
 // Offset of the extruders (uncomment if using more than one and relying on firmware to position when changing).
 // The offset has to be X=0, Y=0 for the extruder 0 hotend (default extruder).
 // For the other hotends it is their distance from the extruder 0 hotend.
-#define EXTRUDER_OFFSET_X {0.0, 20.00} // (in mm) for each extruder, offset of the hotend on the X axis
-#define EXTRUDER_OFFSET_Y {0.0, 5.00}  // (in mm) for each extruder, offset of the hotend on the Y axis
+//#define EXTRUDER_OFFSET_X {0.0, 20.00} // (in mm) for each extruder, offset of the hotend on the X axis
+//#define EXTRUDER_OFFSET_Y {0.0, 5.00}  // (in mm) for each extruder, offset of the hotend on the Y axis
 
 //// The following define selects which power supply you have. Please choose the one that matches your setup
 // 1 = ATX

From cda4bd4ad95d3472bd6961c56bc64d57bbdfd097 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Tue, 31 Mar 2015 03:20:15 -0700
Subject: [PATCH 6/9] Correct the comment about M206

---
 Marlin/Configuration.h                                          | 2 +-
 Marlin/configurator/config/Configuration.h                      | 2 +-
 Marlin/example_configurations/Felix/Configuration.h             | 2 +-
 Marlin/example_configurations/Felix/Configuration_DUAL.h        | 2 +-
 Marlin/example_configurations/Hephestos/Configuration.h         | 2 +-
 Marlin/example_configurations/K8200/Configuration.h             | 2 +-
 Marlin/example_configurations/SCARA/Configuration.h             | 2 +-
 Marlin/example_configurations/WITBOX/Configuration.h            | 2 +-
 Marlin/example_configurations/delta/generic/Configuration.h     | 2 +-
 Marlin/example_configurations/delta/kossel_mini/Configuration.h | 2 +-
 Marlin/example_configurations/makibox/Configuration.h           | 2 +-
 Marlin/example_configurations/tvrrug/Round2/Configuration.h     | 2 +-
 12 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index bd1bff6d22..56fe20089e 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -696,7 +696,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 // Servo Endstops
 //
 // This allows for servo actuated endstops, primary usage is for the Z Axis to eliminate calibration or bed height changes.
-// Use M206 command to correct for switch height offset to actual nozzle height. Store that setting with M500.
+// Use M851 to set the z-probe vertical offset from the nozzle. Store that setting with M500.
 //
 //#define SERVO_ENDSTOPS {-1, -1, 0} // Servo index for X, Y, Z. Disable with -1
 //#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
diff --git a/Marlin/configurator/config/Configuration.h b/Marlin/configurator/config/Configuration.h
index 7ba2597046..7868e8e815 100644
--- a/Marlin/configurator/config/Configuration.h
+++ b/Marlin/configurator/config/Configuration.h
@@ -740,7 +740,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 // Servo Endstops
 //
 // This allows for servo actuated endstops, primary usage is for the Z Axis to eliminate calibration or bed height changes.
-// Use M206 command to correct for switch height offset to actual nozzle height. Store that setting with M500.
+// Use M851 to set the z-probe vertical offset from the nozzle. Store that setting with M500.
 //
 //#define SERVO_ENDSTOPS {-1, -1, 0} // Servo index for X, Y, Z. Disable with -1
 //#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
diff --git a/Marlin/example_configurations/Felix/Configuration.h b/Marlin/example_configurations/Felix/Configuration.h
index 4500b6c1f5..1cc50246a8 100644
--- a/Marlin/example_configurations/Felix/Configuration.h
+++ b/Marlin/example_configurations/Felix/Configuration.h
@@ -680,7 +680,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 // Servo Endstops
 //
 // This allows for servo actuated endstops, primary usage is for the Z Axis to eliminate calibration or bed height changes.
-// Use M206 command to correct for switch height offset to actual nozzle height. Store that setting with M500.
+// Use M851 to set the z-probe vertical offset from the nozzle. Store that setting with M500.
 //
 //#define SERVO_ENDSTOPS {-1, -1, 0} // Servo index for X, Y, Z. Disable with -1
 //#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
diff --git a/Marlin/example_configurations/Felix/Configuration_DUAL.h b/Marlin/example_configurations/Felix/Configuration_DUAL.h
index d3509a7636..956e2dc472 100644
--- a/Marlin/example_configurations/Felix/Configuration_DUAL.h
+++ b/Marlin/example_configurations/Felix/Configuration_DUAL.h
@@ -680,7 +680,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 // Servo Endstops
 //
 // This allows for servo actuated endstops, primary usage is for the Z Axis to eliminate calibration or bed height changes.
-// Use M206 command to correct for switch height offset to actual nozzle height. Store that setting with M500.
+// Use M851 to set the z-probe vertical offset from the nozzle. Store that setting with M500.
 //
 //#define SERVO_ENDSTOPS {-1, -1, 0} // Servo index for X, Y, Z. Disable with -1
 //#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
diff --git a/Marlin/example_configurations/Hephestos/Configuration.h b/Marlin/example_configurations/Hephestos/Configuration.h
index 0ecf1ed462..594295a3e1 100644
--- a/Marlin/example_configurations/Hephestos/Configuration.h
+++ b/Marlin/example_configurations/Hephestos/Configuration.h
@@ -702,7 +702,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 // Servo Endstops
 //
 // This allows for servo actuated endstops, primary usage is for the Z Axis to eliminate calibration or bed height changes.
-// Use M206 command to correct for switch height offset to actual nozzle height. Store that setting with M500.
+// Use M851 to set the z-probe vertical offset from the nozzle. Store that setting with M500.
 //
 //#define SERVO_ENDSTOPS {-1, -1, 0} // Servo index for X, Y, Z. Disable with -1
 //#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
diff --git a/Marlin/example_configurations/K8200/Configuration.h b/Marlin/example_configurations/K8200/Configuration.h
index 97e514cd97..c17e419c7b 100644
--- a/Marlin/example_configurations/K8200/Configuration.h
+++ b/Marlin/example_configurations/K8200/Configuration.h
@@ -708,7 +708,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 // Servo Endstops
 //
 // This allows for servo actuated endstops, primary usage is for the Z Axis to eliminate calibration or bed height changes.
-// Use M206 command to correct for switch height offset to actual nozzle height. Store that setting with M500.
+// Use M851 to set the z-probe vertical offset from the nozzle. Store that setting with M500.
 //
 //#define SERVO_ENDSTOPS {-1, -1, 0} // Servo index for X, Y, Z. Disable with -1
 //#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
diff --git a/Marlin/example_configurations/SCARA/Configuration.h b/Marlin/example_configurations/SCARA/Configuration.h
index 4c781a8c05..96fef00cfe 100644
--- a/Marlin/example_configurations/SCARA/Configuration.h
+++ b/Marlin/example_configurations/SCARA/Configuration.h
@@ -731,7 +731,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 // Servo Endstops
 //
 // This allows for servo actuated endstops, primary usage is for the Z Axis to eliminate calibration or bed height changes.
-// Use M206 command to correct for switch height offset to actual nozzle height. Store that setting with M500.
+// Use M851 to set the z-probe vertical offset from the nozzle. Store that setting with M500.
 //
 //#define SERVO_ENDSTOPS {-1, -1, 0} // Servo index for X, Y, Z. Disable with -1
 //#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
diff --git a/Marlin/example_configurations/WITBOX/Configuration.h b/Marlin/example_configurations/WITBOX/Configuration.h
index 9b863e3dd7..190270e853 100644
--- a/Marlin/example_configurations/WITBOX/Configuration.h
+++ b/Marlin/example_configurations/WITBOX/Configuration.h
@@ -701,7 +701,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 // Servo Endstops
 //
 // This allows for servo actuated endstops, primary usage is for the Z Axis to eliminate calibration or bed height changes.
-// Use M206 command to correct for switch height offset to actual nozzle height. Store that setting with M500.
+// Use M851 to set the z-probe vertical offset from the nozzle. Store that setting with M500.
 //
 //#define SERVO_ENDSTOPS {-1, -1, 0} // Servo index for X, Y, Z. Disable with -1
 //#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
diff --git a/Marlin/example_configurations/delta/generic/Configuration.h b/Marlin/example_configurations/delta/generic/Configuration.h
index 1c5ef0bcf4..4aeb5d55f4 100644
--- a/Marlin/example_configurations/delta/generic/Configuration.h
+++ b/Marlin/example_configurations/delta/generic/Configuration.h
@@ -755,7 +755,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 // Servo Endstops
 //
 // This allows for servo actuated endstops, primary usage is for the Z Axis to eliminate calibration or bed height changes.
-// Use M206 command to correct for switch height offset to actual nozzle height. Store that setting with M500.
+// Use M851 to set the z-probe vertical offset from the nozzle. Store that setting with M500.
 //
 //#define SERVO_ENDSTOPS {-1, -1, 0} // Servo index for X, Y, Z. Disable with -1
 //#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
diff --git a/Marlin/example_configurations/delta/kossel_mini/Configuration.h b/Marlin/example_configurations/delta/kossel_mini/Configuration.h
index 40d4e4ee29..edb7ff5261 100644
--- a/Marlin/example_configurations/delta/kossel_mini/Configuration.h
+++ b/Marlin/example_configurations/delta/kossel_mini/Configuration.h
@@ -759,7 +759,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 // Servo Endstops
 //
 // This allows for servo actuated endstops, primary usage is for the Z Axis to eliminate calibration or bed height changes.
-// Use M206 command to correct for switch height offset to actual nozzle height. Store that setting with M500.
+// Use M851 to set the z-probe vertical offset from the nozzle. Store that setting with M500.
 //
 //#define SERVO_ENDSTOPS {-1, -1, 0} // Servo index for X, Y, Z. Disable with -1
 //#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
diff --git a/Marlin/example_configurations/makibox/Configuration.h b/Marlin/example_configurations/makibox/Configuration.h
index e8e89234f2..a3e8fd0885 100644
--- a/Marlin/example_configurations/makibox/Configuration.h
+++ b/Marlin/example_configurations/makibox/Configuration.h
@@ -699,7 +699,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 // Servo Endstops
 //
 // This allows for servo actuated endstops, primary usage is for the Z Axis to eliminate calibration or bed height changes.
-// Use M206 command to correct for switch height offset to actual nozzle height. Store that setting with M500.
+// Use M851 to set the z-probe vertical offset from the nozzle. Store that setting with M500.
 //
 //#define SERVO_ENDSTOPS {-1, -1, 0} // Servo index for X, Y, Z. Disable with -1
 //#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
diff --git a/Marlin/example_configurations/tvrrug/Round2/Configuration.h b/Marlin/example_configurations/tvrrug/Round2/Configuration.h
index b37489c896..7ffff33534 100644
--- a/Marlin/example_configurations/tvrrug/Round2/Configuration.h
+++ b/Marlin/example_configurations/tvrrug/Round2/Configuration.h
@@ -705,7 +705,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 // Servo Endstops
 //
 // This allows for servo actuated endstops, primary usage is for the Z Axis to eliminate calibration or bed height changes.
-// Use M206 command to correct for switch height offset to actual nozzle height. Store that setting with M500.
+// Use M851 to set the z-probe vertical offset from the nozzle. Store that setting with M500.
 //
 //#define SERVO_ENDSTOPS {-1, -1, 0} // Servo index for X, Y, Z. Disable with -1
 //#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles

From 9a5a7a3f68e2c5437c1521c75355d902038478c4 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Tue, 31 Mar 2015 03:38:03 -0700
Subject: [PATCH 7/9] Raise when not retracting

Code to raise the probe before retraction is good, but the code should
always raise the probe, not just when retracting
---
 Marlin/Marlin_main.cpp | 17 +++++++++++------
 1 file changed, 11 insertions(+), 6 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 86138fb8f9..95bfc6bf1c 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -1254,18 +1254,18 @@ inline void sync_plan_position() {
 
   }
 
-  static void retract_z_probe(const float z_after=Z_RAISE_AFTER_PROBING) {
+  static void retract_z_probe() {
 
     #ifdef SERVO_ENDSTOPS
 
       // Retract Z Servo endstop if enabled
       if (servo_endstops[Z_AXIS] >= 0) {
 
-        if (z_after > 0) {
-          do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_after);
+        #if Z_RAISE_AFTER_PROBING > 0
+          do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], Z_RAISE_AFTER_PROBING);
           st_synchronize();
-        }
-      
+        #endif
+
         #if SERVO_LEVELING
           servos[servo_endstops[Z_AXIS]].attach(0);
         #endif
@@ -1343,8 +1343,13 @@ inline void sync_plan_position() {
     run_z_probe();
     float measured_z = current_position[Z_AXIS];
 
+    #if Z_RAISE_AFTER_PROBING > 0
+      do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], Z_RAISE_AFTER_PROBING);
+      st_synchronize();
+    #endif
+
     #if !defined(Z_PROBE_SLED) && !defined(Z_PROBE_ALLEN_KEY)
-      if (retract_action & ProbeRetract) retract_z_probe(z_before);
+      if (retract_action & ProbeRetract) retract_z_probe();
     #endif
 
     if (verbose_level > 2) {

From ae89ccd2c7384d900e4cf892ce311e380889ce88 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Tue, 31 Mar 2015 03:45:21 -0700
Subject: [PATCH 8/9] Use Z_RAISE_BETWEEN_PROBINGS

---
 Marlin/Marlin_main.cpp | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 95bfc6bf1c..127f02f0c0 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -1343,9 +1343,11 @@ inline void sync_plan_position() {
     run_z_probe();
     float measured_z = current_position[Z_AXIS];
 
-    #if Z_RAISE_AFTER_PROBING > 0
-      do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], Z_RAISE_AFTER_PROBING);
-      st_synchronize();
+    #if Z_RAISE_BETWEEN_PROBINGS > 0
+      if (retract_action == ProbeStay) {
+        do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], Z_RAISE_BETWEEN_PROBINGS);
+        st_synchronize();
+      }
     #endif
 
     #if !defined(Z_PROBE_SLED) && !defined(Z_PROBE_ALLEN_KEY)

From 97a69d9b1c0db6709ead7e3914a2ceb1071a37ba Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Tue, 31 Mar 2015 04:24:13 -0700
Subject: [PATCH 9/9] Correct XYZ after obtaining plan_bed_level_matrix

The current position needs to be updated by using the inverse
`plan_bed_level_matrix` on X, Y, and Z. All moves in the planner are
transformed by the `plan_bed_level_matrix` from this point forward.
---
 Marlin/Marlin_main.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index b7c2f0dc55..7e9ac15a09 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -1055,7 +1055,7 @@ inline void sync_plan_position() {
         //corrected_position.debug("position after");
         current_position[X_AXIS] = corrected_position.x;
         current_position[Y_AXIS] = corrected_position.y;
-        current_position[Z_AXIS] = zprobe_zoffset; // was: corrected_position.z
+        current_position[Z_AXIS] = corrected_position.z;
 
         sync_plan_position();
       }
@@ -1084,7 +1084,7 @@ inline void sync_plan_position() {
       vector_3 corrected_position = plan_get_position();
       current_position[X_AXIS] = corrected_position.x;
       current_position[Y_AXIS] = corrected_position.y;
-      current_position[Z_AXIS] = zprobe_zoffset; // was: corrected_position.z
+      current_position[Z_AXIS] = corrected_position.z;
 
       sync_plan_position();
     }