From 413b61e64aed70f0fd7afaee7e1018c0ff7a7a43 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Sun, 11 Sep 2016 21:02:37 -0500
Subject: [PATCH 01/26] Add HYPOT2 and float comparison macros

---
 Marlin/macros.h | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/Marlin/macros.h b/Marlin/macros.h
index 351272804d..b098597c52 100644
--- a/Marlin/macros.h
+++ b/Marlin/macros.h
@@ -55,7 +55,8 @@
 #endif
 #define RADIANS(d) ((d)*M_PI/180.0)
 #define DEGREES(r) ((r)*180.0/M_PI)
-#define HYPOT(x,y) sqrt(sq(x)+sq(y))
+#define HYPOT2(x,y) (sq(x)+sq(y))
+#define HYPOT(x,y) sqrt(HYPOT2(x,y))
 
 // Macros to contrain values
 #define NOLESS(v,n) do{ if (v < n) v = n; }while(0)
@@ -124,4 +125,8 @@
 #define MAX3(a, b, c)    max(max(a, b), c)
 #define MAX4(a, b, c, d) max(max(max(a, b), c), d)
 
+#define UNEAR_ZERO(x) ((x) < 0.000001)
+#define NEAR_ZERO(x) ((x) > -0.000001 && (x) < 0.000001)
+#define NEAR(x,y) NEAR_ZERO((x)-(y))
+
 #endif //__MACROS_H

From 21514568fd455f7e61a87c114519495e4483f84a Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Sun, 11 Sep 2016 20:14:41 -0500
Subject: [PATCH 02/26] Add enum for ALL_AXES

---
 Marlin/enum.h | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/Marlin/enum.h b/Marlin/enum.h
index ded1be6ec7..18db5a6f5f 100644
--- a/Marlin/enum.h
+++ b/Marlin/enum.h
@@ -42,7 +42,8 @@ enum AxisEnum {
   E_AXIS  = 3,
   X_HEAD  = 4,
   Y_HEAD  = 5,
-  Z_HEAD  = 6
+  Z_HEAD  = 6,
+  ALL_AXES = 100
 };
 
 #define LOOP_XYZ(VAR)  for (uint8_t VAR=X_AXIS; VAR<=Z_AXIS; VAR++)

From 6ab54c60b151aa52492bfec1a797a5475fb9027e Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Sun, 11 Sep 2016 20:11:30 -0500
Subject: [PATCH 03/26] Add conditionals for kinematics, leveling

---
 Marlin/Conditionals_post.h     |  52 ++++++++++-----
 Marlin/Marlin.h                |   2 +-
 Marlin/Marlin_main.cpp         | 115 ++++++++++++++++++---------------
 Marlin/configuration_store.cpp |   8 +--
 Marlin/planner.cpp             |  10 +--
 Marlin/planner_bezier.cpp      |   2 +-
 Marlin/qr_solve.cpp            |   2 +-
 Marlin/ultralcd.cpp            |   4 +-
 8 files changed, 114 insertions(+), 81 deletions(-)

diff --git a/Marlin/Conditionals_post.h b/Marlin/Conditionals_post.h
index 239780f346..9861c2b295 100644
--- a/Marlin/Conditionals_post.h
+++ b/Marlin/Conditionals_post.h
@@ -61,12 +61,16 @@
     #define NORMAL_AXIS X_AXIS
   #endif
 
+  #define IS_SCARA (ENABLED(MORGAN_SCARA) || ENABLED(MAKERARM_SCARA))
+  #define IS_KINEMATIC (ENABLED(DELTA) || IS_SCARA)
+  #define IS_CARTESIAN !IS_KINEMATIC
+
   /**
-   * SCARA
+   * SCARA cannot use SLOWDOWN and requires QUICKHOME
    */
-  #if ENABLED(SCARA)
+  #if IS_SCARA
     #undef SLOWDOWN
-    #define QUICK_HOME //SCARA needs Quickhome
+    #define QUICK_HOME
   #endif
 
   /**
@@ -132,12 +136,6 @@
 
   #define HOMING_Z_WITH_PROBE (HAS_BED_PROBE && Z_HOME_DIR < 0 && ENABLED(Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN))
 
-  // Boundaries for probing based on set limits
-  #define MIN_PROBE_X (max(X_MIN_POS, X_MIN_POS + X_PROBE_OFFSET_FROM_EXTRUDER))
-  #define MAX_PROBE_X (min(X_MAX_POS, X_MAX_POS + X_PROBE_OFFSET_FROM_EXTRUDER))
-  #define MIN_PROBE_Y (max(Y_MIN_POS, Y_MIN_POS + Y_PROBE_OFFSET_FROM_EXTRUDER))
-  #define MAX_PROBE_Y (min(Y_MAX_POS, Y_MAX_POS + Y_PROBE_OFFSET_FROM_EXTRUDER))
-
   #define HAS_Z_SERVO_ENDSTOP (defined(Z_ENDSTOP_SERVO_NR) && Z_ENDSTOP_SERVO_NR >= 0)
 
   /**
@@ -657,18 +655,28 @@
     #ifndef DELTA_DIAGONAL_ROD_TRIM_TOWER_3
       #define DELTA_DIAGONAL_ROD_TRIM_TOWER_3 0.0
     #endif
-    #if ENABLED(AUTO_BED_LEVELING_GRID)
-      #define DELTA_BED_LEVELING_GRID
-    #endif
   #endif
 
   /**
-   * When not using other bed leveling...
+   * Specify the exact style of auto bed leveling
+   *
+   *  3POINT    - 3 Point Probing with the least-squares solution.
+   *  LINEAR    - Grid Probing with the least-squares solution.
+   *  NONLINEAR - Grid Probing with a mesh solution. Best for large beds.
    */
-  #if ENABLED(AUTO_BED_LEVELING_FEATURE) && DISABLED(AUTO_BED_LEVELING_GRID) && DISABLED(DELTA_BED_LEVELING_GRID)
-    #define AUTO_BED_LEVELING_3POINT
+  #if ENABLED(AUTO_BED_LEVELING_FEATURE)
+    #if DISABLED(AUTO_BED_LEVELING_GRID)
+      #define AUTO_BED_LEVELING_LINEAR
+      #define AUTO_BED_LEVELING_3POINT
+    #elif IS_KINEMATIC
+      #define AUTO_BED_LEVELING_NONLINEAR
+    #else
+      #define AUTO_BED_LEVELING_LINEAR
+    #endif
   #endif
 
+  #define PLANNER_LEVELING (ENABLED(MESH_BED_LEVELING) || ENABLED(AUTO_BED_LEVELING_LINEAR))
+
   /**
    * Buzzer/Speaker
    */
@@ -702,4 +710,18 @@
     #define Z_PROBE_TRAVEL_HEIGHT Z_HOMING_HEIGHT
   #endif
 
+  #if IS_KINEMATIC
+    // Check for this in the code instead
+    #define MIN_PROBE_X X_MIN_POS
+    #define MAX_PROBE_X X_MAX_POS
+    #define MIN_PROBE_Y Y_MIN_POS
+    #define MAX_PROBE_Y Y_MAX_POS
+  #else
+    // Boundaries for probing based on set limits
+    #define MIN_PROBE_X (max(X_MIN_POS, X_MIN_POS + X_PROBE_OFFSET_FROM_EXTRUDER))
+    #define MAX_PROBE_X (min(X_MAX_POS, X_MAX_POS + X_PROBE_OFFSET_FROM_EXTRUDER))
+    #define MIN_PROBE_Y (max(Y_MIN_POS, Y_MIN_POS + Y_PROBE_OFFSET_FROM_EXTRUDER))
+    #define MAX_PROBE_Y (min(Y_MAX_POS, Y_MAX_POS + Y_PROBE_OFFSET_FROM_EXTRUDER))
+  #endif
+
 #endif // CONDITIONALS_POST_H
diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h
index da9bc0de90..e0a540c263 100644
--- a/Marlin/Marlin.h
+++ b/Marlin/Marlin.h
@@ -313,7 +313,7 @@ float code_value_temp_diff();
     extern int delta_grid_spacing[2];
     void adjust_delta(float cartesian[XYZ]);
   #endif
-#elif ENABLED(SCARA)
+#elif IS_SCARA
   extern float delta[ABC];
   extern float axis_scaling[ABC];  // Build size scaling
   void inverse_kinematics(const float cartesian[XYZ]);
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 4ea114504f..73376b4d1c 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -36,12 +36,11 @@
 
 #if ENABLED(AUTO_BED_LEVELING_FEATURE)
   #include "vector_3.h"
-  #if ENABLED(AUTO_BED_LEVELING_GRID)
-    #include "qr_solve.h"
-  #endif
-#endif // AUTO_BED_LEVELING_FEATURE
+#endif
 
-#if ENABLED(MESH_BED_LEVELING)
+#if ENABLED(AUTO_BED_LEVELING_LINEAR)
+  #include "qr_solve.h"
+#elif ENABLED(MESH_BED_LEVELING)
   #include "mesh_bed_leveling.h"
 #endif
 
@@ -497,7 +496,12 @@ static uint8_t target_extruder;
 
 #endif
 
-#if ENABLED(SCARA)
+#if IS_SCARA
+  // Float constants for SCARA calculations
+  const float L1 = SCARA_LINKAGE_1, L2 = SCARA_LINKAGE_2,
+              L1_2 = sq(float(L1)), L1_2_2 = 2.0 * L1_2,
+              L2_2 = sq(float(L2));
+
   float delta_segments_per_second = SCARA_SEGMENTS_PER_SECOND,
         delta[ABC],
         axis_scaling[ABC] = { 1, 1, 1 },    // Build size scaling, default to 1
@@ -651,7 +655,7 @@ inline void sync_plan_position() {
 }
 inline void sync_plan_position_e() { planner.set_e_position_mm(current_position[E_AXIS]); }
 
-#if ENABLED(DELTA) || ENABLED(SCARA)
+#if IS_KINEMATIC
   inline void sync_plan_position_delta() {
     #if ENABLED(DEBUG_LEVELING_FEATURE)
       if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position_delta", current_position);
@@ -2161,7 +2165,7 @@ static void clean_up_after_endstop_or_probe_move() {
     // Prevent stepper_inactive_time from running out and EXTRUDER_RUNOUT_PREVENT from extruding
     refresh_cmd_timeout();
 
-    #if ENABLED(AUTO_BED_LEVELING_FEATURE)
+    #if ENABLED(AUTO_BED_LEVELING_LINEAR)
       planner.bed_level_matrix.set_to_identity();
     #endif
 
@@ -2272,7 +2276,7 @@ static void clean_up_after_endstop_or_probe_move() {
 
 #if ENABLED(AUTO_BED_LEVELING_FEATURE)
 
-  #if DISABLED(DELTA)
+  #if ENABLED(AUTO_BED_LEVELING_LINEAR)
 
     /**
      * Get the stepper positions, apply the rotation matrix
@@ -2302,9 +2306,7 @@ static void clean_up_after_endstop_or_probe_move() {
       return pos;
     }
 
-  #endif // !DELTA
-
-  #if ENABLED(DELTA)
+  #elif ENABLED(AUTO_BED_LEVELING_NONLINEAR)
 
     /**
      * All DELTA leveling in the Marlin uses NONLINEAR_BED_LEVELING
@@ -2870,7 +2872,7 @@ inline void gcode_G4() {
     SERIAL_ECHOPGM("Machine Type: ");
     #if ENABLED(DELTA)
       SERIAL_ECHOLNPGM("Delta");
-    #elif ENABLED(SCARA)
+    #elif IS_SCARA
       SERIAL_ECHOLNPGM("SCARA");
     #elif ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ)
       SERIAL_ECHOLNPGM("Core");
@@ -2947,11 +2949,12 @@ inline void gcode_G28() {
   stepper.synchronize();
 
   // For auto bed leveling, clear the level matrix
-  #if ENABLED(AUTO_BED_LEVELING_FEATURE)
+  #if ENABLED(AUTO_BED_LEVELING_LINEAR)
     planner.bed_level_matrix.set_to_identity();
-    #if ENABLED(DELTA)
-      reset_bed_level();
-    #endif
+  #endif
+
+  #if ENABLED(AUTO_BED_LEVELING_NONLINEAR)
+    reset_bed_level();
   #endif
 
   // Always home with tool 0 active
@@ -3533,7 +3536,7 @@ inline void gcode_G28() {
 
     #if ENABLED(AUTO_BED_LEVELING_GRID)
 
-      #if DISABLED(DELTA)
+      #if ENABLED(AUTO_BED_LEVELING_LINEAR)
         bool do_topography_map = verbose_level > 2 || code_seen('T');
       #endif
 
@@ -3544,7 +3547,7 @@ inline void gcode_G28() {
 
       int auto_bed_leveling_grid_points = AUTO_BED_LEVELING_GRID_POINTS;
 
-      #if DISABLED(DELTA)
+      #if ENABLED(AUTO_BED_LEVELING_LINEAR)
         if (code_seen('P')) auto_bed_leveling_grid_points = code_value_int();
         if (auto_bed_leveling_grid_points < 2) {
           SERIAL_PROTOCOLLNPGM("?Number of probed (P)oints is implausible (2 minimum).");
@@ -3594,17 +3597,19 @@ inline void gcode_G28() {
 
     if (!dryrun) {
 
-      // Reset the bed_level_matrix because leveling
-      // needs to be done without leveling enabled.
-      planner.bed_level_matrix.set_to_identity();
+      #if ENABLED(AUTO_BED_LEVELING_LINEAR)
+        // Reset the bed_level_matrix because leveling
+        // needs to be done without leveling enabled.
+        planner.bed_level_matrix.set_to_identity();
+      #endif
 
       //
       // Re-orient the current position without leveling
       // based on where the steppers are positioned.
       //
-      #if ENABLED(DELTA) || ENABLED(SCARA)
+      #if IS_KINEMATIC
 
-        #if ENABLED(DELTA)
+        #if ENABLED(AUTO_BED_LEVELING_NONLINEAR)
           reset_bed_level();
         #endif
 
@@ -3639,12 +3644,14 @@ inline void gcode_G28() {
       const float xGridSpacing = (right_probe_bed_position - left_probe_bed_position) / (auto_bed_leveling_grid_points - 1),
                   yGridSpacing = (back_probe_bed_position - front_probe_bed_position) / (auto_bed_leveling_grid_points - 1);
 
-      #if ENABLED(DELTA)
+      #if ENABLED(AUTO_BED_LEVELING_NONLINEAR)
         delta_grid_spacing[X_AXIS] = xGridSpacing;
         delta_grid_spacing[Y_AXIS] = yGridSpacing;
         float zoffset = zprobe_zoffset;
         if (code_seen('Z')) zoffset += code_value_axis_units(Z_AXIS);
-      #else // !DELTA
+
+      #elif ENABLED(AUTO_BED_LEVELING_LINEAR)
+
         /**
          * solve the plane equation ax + by + d = z
          * A is the matrix with rows [x y 1] for all the probed points
@@ -3660,7 +3667,8 @@ inline void gcode_G28() {
                eqnBVector[abl2],     // "B" vector of Z points
                mean = 0.0;
         int8_t indexIntoAB[auto_bed_leveling_grid_points][auto_bed_leveling_grid_points];
-      #endif // !DELTA
+
+      #endif // AUTO_BED_LEVELING_LINEAR
 
       int probePointCounter = 0;
       bool zig = auto_bed_leveling_grid_points & 1; //always end at [RIGHT_PROBE_BED_POSITION, BACK_PROBE_BED_POSITION]
@@ -3694,16 +3702,19 @@ inline void gcode_G28() {
 
           float measured_z = probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level);
 
-          #if DISABLED(DELTA)
-            mean += measured_z;
+          #if ENABLED(AUTO_BED_LEVELING_LINEAR)
 
+            mean += measured_z;
             eqnBVector[probePointCounter] = measured_z;
             eqnAMatrix[probePointCounter + 0 * abl2] = xProbe;
             eqnAMatrix[probePointCounter + 1 * abl2] = yProbe;
             eqnAMatrix[probePointCounter + 2 * abl2] = 1;
             indexIntoAB[xCount][yCount] = probePointCounter;
-          #else
+
+          #elif ENABLED(AUTO_BED_LEVELING_NONLINEAR)
+
             bed_level[xCount][yCount] = measured_z + zoffset;
+
           #endif
 
           probePointCounter++;
@@ -3713,7 +3724,7 @@ inline void gcode_G28() {
         } //xProbe
       } //yProbe
 
-    #else // !AUTO_BED_LEVELING_GRID
+    #elif ENABLED(AUTO_BED_LEVELING_3POINT)
 
       #if ENABLED(DEBUG_LEVELING_FEATURE)
         if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("> 3-point Leveling");
@@ -3759,12 +3770,12 @@ inline void gcode_G28() {
 
     // Calculate leveling, print reports, correct the position
     #if ENABLED(AUTO_BED_LEVELING_GRID)
-      #if ENABLED(DELTA)
+      #if ENABLED(AUTO_BED_LEVELING_NONLINEAR)
 
         if (!dryrun) extrapolate_unprobed_bed_level();
         print_bed_level();
 
-      #else // !DELTA
+      #elif ENABLED(AUTO_BED_LEVELING_LINEAR)
 
         // solve lsq problem
         double plane_equation_coefficients[3];
@@ -3860,11 +3871,11 @@ inline void gcode_G28() {
           }
         } //do_topography_map
 
-      #endif //!DELTA
+      #endif // AUTO_BED_LEVELING_LINEAR
 
     #endif // AUTO_BED_LEVELING_GRID
 
-    #if DISABLED(DELTA)
+    #if ENABLED(AUTO_BED_LEVELING_LINEAR)
 
       if (verbose_level > 0)
         planner.bed_level_matrix.debug("\n\nBed Level Correction Matrix:");
@@ -4358,10 +4369,10 @@ inline void gcode_M42() {
     if (verbose_level > 2)
       SERIAL_PROTOCOLLNPGM("Positioning the probe...");
 
-    #if ENABLED(DELTA)
+    #if ENABLED(AUTO_BED_LEVELING_NONLINEAR)
       // we don't do bed level correction in M48 because we want the raw data when we probe
       reset_bed_level();
-    #elif ENABLED(AUTO_BED_LEVELING_FEATURE)
+    #elif ENABLED(AUTO_BED_LEVELING_LINEAR)
       // we don't do bed level correction in M48 because we want the raw data when we probe
       planner.bed_level_matrix.set_to_identity();
     #endif
@@ -6361,7 +6372,7 @@ inline void gcode_M503() {
       lastpos[i] = destination[i] = current_position[i];
 
     // Define runplan for move axes
-    #if ENABLED(DELTA)
+    #if IS_KINEMATIC
       #define RUNPLAN(RATE_MM_S) inverse_kinematics(destination); \
                                  planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], RATE_MM_S, active_extruder);
     #else
@@ -6482,7 +6493,7 @@ inline void gcode_M503() {
     destination[E_AXIS] = lastpos[E_AXIS];
     planner.set_e_position_mm(current_position[E_AXIS]);
 
-    #if ENABLED(DELTA)
+    #if IS_KINEMATIC
       // Move XYZ to starting position, then E
       inverse_kinematics(lastpos);
       planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], FILAMENT_CHANGE_XY_FEEDRATE, active_extruder);
@@ -6925,7 +6936,7 @@ void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool n
            * Z software endstop. But this is technically correct (and
            * there is no viable alternative).
            */
-          #if ENABLED(AUTO_BED_LEVELING_FEATURE)
+          #if ENABLED(AUTO_BED_LEVELING_LINEAR)
             // Offset extruder, make sure to apply the bed level rotation matrix
             vector_3 tmp_offset_vec = vector_3(hotend_offset[X_AXIS][tmp_extruder],
                                                hotend_offset[Y_AXIS][tmp_extruder],
@@ -7961,7 +7972,7 @@ void ok_to_send() {
                              stepper.get_axis_position_mm(C_AXIS));
   }
 
-  #if ENABLED(AUTO_BED_LEVELING_FEATURE)
+  #if ENABLED(AUTO_BED_LEVELING_NONLINEAR)
 
     // Adjust print surface height by linear interpolation over the bed_level array.
     void adjust_delta(float cartesian[XYZ]) {
@@ -8001,7 +8012,7 @@ void ok_to_send() {
       SERIAL_ECHOPGM(" offset="); SERIAL_ECHOLN(offset);
       */
     }
-  #endif // AUTO_BED_LEVELING_FEATURE
+  #endif // AUTO_BED_LEVELING_NONLINEAR
 
 #endif // DELTA
 
@@ -8076,7 +8087,7 @@ void mesh_line_to_destination(float fr_mm_s, uint8_t x_splits = 0xff, uint8_t y_
 }
 #endif  // MESH_BED_LEVELING
 
-#if ENABLED(DELTA) || ENABLED(SCARA)
+#if IS_KINEMATIC
 
   inline bool prepare_kinematic_move_to(float target[NUM_AXIS]) {
     float difference[NUM_AXIS];
@@ -8103,7 +8114,7 @@ void mesh_line_to_destination(float fr_mm_s, uint8_t x_splits = 0xff, uint8_t y_
 
       inverse_kinematics(target);
 
-      #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_FEATURE)
+      #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_NONLINEAR)
         if (!bed_leveling_in_progress) adjust_delta(target);
       #endif
 
@@ -8115,7 +8126,7 @@ void mesh_line_to_destination(float fr_mm_s, uint8_t x_splits = 0xff, uint8_t y_
     return true;
   }
 
-#endif // DELTA || SCARA
+#endif // IS_KINEMATIC
 
 #if ENABLED(DUAL_X_CARRIAGE)
 
@@ -8161,7 +8172,7 @@ void mesh_line_to_destination(float fr_mm_s, uint8_t x_splits = 0xff, uint8_t y_
 
 #endif // DUAL_X_CARRIAGE
 
-#if DISABLED(DELTA) && DISABLED(SCARA)
+#if !IS_KINEMATIC
 
   inline bool prepare_move_to_destination_cartesian() {
     // Do not use feedrate_percentage for E or Z only moves
@@ -8181,7 +8192,7 @@ void mesh_line_to_destination(float fr_mm_s, uint8_t x_splits = 0xff, uint8_t y_
     return true;
   }
 
-#endif // !DELTA && !SCARA
+#endif // !IS_KINEMATIC
 
 #if ENABLED(PREVENT_COLD_EXTRUSION)
 
@@ -8220,7 +8231,7 @@ void prepare_move_to_destination() {
     prevent_dangerous_extrude(current_position[E_AXIS], destination[E_AXIS]);
   #endif
 
-  #if ENABLED(DELTA) || ENABLED(SCARA)
+  #if IS_KINEMATIC
     if (!prepare_kinematic_move_to(destination)) return;
   #else
     #if ENABLED(DUAL_X_CARRIAGE)
@@ -8356,9 +8367,9 @@ void prepare_move_to_destination() {
 
       clamp_to_software_endstops(arc_target);
 
-      #if ENABLED(DELTA) || ENABLED(SCARA)
+      #if IS_KINEMATIC
         inverse_kinematics(arc_target);
-        #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_FEATURE)
+        #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_NONLINEAR)
           adjust_delta(arc_target);
         #endif
         planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], arc_target[E_AXIS], fr_mm_s, active_extruder);
@@ -8368,9 +8379,9 @@ void prepare_move_to_destination() {
     }
 
     // Ensure last segment arrives at target location.
-    #if ENABLED(DELTA) || ENABLED(SCARA)
+    #if IS_KINEMATIC
       inverse_kinematics(target);
-      #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_FEATURE)
+      #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_NONLINEAR)
         adjust_delta(target);
       #endif
       planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], target[E_AXIS], fr_mm_s, active_extruder);
diff --git a/Marlin/configuration_store.cpp b/Marlin/configuration_store.cpp
index f54cd88a2f..0dd1ce6da4 100644
--- a/Marlin/configuration_store.cpp
+++ b/Marlin/configuration_store.cpp
@@ -330,7 +330,7 @@ void Config_StoreSettings()  {
   #endif
   EEPROM_WRITE(lcd_contrast);
 
-  #if ENABLED(SCARA)
+  #if IS_SCARA
     EEPROM_WRITE(axis_scaling); // 3 floats
   #else
     dummy = 1.0f;
@@ -520,7 +520,7 @@ void Config_RetrieveSettings() {
     #endif
     EEPROM_READ(lcd_contrast);
 
-    #if ENABLED(SCARA)
+    #if IS_SCARA
       EEPROM_READ(axis_scaling);  // 3 floats
     #else
       EEPROM_READ(dummy);
@@ -584,7 +584,7 @@ void Config_ResetDefault() {
     planner.axis_steps_per_mm[i] = tmp1[i];
     planner.max_feedrate_mm_s[i] = tmp2[i];
     planner.max_acceleration_mm_per_s2[i] = tmp3[i];
-    #if ENABLED(SCARA)
+    #if IS_SCARA
       if (i < COUNT(axis_scaling))
         axis_scaling[i] = 1;
     #endif
@@ -716,7 +716,7 @@ void Config_PrintSettings(bool forReplay) {
 
   CONFIG_ECHO_START;
 
-  #if ENABLED(SCARA)
+  #if IS_SCARA
     if (!forReplay) {
       SERIAL_ECHOLNPGM("Scaling factors:");
       CONFIG_ECHO_START;
diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index bcad2c9069..f8dc5efd2b 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -98,7 +98,7 @@ float Planner::min_feedrate_mm_s,
       Planner::max_e_jerk,
       Planner::min_travel_feedrate_mm_s;
 
-#if ENABLED(AUTO_BED_LEVELING_FEATURE)
+#if ENABLED(AUTO_BED_LEVELING_LINEAR)
   matrix_3x3 Planner::bed_level_matrix; // Transform to compensate for bed level
 #endif
 
@@ -138,7 +138,7 @@ void Planner::init() {
   memset(position, 0, sizeof(position)); // clear position
   LOOP_XYZE(i) previous_speed[i] = 0.0;
   previous_nominal_speed = 0.0;
-  #if ENABLED(AUTO_BED_LEVELING_FEATURE)
+  #if ENABLED(AUTO_BED_LEVELING_LINEAR)
     bed_level_matrix.set_to_identity();
   #endif
 }
@@ -521,7 +521,7 @@ void Planner::check_axes_activity() {
   #endif
 }
 
-#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
+#if PLANNER_LEVELING
 
   void Planner::apply_leveling(
     #if ENABLED(MESH_BED_LEVELING)
@@ -551,7 +551,7 @@ void Planner::check_axes_activity() {
     #endif
   }
 
-#endif
+#endif // PLANNER_LEVELING
 
 /**
  * Planner::buffer_line
@@ -1193,7 +1193,7 @@ void Planner::reset_acceleration_rates() {
 // Recalculate position, steps_to_mm if axis_steps_per_mm changes!
 void Planner::refresh_positioning() {
   LOOP_XYZE(i) steps_to_mm[i] = 1.0 / axis_steps_per_mm[i];
-  #if ENABLED(DELTA) || ENABLED(SCARA)
+  #if IS_KINEMATIC
     inverse_kinematics(current_position);
     set_position_mm(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
   #else
diff --git a/Marlin/planner_bezier.cpp b/Marlin/planner_bezier.cpp
index 6ca7afd1d6..15c8091635 100644
--- a/Marlin/planner_bezier.cpp
+++ b/Marlin/planner_bezier.cpp
@@ -188,7 +188,7 @@ void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS]
     bez_target[E_AXIS] = interp(position[E_AXIS], target[E_AXIS], t);
     clamp_to_software_endstops(bez_target);
 
-    #if ENABLED(DELTA) || ENABLED(SCARA)
+    #if IS_KINEMATIC
       inverse_kinematics(bez_target);
       #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_FEATURE)
         adjust_delta(bez_target);
diff --git a/Marlin/qr_solve.cpp b/Marlin/qr_solve.cpp
index ddafb005ea..e60b1d3274 100644
--- a/Marlin/qr_solve.cpp
+++ b/Marlin/qr_solve.cpp
@@ -22,7 +22,7 @@
 
 #include "qr_solve.h"
 
-#if ENABLED(AUTO_BED_LEVELING_GRID)
+#if ENABLED(AUTO_BED_LEVELING_LINEAR)
 
 #include <stdlib.h>
 #include <math.h>
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index f5459757db..55247c15d5 100755
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -1418,7 +1418,7 @@ void kill_screen(const char* lcd_msg) {
    *
    */
 
-  #if ENABLED(DELTA) || ENABLED(SCARA)
+  #if IS_KINEMATIC
     #define _MOVE_XYZ_ALLOWED (axis_homed[X_AXIS] && axis_homed[Y_AXIS] && axis_homed[Z_AXIS])
   #else
     #define _MOVE_XYZ_ALLOWED true
@@ -1823,7 +1823,7 @@ void kill_screen(const char* lcd_msg) {
     #if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
       MENU_ITEM_EDIT(bool, MSG_ENDSTOP_ABORT, &stepper.abort_on_endstop_hit);
     #endif
-    #if ENABLED(SCARA)
+    #if IS_SCARA
       MENU_ITEM_EDIT(float74, MSG_XSCALE, &axis_scaling[X_AXIS], 0.5, 2);
       MENU_ITEM_EDIT(float74, MSG_YSCALE, &axis_scaling[Y_AXIS], 0.5, 2);
     #endif

From 74d7f5e57b0f2561f6ff7f768afd9efff1b4c4a5 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Sun, 11 Sep 2016 20:11:39 -0500
Subject: [PATCH 04/26] Patch SCARA example config

---
 Marlin/Marlin_main.cpp                        | 69 ++++++++-----------
 Marlin/SanityCheck.h                          |  9 ++-
 .../SCARA/Configuration.h                     | 42 +++++------
 3 files changed, 58 insertions(+), 62 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 73376b4d1c..cc17633724 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -8457,18 +8457,18 @@ void prepare_move_to_destination() {
     //SERIAL_ECHOPGM("f_delta x="); SERIAL_ECHO(f_scara[X_AXIS]);
     //SERIAL_ECHOPGM(" y="); SERIAL_ECHO(f_scara[Y_AXIS]);
 
-    x_sin = sin(f_scara[X_AXIS] / SCARA_RAD2DEG) * Linkage_1;
-    x_cos = cos(f_scara[X_AXIS] / SCARA_RAD2DEG) * Linkage_1;
-    y_sin = sin(f_scara[Y_AXIS] / SCARA_RAD2DEG) * Linkage_2;
-    y_cos = cos(f_scara[Y_AXIS] / SCARA_RAD2DEG) * Linkage_2;
+    x_sin = sin(RADIANS(f_scara[X_AXIS])) * L1;
+    x_cos = cos(RADIANS(f_scara[X_AXIS])) * L1;
+    y_sin = sin(RADIANS(f_scara[Y_AXIS])) * L2;
+    y_cos = cos(RADIANS(f_scara[Y_AXIS])) * L2;
 
     //SERIAL_ECHOPGM(" x_sin="); SERIAL_ECHO(x_sin);
     //SERIAL_ECHOPGM(" x_cos="); SERIAL_ECHO(x_cos);
     //SERIAL_ECHOPGM(" y_sin="); SERIAL_ECHO(y_sin);
     //SERIAL_ECHOPGM(" y_cos="); SERIAL_ECHOLN(y_cos);
 
-    delta[X_AXIS] = x_cos + y_cos + SCARA_offset_x;  //theta
-    delta[Y_AXIS] = x_sin + y_sin + SCARA_offset_y;  //theta+phi
+    delta[X_AXIS] = x_cos + y_cos + SCARA_OFFSET_X;  //theta
+    delta[Y_AXIS] = x_sin + y_sin + SCARA_OFFSET_Y;  //theta+phi
 
     //SERIAL_ECHOPGM(" delta[X_AXIS]="); SERIAL_ECHO(delta[X_AXIS]);
     //SERIAL_ECHOPGM(" delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]);
@@ -8480,51 +8480,42 @@ void prepare_move_to_destination() {
     // The maths and first version were done by QHARLEY.
     // Integrated, tweaked by Joachim Cerny in June 2014.
 
-    float SCARA_pos[2];
-    static float SCARA_C2, SCARA_S2, SCARA_K1, SCARA_K2, SCARA_theta, SCARA_psi;
+    static float C2, S2, SK1, SK2, THETA, PSI;
 
-    SCARA_pos[X_AXIS] = RAW_X_POSITION(cartesian[X_AXIS]) * axis_scaling[X_AXIS] - SCARA_offset_x;  //Translate SCARA to standard X Y
-    SCARA_pos[Y_AXIS] = RAW_Y_POSITION(cartesian[Y_AXIS]) * axis_scaling[Y_AXIS] - SCARA_offset_y;  // With scaling factor.
+    float sx = RAW_X_POSITION(cartesian[X_AXIS]) * axis_scaling[X_AXIS] - SCARA_OFFSET_X,  //Translate SCARA to standard X Y
+          sy = RAW_Y_POSITION(cartesian[Y_AXIS]) * axis_scaling[Y_AXIS] - SCARA_OFFSET_Y;  // With scaling factor.
 
-    #if (Linkage_1 == Linkage_2)
-      SCARA_C2 = ((sq(SCARA_pos[X_AXIS]) + sq(SCARA_pos[Y_AXIS])) / (2 * (float)L1_2)) - 1;
+    #if (L1 == L2)
+      C2 = HYPOT2(sx, sy) / (2 * L1_2) - 1;
     #else
-      SCARA_C2 = (sq(SCARA_pos[X_AXIS]) + sq(SCARA_pos[Y_AXIS]) - (float)L1_2 - (float)L2_2) / 45000;
+      C2 = (HYPOT2(sx, sy) - L1_2 - L2_2) / 45000;
     #endif
 
-    SCARA_S2 = sqrt(1 - sq(SCARA_C2));
+    S2 = sqrt(1 - sq(C2));
 
-    SCARA_K1 = Linkage_1 + Linkage_2 * SCARA_C2;
-    SCARA_K2 = Linkage_2 * SCARA_S2;
+    SK1 = L1 + L2 * C2;
+    SK2 = L2 * S2;
 
-    SCARA_theta = (atan2(SCARA_pos[X_AXIS], SCARA_pos[Y_AXIS]) - atan2(SCARA_K1, SCARA_K2)) * -1;
-    SCARA_psi = atan2(SCARA_S2, SCARA_C2);
+    THETA = (atan2(sx, sy) - atan2(SK1, SK2)) * -1;
+    PSI = atan2(S2, C2);
 
-    delta[X_AXIS] = SCARA_theta * SCARA_RAD2DEG;  // Multiply by 180/Pi  -  theta is support arm angle
-    delta[Y_AXIS] = (SCARA_theta + SCARA_psi) * SCARA_RAD2DEG;  //       -  equal to sub arm angle (inverted motor)
-    delta[Z_AXIS] = RAW_Z_POSITION(cartesian[Z_AXIS]);
+    delta[A_AXIS] = DEGREES(THETA);        // theta is support arm angle
+    delta[B_AXIS] = DEGREES(THETA + PSI);  // equal to sub arm angle (inverted motor)
+    delta[Z_AXIS] = cartesian[Z_AXIS];
 
     /**
-    SERIAL_ECHOPGM("cartesian x="); SERIAL_ECHO(cartesian[X_AXIS]);
-    SERIAL_ECHOPGM(" y="); SERIAL_ECHO(cartesian[Y_AXIS]);
-    SERIAL_ECHOPGM(" z="); SERIAL_ECHOLN(cartesian[Z_AXIS]);
-
-    SERIAL_ECHOPGM("scara x="); SERIAL_ECHO(SCARA_pos[X_AXIS]);
-    SERIAL_ECHOPGM(" y="); SERIAL_ECHOLN(SCARA_pos[Y_AXIS]);
-
-    SERIAL_ECHOPGM("delta x="); SERIAL_ECHO(delta[X_AXIS]);
-    SERIAL_ECHOPGM(" y="); SERIAL_ECHO(delta[Y_AXIS]);
-    SERIAL_ECHOPGM(" z="); SERIAL_ECHOLN(delta[Z_AXIS]);
-
-    SERIAL_ECHOPGM("C2="); SERIAL_ECHO(SCARA_C2);
-    SERIAL_ECHOPGM(" S2="); SERIAL_ECHO(SCARA_S2);
-    SERIAL_ECHOPGM(" Theta="); SERIAL_ECHO(SCARA_theta);
-    SERIAL_ECHOPGM(" Psi="); SERIAL_ECHOLN(SCARA_psi);
-    SERIAL_EOL;
-    */
+      DEBUG_POS("SCARA IK", cartesian);
+      DEBUG_POS("SCARA IK", delta);
+      SERIAL_ECHOPAIR("  SCARA (x,y) ", sx);
+      SERIAL_ECHOPAIR(",", sy);
+      SERIAL_ECHOPAIR(" C2=", C2);
+      SERIAL_ECHOPAIR(" S2=", S2);
+      SERIAL_ECHOPAIR(" Theta=", THETA);
+      SERIAL_ECHOLNPAIR(" Phi=", PHI);
+    //*/
   }
 
-#endif // SCARA
+#endif // MORGAN_SCARA
 
 #if ENABLED(TEMP_STAT_LEDS)
 
diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h
index 0234baee1c..d04ae6992c 100644
--- a/Marlin/SanityCheck.h
+++ b/Marlin/SanityCheck.h
@@ -137,6 +137,8 @@
   #error Please replace "const int dropsegments" with "#define MIN_STEPS_PER_SEGMENT" (and increase by 1) in Configuration_adv.h.
 #elif defined(PREVENT_DANGEROUS_EXTRUDE)
   #error "PREVENT_DANGEROUS_EXTRUDE is now PREVENT_COLD_EXTRUSION. Please update your configuration."
+#elif defined(SCARA)
+  #error "SCARA is now MORGAN_SCARA. Please update your configuration."
 #endif
 
 /**
@@ -573,11 +575,12 @@
 /**
  * Don't set more than one kinematic type
  */
-#if (ENABLED(DELTA) && (ENABLED(SCARA) || ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ))) \
+#if (ENABLED(DELTA) && (ENABLED(MORGAN_SCARA) || ENABLED(MAKERARM_SCARA) || ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ))) \
+ || (ENABLED(DELTA) && (ENABLED(MAKERARM_SCARA) || ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ))) \
  || (ENABLED(SCARA) && (ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ))) \
  || (ENABLED(COREXY) && (ENABLED(COREXZ) || ENABLED(COREYZ))) \
  || (ENABLED(COREXZ) && ENABLED(COREYZ))
-  #error "Please enable only one of DELTA, SCARA, COREXY, COREXZ, or COREYZ."
+  #error "Please enable only one of DELTA, MORGAN_SCARA, MAKERARM_SCARA, COREXY, COREXZ, or COREYZ."
 #endif
 
 /**
@@ -750,7 +753,7 @@
   #elif ENABLED(DELTA)
     #error "Z_DUAL_ENDSTOPS is not compatible with DELTA."
   #endif
-#elif DISABLED(SCARA)
+#elif !IS_SCARA
   #if X_HOME_DIR < 0 && DISABLED(USE_XMIN_PLUG)
     #error "Enable USE_XMIN_PLUG when homing X to MIN."
   #elif X_HOME_DIR > 0 && DISABLED(USE_XMAX_PLUG)
diff --git a/Marlin/example_configurations/SCARA/Configuration.h b/Marlin/example_configurations/SCARA/Configuration.h
index 085880e210..3c2e96a696 100644
--- a/Marlin/example_configurations/SCARA/Configuration.h
+++ b/Marlin/example_configurations/SCARA/Configuration.h
@@ -75,35 +75,37 @@
 //
 
 //===========================================================================
-//========================= SCARA Settings ==================================
+//============================= SCARA Printer ===============================
 //===========================================================================
-// SCARA-mode for Marlin has been developed by QHARLEY in ZA in 2012/2013. Implemented
+// MORGAN_SCARA for Marlin was developed by QHARLEY in ZA in 2012/2013. Implemented
 // and slightly reworked by JCERNY in 06/2014 with the goal to bring it into Master-Branch
 // QHARLEYS Autobedlevelling has not been ported, because Marlin has now Bed-levelling
 // You might need Z-Min endstop on SCARA-Printer to use this feature. Actually untested!
-// Uncomment to use Morgan scara mode
-#define SCARA
-#define SCARA_SEGMENTS_PER_SECOND 200 // If movement is choppy try lowering this value
-// Length of inner support arm
-#define Linkage_1 150 //mm      Preprocessor cannot handle decimal point...
-// Length of outer support arm     Measure arm lengths precisely and enter
-#define Linkage_2 150 //mm
 
-// SCARA tower offset (position of Tower relative to bed zero position)
-// This needs to be reasonably accurate as it defines the printbed position in the SCARA space.
-#define SCARA_offset_x 100 //mm
-#define SCARA_offset_y -56 //mm
-#define SCARA_RAD2DEG 57.2957795  // to convert RAD to degrees
+// Specify the specific SCARA model
+#define MORGAN_SCARA
+//#define MAKERARM_SCARA
 
-#define THETA_HOMING_OFFSET 0  //calculatated from Calibration Guide and command M360 / M114 see picture in http://reprap.harleystudio.co.za/?page_id=1073
-#define PSI_HOMING_OFFSET   0  //calculatated from Calibration Guide and command M364 / M114 see picture in http://reprap.harleystudio.co.za/?page_id=1073
+#if ENABLED(MORGAN_SCARA) || ENABLED(MAKERARM_SCARA)
+  //#define DEBUG_SCARA_KINEMATICS
 
-//some helper variables to make kinematics faster
-#define L1_2 sq(Linkage_1) // do not change
-#define L2_2 sq(Linkage_2) // do not change
+  #define SCARA_SEGMENTS_PER_SECOND 200 // If movement is choppy try lowering this value
+  // Length of inner support arm
+  #define SCARA_LINKAGE_1 150 //mm      Preprocessor cannot handle decimal point...
+  // Length of outer support arm     Measure arm lengths precisely and enter
+  #define SCARA_LINKAGE_2 150 //mm
+
+  // SCARA tower offset (position of Tower relative to bed zero position)
+  // This needs to be reasonably accurate as it defines the printbed position in the SCARA space.
+  #define SCARA_OFFSET_X 100 //mm
+  #define SCARA_OFFSET_Y -56 //mm
+
+  #define THETA_HOMING_OFFSET 0  //calculatated from Calibration Guide and command M360 / M114 see picture in http://reprap.harleystudio.co.za/?page_id=1073
+  #define PSI_HOMING_OFFSET   0  //calculatated from Calibration Guide and command M364 / M114 see picture in http://reprap.harleystudio.co.za/?page_id=1073
+#endif
 
 //===========================================================================
-//========================= SCARA Settings end ==============================
+//==================== END ==== SCARA Printer ==== END ======================
 //===========================================================================
 
 // @section info

From 8ff338c2b99e2f880cfa60031e48545f59631d77 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Sun, 11 Sep 2016 20:18:52 -0500
Subject: [PATCH 05/26] Patch stepper.h for SCARA

---
 Marlin/stepper.h | 17 ++++++++++++-----
 1 file changed, 12 insertions(+), 5 deletions(-)

diff --git a/Marlin/stepper.h b/Marlin/stepper.h
index 4a2287c7d7..d7507aff2c 100644
--- a/Marlin/stepper.h
+++ b/Marlin/stepper.h
@@ -91,6 +91,11 @@ class Stepper {
       static bool performing_homing;
     #endif
 
+    //
+    // Positions of stepper motors, in step units
+    //
+    static volatile long count_position[NUM_AXIS];
+
   private:
 
     static unsigned char last_direction_bits;        // The next stepping-bits to be output
@@ -138,11 +143,6 @@ class Stepper {
       static constexpr int motor_current_setting[3] = PWM_MOTOR_CURRENT;
     #endif
 
-    //
-    // Positions of stepper motors, in step units
-    //
-    static volatile long count_position[NUM_AXIS];
-
     //
     // Current direction of stepper motors (+1 or -1)
     //
@@ -211,6 +211,13 @@ class Stepper {
     //
     static float get_axis_position_mm(AxisEnum axis);
 
+    //
+    // SCARA AB axes are in degrees, not mm
+    //
+    #if IS_SCARA
+      static FORCE_INLINE float get_axis_position_degrees(AxisEnum axis) { return get_axis_position_mm(axis); }
+    #endif
+
     //
     // The stepper subsystem goes to sleep when it runs out of things to execute. Call this
     // to notify the subsystem that it is time to go to work.

From f9a192c7e340286093c891092f3f5986e5c1f36d Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Sun, 11 Sep 2016 20:21:54 -0500
Subject: [PATCH 06/26] Move setup() and loop() to the end

---
 Marlin/Marlin_main.cpp | 428 ++++++++++++++++++++---------------------
 1 file changed, 214 insertions(+), 214 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index cc17633724..e0b19118c9 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -850,10 +850,6 @@ void servo_init() {
      */
     STOW_Z_SERVO();
   #endif
-
-  #if HAS_BED_PROBE
-    endstops.enable_z_probe(false);
-  #endif
 }
 
 /**
@@ -879,216 +875,6 @@ void servo_init() {
 
 #endif
 
-/**
- * Marlin entry-point: Set up before the program loop
- *  - Set up the kill pin, filament runout, power hold
- *  - Start the serial port
- *  - Print startup messages and diagnostics
- *  - Get EEPROM or default settings
- *  - Initialize managers for:
- *    • temperature
- *    • planner
- *    • watchdog
- *    • stepper
- *    • photo pin
- *    • servos
- *    • LCD controller
- *    • Digipot I2C
- *    • Z probe sled
- *    • status LEDs
- */
-void setup() {
-
-  #ifdef DISABLE_JTAG
-    // Disable JTAG on AT90USB chips to free up pins for IO
-    MCUCR = 0x80;
-    MCUCR = 0x80;
-  #endif
-
-  #if ENABLED(FILAMENT_RUNOUT_SENSOR)
-    setup_filrunoutpin();
-  #endif
-
-  setup_killpin();
-
-  setup_powerhold();
-
-  #if HAS_STEPPER_RESET
-    disableStepperDrivers();
-  #endif
-
-  MYSERIAL.begin(BAUDRATE);
-  SERIAL_PROTOCOLLNPGM("start");
-  SERIAL_ECHO_START;
-
-  // Check startup - does nothing if bootloader sets MCUSR to 0
-  byte mcu = MCUSR;
-  if (mcu & 1) SERIAL_ECHOLNPGM(MSG_POWERUP);
-  if (mcu & 2) SERIAL_ECHOLNPGM(MSG_EXTERNAL_RESET);
-  if (mcu & 4) SERIAL_ECHOLNPGM(MSG_BROWNOUT_RESET);
-  if (mcu & 8) SERIAL_ECHOLNPGM(MSG_WATCHDOG_RESET);
-  if (mcu & 32) SERIAL_ECHOLNPGM(MSG_SOFTWARE_RESET);
-  MCUSR = 0;
-
-  SERIAL_ECHOPGM(MSG_MARLIN);
-  SERIAL_ECHOLNPGM(" " SHORT_BUILD_VERSION);
-
-  #ifdef STRING_DISTRIBUTION_DATE
-    #ifdef STRING_CONFIG_H_AUTHOR
-      SERIAL_ECHO_START;
-      SERIAL_ECHOPGM(MSG_CONFIGURATION_VER);
-      SERIAL_ECHOPGM(STRING_DISTRIBUTION_DATE);
-      SERIAL_ECHOPGM(MSG_AUTHOR);
-      SERIAL_ECHOLNPGM(STRING_CONFIG_H_AUTHOR);
-      SERIAL_ECHOPGM("Compiled: ");
-      SERIAL_ECHOLNPGM(__DATE__);
-    #endif // STRING_CONFIG_H_AUTHOR
-  #endif // STRING_DISTRIBUTION_DATE
-
-  SERIAL_ECHO_START;
-  SERIAL_ECHOPGM(MSG_FREE_MEMORY);
-  SERIAL_ECHO(freeMemory());
-  SERIAL_ECHOPGM(MSG_PLANNER_BUFFER_BYTES);
-  SERIAL_ECHOLN((int)sizeof(block_t)*BLOCK_BUFFER_SIZE);
-
-  // Send "ok" after commands by default
-  for (int8_t i = 0; i < BUFSIZE; i++) send_ok[i] = true;
-
-  // Load data from EEPROM if available (or use defaults)
-  // This also updates variables in the planner, elsewhere
-  Config_RetrieveSettings();
-
-  // Initialize current position based on home_offset
-  memcpy(current_position, home_offset, sizeof(home_offset));
-
-  // Vital to init stepper/planner equivalent for current_position
-  SYNC_PLAN_POSITION_KINEMATIC();
-
-  thermalManager.init();    // Initialize temperature loop
-
-  #if ENABLED(USE_WATCHDOG)
-    watchdog_init();
-  #endif
-
-  stepper.init();    // Initialize stepper, this enables interrupts!
-  setup_photpin();
-  servo_init();
-
-  #if HAS_CONTROLLERFAN
-    SET_OUTPUT(CONTROLLERFAN_PIN); //Set pin used for driver cooling fan
-  #endif
-
-  #if HAS_STEPPER_RESET
-    enableStepperDrivers();
-  #endif
-
-  #if ENABLED(DIGIPOT_I2C)
-    digipot_i2c_init();
-  #endif
-
-  #if ENABLED(DAC_STEPPER_CURRENT)
-    dac_init();
-  #endif
-
-  #if ENABLED(Z_PROBE_SLED) && PIN_EXISTS(SLED)
-    pinMode(SLED_PIN, OUTPUT);
-    digitalWrite(SLED_PIN, LOW); // turn it off
-  #endif // Z_PROBE_SLED
-
-  setup_homepin();
-
-  #ifdef STAT_LED_RED
-    pinMode(STAT_LED_RED, OUTPUT);
-    digitalWrite(STAT_LED_RED, LOW); // turn it off
-  #endif
-
-  #ifdef STAT_LED_BLUE
-    pinMode(STAT_LED_BLUE, OUTPUT);
-    digitalWrite(STAT_LED_BLUE, LOW); // turn it off
-  #endif
-
-  lcd_init();
-  #if ENABLED(SHOW_BOOTSCREEN)
-    #if ENABLED(DOGLCD)
-      safe_delay(BOOTSCREEN_TIMEOUT);
-    #elif ENABLED(ULTRA_LCD)
-      bootscreen();
-      lcd_init();
-    #endif
-  #endif
-
-  #if ENABLED(MIXING_EXTRUDER) && MIXING_VIRTUAL_TOOLS > 1
-    // Initialize mixing to 100% color 1
-    for (uint8_t i = 0; i < MIXING_STEPPERS; i++)
-      mixing_factor[i] = (i == 0) ? 1 : 0;
-    for (uint8_t t = 0; t < MIXING_VIRTUAL_TOOLS; t++)
-      for (uint8_t i = 0; i < MIXING_STEPPERS; i++)
-        mixing_virtual_tool_mix[t][i] = mixing_factor[i];
-  #endif
-
-  #if ENABLED(EXPERIMENTAL_I2CBUS) && I2C_SLAVE_ADDRESS > 0
-    i2c.onReceive(i2c_on_receive);
-    i2c.onRequest(i2c_on_request);
-  #endif
-}
-
-/**
- * The main Marlin program loop
- *
- *  - Save or log commands to SD
- *  - Process available commands (if not saving)
- *  - Call heater manager
- *  - Call inactivity manager
- *  - Call endstop manager
- *  - Call LCD update
- */
-void loop() {
-  if (commands_in_queue < BUFSIZE) get_available_commands();
-
-  #if ENABLED(SDSUPPORT)
-    card.checkautostart(false);
-  #endif
-
-  if (commands_in_queue) {
-
-    #if ENABLED(SDSUPPORT)
-
-      if (card.saving) {
-        char* command = command_queue[cmd_queue_index_r];
-        if (strstr_P(command, PSTR("M29"))) {
-          // M29 closes the file
-          card.closefile();
-          SERIAL_PROTOCOLLNPGM(MSG_FILE_SAVED);
-          ok_to_send();
-        }
-        else {
-          // Write the string from the read buffer to SD
-          card.write_command(command);
-          if (card.logging)
-            process_next_command(); // The card is saving because it's logging
-          else
-            ok_to_send();
-        }
-      }
-      else
-        process_next_command();
-
-    #else
-
-      process_next_command();
-
-    #endif // SDSUPPORT
-
-    // The queue may be reset by a command handler or by code invoked by idle() within a handler
-    if (commands_in_queue) {
-      --commands_in_queue;
-      cmd_queue_index_r = (cmd_queue_index_r + 1) % BUFSIZE;
-    }
-  }
-  endstops.report_state();
-  idle();
-}
-
 void gcode_line_error(const char* err, bool doFlush = true) {
   SERIAL_ERROR_START;
   serialprintPGM(err);
@@ -8889,3 +8675,217 @@ void calculate_volumetric_multipliers() {
   for (uint8_t i = 0; i < COUNT(filament_size); i++)
     volumetric_multiplier[i] = calculate_volumetric_multiplier(filament_size[i]);
 }
+
+/**
+ * Marlin entry-point: Set up before the program loop
+ *  - Set up the kill pin, filament runout, power hold
+ *  - Start the serial port
+ *  - Print startup messages and diagnostics
+ *  - Get EEPROM or default settings
+ *  - Initialize managers for:
+ *    • temperature
+ *    • planner
+ *    • watchdog
+ *    • stepper
+ *    • photo pin
+ *    • servos
+ *    • LCD controller
+ *    • Digipot I2C
+ *    • Z probe sled
+ *    • status LEDs
+ */
+void setup() {
+
+  #ifdef DISABLE_JTAG
+    // Disable JTAG on AT90USB chips to free up pins for IO
+    MCUCR = 0x80;
+    MCUCR = 0x80;
+  #endif
+
+  #if ENABLED(FILAMENT_RUNOUT_SENSOR)
+    setup_filrunoutpin();
+  #endif
+
+  setup_killpin();
+
+  setup_powerhold();
+
+  #if HAS_STEPPER_RESET
+    disableStepperDrivers();
+  #endif
+
+  MYSERIAL.begin(BAUDRATE);
+  SERIAL_PROTOCOLLNPGM("start");
+  SERIAL_ECHO_START;
+
+  // Check startup - does nothing if bootloader sets MCUSR to 0
+  byte mcu = MCUSR;
+  if (mcu & 1) SERIAL_ECHOLNPGM(MSG_POWERUP);
+  if (mcu & 2) SERIAL_ECHOLNPGM(MSG_EXTERNAL_RESET);
+  if (mcu & 4) SERIAL_ECHOLNPGM(MSG_BROWNOUT_RESET);
+  if (mcu & 8) SERIAL_ECHOLNPGM(MSG_WATCHDOG_RESET);
+  if (mcu & 32) SERIAL_ECHOLNPGM(MSG_SOFTWARE_RESET);
+  MCUSR = 0;
+
+  SERIAL_ECHOPGM(MSG_MARLIN);
+  SERIAL_ECHOLNPGM(" " SHORT_BUILD_VERSION);
+
+  #ifdef STRING_DISTRIBUTION_DATE
+    #ifdef STRING_CONFIG_H_AUTHOR
+      SERIAL_ECHO_START;
+      SERIAL_ECHOPGM(MSG_CONFIGURATION_VER);
+      SERIAL_ECHOPGM(STRING_DISTRIBUTION_DATE);
+      SERIAL_ECHOPGM(MSG_AUTHOR);
+      SERIAL_ECHOLNPGM(STRING_CONFIG_H_AUTHOR);
+      SERIAL_ECHOPGM("Compiled: ");
+      SERIAL_ECHOLNPGM(__DATE__);
+    #endif // STRING_CONFIG_H_AUTHOR
+  #endif // STRING_DISTRIBUTION_DATE
+
+  SERIAL_ECHO_START;
+  SERIAL_ECHOPGM(MSG_FREE_MEMORY);
+  SERIAL_ECHO(freeMemory());
+  SERIAL_ECHOPGM(MSG_PLANNER_BUFFER_BYTES);
+  SERIAL_ECHOLN((int)sizeof(block_t)*BLOCK_BUFFER_SIZE);
+
+  // Send "ok" after commands by default
+  for (int8_t i = 0; i < BUFSIZE; i++) send_ok[i] = true;
+
+  // Load data from EEPROM if available (or use defaults)
+  // This also updates variables in the planner, elsewhere
+  Config_RetrieveSettings();
+
+  // Initialize current position based on home_offset
+  memcpy(current_position, home_offset, sizeof(home_offset));
+
+  // Vital to init stepper/planner equivalent for current_position
+  SYNC_PLAN_POSITION_KINEMATIC();
+
+  thermalManager.init();    // Initialize temperature loop
+
+  #if ENABLED(USE_WATCHDOG)
+    watchdog_init();
+  #endif
+
+  stepper.init();    // Initialize stepper, this enables interrupts!
+  setup_photpin();
+  servo_init();
+
+  #if HAS_BED_PROBE
+    endstops.enable_z_probe(false);
+  #endif
+
+  #if HAS_CONTROLLERFAN
+    SET_OUTPUT(CONTROLLERFAN_PIN); //Set pin used for driver cooling fan
+  #endif
+
+  #if HAS_STEPPER_RESET
+    enableStepperDrivers();
+  #endif
+
+  #if ENABLED(DIGIPOT_I2C)
+    digipot_i2c_init();
+  #endif
+
+  #if ENABLED(DAC_STEPPER_CURRENT)
+    dac_init();
+  #endif
+
+  #if ENABLED(Z_PROBE_SLED) && PIN_EXISTS(SLED)
+    pinMode(SLED_PIN, OUTPUT);
+    digitalWrite(SLED_PIN, LOW); // turn it off
+  #endif // Z_PROBE_SLED
+
+  setup_homepin();
+
+  #ifdef STAT_LED_RED
+    pinMode(STAT_LED_RED, OUTPUT);
+    digitalWrite(STAT_LED_RED, LOW); // turn it off
+  #endif
+
+  #ifdef STAT_LED_BLUE
+    pinMode(STAT_LED_BLUE, OUTPUT);
+    digitalWrite(STAT_LED_BLUE, LOW); // turn it off
+  #endif
+
+  lcd_init();
+  #if ENABLED(SHOW_BOOTSCREEN)
+    #if ENABLED(DOGLCD)
+      safe_delay(BOOTSCREEN_TIMEOUT);
+    #elif ENABLED(ULTRA_LCD)
+      bootscreen();
+      lcd_init();
+    #endif
+  #endif
+
+  #if ENABLED(MIXING_EXTRUDER) && MIXING_VIRTUAL_TOOLS > 1
+    // Initialize mixing to 100% color 1
+    for (uint8_t i = 0; i < MIXING_STEPPERS; i++)
+      mixing_factor[i] = (i == 0) ? 1 : 0;
+    for (uint8_t t = 0; t < MIXING_VIRTUAL_TOOLS; t++)
+      for (uint8_t i = 0; i < MIXING_STEPPERS; i++)
+        mixing_virtual_tool_mix[t][i] = mixing_factor[i];
+  #endif
+
+  #if ENABLED(EXPERIMENTAL_I2CBUS) && I2C_SLAVE_ADDRESS > 0
+    i2c.onReceive(i2c_on_receive);
+    i2c.onRequest(i2c_on_request);
+  #endif
+}
+
+/**
+ * The main Marlin program loop
+ *
+ *  - Save or log commands to SD
+ *  - Process available commands (if not saving)
+ *  - Call heater manager
+ *  - Call inactivity manager
+ *  - Call endstop manager
+ *  - Call LCD update
+ */
+void loop() {
+  if (commands_in_queue < BUFSIZE) get_available_commands();
+
+  #if ENABLED(SDSUPPORT)
+    card.checkautostart(false);
+  #endif
+
+  if (commands_in_queue) {
+
+    #if ENABLED(SDSUPPORT)
+
+      if (card.saving) {
+        char* command = command_queue[cmd_queue_index_r];
+        if (strstr_P(command, PSTR("M29"))) {
+          // M29 closes the file
+          card.closefile();
+          SERIAL_PROTOCOLLNPGM(MSG_FILE_SAVED);
+          ok_to_send();
+        }
+        else {
+          // Write the string from the read buffer to SD
+          card.write_command(command);
+          if (card.logging)
+            process_next_command(); // The card is saving because it's logging
+          else
+            ok_to_send();
+        }
+      }
+      else
+        process_next_command();
+
+    #else
+
+      process_next_command();
+
+    #endif // SDSUPPORT
+
+    // The queue may be reset by a command handler or by code invoked by idle() within a handler
+    if (commands_in_queue) {
+      --commands_in_queue;
+      cmd_queue_index_r = (cmd_queue_index_r + 1) % BUFSIZE;
+    }
+  }
+  endstops.report_state();
+  idle();
+}

From c84b14c77a5c1ae277a28e1dc0bd84094efc42ec Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Sun, 11 Sep 2016 20:25:44 -0500
Subject: [PATCH 07/26] Move delta homing to its own function

---
 Marlin/Marlin_main.cpp | 78 +++++++++++++++++++++++-------------------
 1 file changed, 43 insertions(+), 35 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index e0b19118c9..f9b0a5c714 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -2707,6 +2707,48 @@ inline void gcode_G4() {
 
 #endif // DEBUG_LEVELING_FEATURE
 
+#if ENABLED(DELTA)
+
+  /**
+   * A delta can only safely home all axes at the same time
+   * This is like quick_home_xy() but for 3 towers.
+   */
+  inline void home_delta() {
+    // Init the current position of all carriages to 0,0,0
+    memset(current_position, 0, sizeof(current_position));
+    sync_plan_position();
+
+    // Move all carriages together linearly until an endstop is hit.
+    current_position[X_AXIS] = current_position[Y_AXIS] = current_position[Z_AXIS] = (Z_MAX_LENGTH + 10);
+    feedrate_mm_s = homing_feedrate_mm_s[X_AXIS];
+    line_to_current_position();
+    stepper.synchronize();
+    endstops.hit_on_purpose(); // clear endstop hit flags
+
+    // Probably not needed. Double-check this line:
+    memset(current_position, 0, sizeof(current_position));
+
+    // At least one carriage has reached the top.
+    // Now back off and re-home each carriage separately.
+    HOMEAXIS(A);
+    HOMEAXIS(B);
+    HOMEAXIS(C);
+
+    // Set all carriages to their home positions
+    // Do this here all at once for Delta, because
+    // XYZ isn't ABC. Applying this per-tower would
+    // give the impression that they are the same.
+    LOOP_XYZ(i) set_axis_is_at_home((AxisEnum)i);
+
+    SYNC_PLAN_POSITION_KINEMATIC();
+
+    #if ENABLED(DEBUG_LEVELING_FEATURE)
+      if (DEBUGGING(LEVELING)) DEBUG_POS("(DELTA)", current_position);
+    #endif
+  }
+
+#endif // DELTA
+
 /**
  * G28: Home all axes according to settings
  *
@@ -2784,42 +2826,8 @@ inline void gcode_G28() {
 
 
   #if ENABLED(DELTA)
-    /**
-     * A delta can only safely home all axes at the same time
-     * This is like quick_home_xy() but for 3 towers.
-     */
 
-    // Init the current position of all carriages to 0,0,0
-    memset(current_position, 0, sizeof(current_position));
-    sync_plan_position();
-
-    // Move all carriages together linearly until an endstop is hit.
-    current_position[X_AXIS] = current_position[Y_AXIS] = current_position[Z_AXIS] = (Z_MAX_LENGTH + 10);
-    feedrate_mm_s = homing_feedrate_mm_s[X_AXIS];
-    line_to_current_position();
-    stepper.synchronize();
-    endstops.hit_on_purpose(); // clear endstop hit flags
-
-    // Probably not needed. Double-check this line:
-    memset(current_position, 0, sizeof(current_position));
-
-    // At least one carriage has reached the top.
-    // Now back off and re-home each carriage separately.
-    HOMEAXIS(A);
-    HOMEAXIS(B);
-    HOMEAXIS(C);
-
-    // Set all carriages to their home positions
-    // Do this here all at once for Delta, because
-    // XYZ isn't ABC. Applying this per-tower would
-    // give the impression that they are the same.
-    LOOP_XYZ(i) set_axis_is_at_home((AxisEnum)i);
-
-    SYNC_PLAN_POSITION_KINEMATIC();
-
-    #if ENABLED(DEBUG_LEVELING_FEATURE)
-      if (DEBUGGING(LEVELING)) DEBUG_POS("(DELTA)", current_position);
-    #endif
+    home_delta();
 
   #else // NOT DELTA
 

From b4034915f8f6ff4c7eeeacf9f9876ec12f77d34c Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Sun, 11 Sep 2016 20:39:39 -0500
Subject: [PATCH 08/26] Move z safe homing to its own function

---
 Marlin/Marlin_main.cpp | 123 ++++++++++++++++++-----------------------
 Marlin/language_en.h   |   3 +
 2 files changed, 57 insertions(+), 69 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index f9b0a5c714..abc938110c 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -2749,6 +2749,57 @@ inline void gcode_G4() {
 
 #endif // DELTA
 
+#if ENABLED(Z_SAFE_HOMING)
+
+  inline void home_z_safely() {
+
+    // Disallow Z homing if X or Y are unknown
+    if (!axis_known_position[X_AXIS] || !axis_known_position[Y_AXIS]) {
+      LCD_MESSAGEPGM(MSG_ERR_Z_HOMING);
+      SERIAL_ECHO_START;
+      SERIAL_ECHOLNPGM(MSG_ERR_Z_HOMING);
+      return;
+    }
+
+    #if ENABLED(DEBUG_LEVELING_FEATURE)
+      if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("Z_SAFE_HOMING >>>");
+    #endif
+
+    SYNC_PLAN_POSITION_KINEMATIC();
+
+    /**
+     * Move the Z probe (or just the nozzle) to the safe homing point
+     */
+    float cpx = Z_SAFE_HOMING_X_POINT, cpy = Z_SAFE_HOMING_Y_POINT;
+    #if HAS_BED_PROBE
+      cpx -= X_PROBE_OFFSET_FROM_EXTRUDER;
+      cpy -= Y_PROBE_OFFSET_FROM_EXTRUDER;
+    #endif
+
+    #if ENABLED(DEBUG_LEVELING_FEATURE)
+      if (DEBUGGING(LEVELING)) {
+        SERIAL_ECHOPAIR("Z_SAFE_HOMING X:", cpx);
+        SERIAL_ECHOLNPAIR(" Y:", cpy);
+      }
+    #endif
+
+    if (cpx >= X_MIN_POS && cpx <= X_MAX_POS && cpy >= Y_MIN_POS && cpy <= Y_MAX_POS) {
+      do_blocking_move_to_xy(LOGICAL_X_POSITION(destination[X_AXIS]), LOGICAL_Y_POSITION(destination[Y_AXIS]));
+      HOMEAXIS(Z);
+    }
+    else {
+      LCD_MESSAGEPGM(MSG_ZPROBE_OUT);
+      SERIAL_ECHO_START;
+      SERIAL_ECHOLNPGM(MSG_ZPROBE_OUT);
+    }
+
+    #if ENABLED(DEBUG_LEVELING_FEATURE)
+      if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("<<< Z_SAFE_HOMING");
+    #endif
+  }
+
+#endif // Z_SAFE_HOMING
+
 /**
  * G28: Home all axes according to settings
  *
@@ -2824,7 +2875,6 @@ inline void gcode_G28() {
   #endif
   endstops.enable(true); // Enable endstops for next homing move
 
-
   #if ENABLED(DELTA)
 
     home_delta();
@@ -2915,81 +2965,16 @@ inline void gcode_G28() {
 
     // Home Z last if homing towards the bed
     #if Z_HOME_DIR < 0
-
       if (home_all_axis || homeZ) {
-
         #if ENABLED(Z_SAFE_HOMING)
-
-          #if ENABLED(DEBUG_LEVELING_FEATURE)
-            if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("> Z_SAFE_HOMING >>>");
-          #endif
-
-          if (home_all_axis) {
-
-            /**
-             * At this point we already have Z at Z_HOMING_HEIGHT height
-             * No need to move Z any more as this height should already be safe
-             * enough to reach Z_SAFE_HOMING XY positions.
-             * Just make sure the planner is in sync.
-             */
-            SYNC_PLAN_POSITION_KINEMATIC();
-
-            /**
-             * Move the Z probe (or just the nozzle) to the safe homing point
-             */
-            destination[X_AXIS] = round(Z_SAFE_HOMING_X_POINT - (X_PROBE_OFFSET_FROM_EXTRUDER));
-            destination[Y_AXIS] = round(Z_SAFE_HOMING_Y_POINT - (Y_PROBE_OFFSET_FROM_EXTRUDER));
-            destination[Z_AXIS] = current_position[Z_AXIS]; // Z is already at the right height
-
-            #if ENABLED(DEBUG_LEVELING_FEATURE)
-              if (DEBUGGING(LEVELING)) DEBUG_POS("> Z_SAFE_HOMING > home_all_axis", destination);
-            #endif
-
-            // Move in the XY plane
-            do_blocking_move_to_xy(destination[X_AXIS], destination[Y_AXIS]);
-          }
-
-          // Let's see if X and Y are homed
-          if (axis_unhomed_error(true, true, false)) return;
-
-          /**
-           * Make sure the Z probe is within the physical limits
-           * NOTE: This doesn't necessarily ensure the Z probe is also
-           * within the bed!
-           */
-          float cpx = RAW_CURRENT_POSITION(X_AXIS), cpy = RAW_CURRENT_POSITION(Y_AXIS);
-          if (   cpx >= X_MIN_POS - (X_PROBE_OFFSET_FROM_EXTRUDER)
-              && cpx <= X_MAX_POS - (X_PROBE_OFFSET_FROM_EXTRUDER)
-              && cpy >= Y_MIN_POS - (Y_PROBE_OFFSET_FROM_EXTRUDER)
-              && cpy <= Y_MAX_POS - (Y_PROBE_OFFSET_FROM_EXTRUDER)) {
-
-            // Home the Z axis
-            HOMEAXIS(Z);
-          }
-          else {
-            LCD_MESSAGEPGM(MSG_ZPROBE_OUT);
-            SERIAL_ECHO_START;
-            SERIAL_ECHOLNPGM(MSG_ZPROBE_OUT);
-          }
-
-          #if ENABLED(DEBUG_LEVELING_FEATURE)
-            if (DEBUGGING(LEVELING)) {
-              SERIAL_ECHOLNPGM("<<< Z_SAFE_HOMING");
-            }
-          #endif
-
-        #else // !Z_SAFE_HOMING
-
+          home_z_safely();
+        #else
           HOMEAXIS(Z);
-
-        #endif // !Z_SAFE_HOMING
-
+        #endif
         #if ENABLED(DEBUG_LEVELING_FEATURE)
           if (DEBUGGING(LEVELING)) DEBUG_POS("> (home_all_axis || homeZ) > final", current_position);
         #endif
-
       } // home_all_axis || homeZ
-
     #endif // Z_HOME_DIR < 0
 
     SYNC_PLAN_POSITION_KINEMATIC();
diff --git a/Marlin/language_en.h b/Marlin/language_en.h
index a0443f4d58..b1b8e0af75 100644
--- a/Marlin/language_en.h
+++ b/Marlin/language_en.h
@@ -408,6 +408,9 @@
 #ifndef MSG_ERR_MINTEMP_BED
   #define MSG_ERR_MINTEMP_BED                 "Err: MINTEMP BED"
 #endif
+#ifndef MSG_ERR_Z_HOMING
+  #define MSG_ERR_Z_HOMING                    "G28 Z Forbidden"
+#endif
 #ifndef MSG_HALTED
   #define MSG_HALTED                          "PRINTER HALTED"
 #endif

From 786d1afb72f6374bdef5fd8fcc591d7a0df22499 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Sun, 11 Sep 2016 22:21:32 -0500
Subject: [PATCH 09/26] Add position_is_reachable, use in home_z_safely

---
 Marlin/Marlin_main.cpp | 32 +++++++++++++++++++++++---------
 1 file changed, 23 insertions(+), 9 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index abc938110c..6ac438c4ec 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -2451,6 +2451,20 @@ void unknown_command_error() {
 
 #endif //HOST_KEEPALIVE_FEATURE
 
+bool position_is_reachable(float target[XYZ]) {
+  float dx = RAW_X_POSITION(target[X_AXIS]),
+        dy = RAW_Y_POSITION(target[Y_AXIS]);
+
+  #if ENABLED(DELTA)
+    return HYPOT2(dx, dy) <= sq(DELTA_PRINTABLE_RADIUS);
+  #else
+    float dz = RAW_Z_POSITION(target[Z_AXIS]);
+    return  dx >= X_MIN_POS - 0.0001 && dx <= X_MAX_POS + 0.0001
+         && dy >= Y_MIN_POS - 0.0001 && dy <= Y_MAX_POS + 0.0001
+         && dz >= Z_MIN_POS - 0.0001 && dz <= Z_MAX_POS + 0.0001;
+  #endif
+}
+
 /**
  * G0, G1: Coordinated movement of X Y Z E axes
  */
@@ -2770,21 +2784,21 @@ inline void gcode_G4() {
     /**
      * Move the Z probe (or just the nozzle) to the safe homing point
      */
-    float cpx = Z_SAFE_HOMING_X_POINT, cpy = Z_SAFE_HOMING_Y_POINT;
+    destination[X_AXIS] = LOGICAL_X_POSITION(Z_SAFE_HOMING_X_POINT);
+    destination[Y_AXIS] = LOGICAL_Y_POSITION(Z_SAFE_HOMING_Y_POINT);
+    destination[Z_AXIS] = current_position[Z_AXIS]; // Z is already at the right height
+
     #if HAS_BED_PROBE
-      cpx -= X_PROBE_OFFSET_FROM_EXTRUDER;
-      cpy -= Y_PROBE_OFFSET_FROM_EXTRUDER;
+      destination[X_AXIS] -= X_PROBE_OFFSET_FROM_EXTRUDER;
+      destination[Y_AXIS] -= Y_PROBE_OFFSET_FROM_EXTRUDER;
     #endif
 
     #if ENABLED(DEBUG_LEVELING_FEATURE)
-      if (DEBUGGING(LEVELING)) {
-        SERIAL_ECHOPAIR("Z_SAFE_HOMING X:", cpx);
-        SERIAL_ECHOLNPAIR(" Y:", cpy);
-      }
+      if (DEBUGGING(LEVELING)) DEBUG_POS("Z_SAFE_HOMING", destination);
     #endif
 
-    if (cpx >= X_MIN_POS && cpx <= X_MAX_POS && cpy >= Y_MIN_POS && cpy <= Y_MAX_POS) {
-      do_blocking_move_to_xy(LOGICAL_X_POSITION(destination[X_AXIS]), LOGICAL_Y_POSITION(destination[Y_AXIS]));
+    if (position_is_reachable(destination)) {
+      do_blocking_move_to_xy(destination[X_AXIS], destination[Y_AXIS]);
       HOMEAXIS(Z);
     }
     else {

From c5fa70809b836d3dff4c3ad100eef171c2bfb8c0 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Sun, 11 Sep 2016 20:51:53 -0500
Subject: [PATCH 10/26] Implement M0/M1 for EMERGENCY_PARSER

---
 Marlin/Marlin.h         |  4 ++
 Marlin/MarlinSerial.cpp |  3 ++
 Marlin/Marlin_main.cpp  | 88 ++++++++++++++++++++++++++++-------------
 3 files changed, 68 insertions(+), 27 deletions(-)

diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h
index e0a540c263..e4fe48b0df 100644
--- a/Marlin/Marlin.h
+++ b/Marlin/Marlin.h
@@ -266,6 +266,10 @@ extern bool axis_known_position[XYZ]; // axis[n].is_known
 extern bool axis_homed[XYZ]; // axis[n].is_homed
 extern volatile bool wait_for_heatup;
 
+#if ENABLED(EMERGENCY_PARSER) && DISABLED(ULTIPANEL)
+  extern volatile bool wait_for_user;
+#endif
+
 extern float current_position[NUM_AXIS];
 extern float position_shift[XYZ];
 extern float home_offset[XYZ];
diff --git a/Marlin/MarlinSerial.cpp b/Marlin/MarlinSerial.cpp
index a1b3349fec..3bb2a822e3 100644
--- a/Marlin/MarlinSerial.cpp
+++ b/Marlin/MarlinSerial.cpp
@@ -509,6 +509,9 @@ MarlinSerial customizedSerial;
           switch (state) {
             case state_M108:
               wait_for_heatup = false;
+              #if DISABLED(ULTIPANEL)
+                wait_for_user = false;
+              #endif
               break;
             case state_M112:
               kill(PSTR(MSG_KILLED));
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 6ac438c4ec..9c4da6b509 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -351,6 +351,10 @@ static bool relative_mode = false;
 
 volatile bool wait_for_heatup = true;
 
+#if ENABLED(EMERGENCY_PARSER) && DISABLED(ULTIPANEL)
+  wait_for_user = false;
+#endif
+
 const char errormagic[] PROGMEM = "Error:";
 const char echomagic[] PROGMEM = "echo:";
 const char axis_codes[NUM_AXIS] = {'X', 'Y', 'Z', 'E'};
@@ -3815,7 +3819,7 @@ inline void gcode_G92() {
     sync_plan_position_e();
 }
 
-#if ENABLED(ULTIPANEL)
+#if ENABLED(ULTIPANEL) || ENABLED(EMERGENCY_PARSER)
 
   /**
    * M0: Unconditional stop - Wait for user button press on LCD
@@ -3835,38 +3839,68 @@ inline void gcode_G92() {
       hasS = codenum > 0;
     }
 
-    if (!hasP && !hasS && *args != '\0')
-      lcd_setstatus(args, true);
-    else {
-      LCD_MESSAGEPGM(MSG_USERWAIT);
-      #if ENABLED(LCD_PROGRESS_BAR) && PROGRESS_MSG_EXPIRE > 0
-        dontExpireStatus();
-      #endif
-    }
+    #if ENABLED(ULTIPANEL)
+
+      if (!hasP && !hasS && *args != '\0')
+        lcd_setstatus(args, true);
+      else {
+        LCD_MESSAGEPGM(MSG_USERWAIT);
+        #if ENABLED(LCD_PROGRESS_BAR) && PROGRESS_MSG_EXPIRE > 0
+          dontExpireStatus();
+        #endif
+      }
+      lcd_ignore_click();
+
+    #else
+
+      if (!hasP && !hasS && *args != '\0') {
+        SERIAL_ECHO_START;
+        SERIAL_ECHOLN(args);
+      }
+
+    #endif
 
-    lcd_ignore_click();
     stepper.synchronize();
     refresh_cmd_timeout();
-    if (codenum > 0) {
-      codenum += previous_cmd_ms;  // wait until this time for a click
+
+    #if ENABLED(ULTIPANEL)
+
+      if (codenum > 0) {
+        codenum += previous_cmd_ms;  // wait until this time for a click
+        KEEPALIVE_STATE(PAUSED_FOR_USER);
+        while (PENDING(millis(), codenum) && !lcd_clicked()) idle();
+        lcd_ignore_click(false);
+      }
+      else if (lcd_detected()) {
+        KEEPALIVE_STATE(PAUSED_FOR_USER);
+        while (!lcd_clicked()) idle();
+      }
+      else return;
+
+      if (IS_SD_PRINTING)
+        LCD_MESSAGEPGM(MSG_RESUMING);
+      else
+        LCD_MESSAGEPGM(WELCOME_MSG);
+
+    #else
+
       KEEPALIVE_STATE(PAUSED_FOR_USER);
-      while (PENDING(millis(), codenum) && !lcd_clicked()) idle();
-      KEEPALIVE_STATE(IN_HANDLER);
-      lcd_ignore_click(false);
-    }
-    else {
-      if (!lcd_detected()) return;
-      KEEPALIVE_STATE(PAUSED_FOR_USER);
-      while (!lcd_clicked()) idle();
-      KEEPALIVE_STATE(IN_HANDLER);
-    }
-    if (IS_SD_PRINTING)
-      LCD_MESSAGEPGM(MSG_RESUMING);
-    else
-      LCD_MESSAGEPGM(WELCOME_MSG);
+      wait_for_user = true;
+
+      if (codenum > 0) {
+        codenum += previous_cmd_ms;  // wait until this time for an M108
+        while (PENDING(millis(), codenum) && wait_for_user) idle();
+      }
+      else while (wait_for_user) idle();
+
+      wait_for_user = false;
+
+    #endif
+
+    KEEPALIVE_STATE(IN_HANDLER);
   }
 
-#endif // ULTIPANEL
+#endif // ULTIPANEL || EMERGENCY_PARSER
 
 /**
  * M17: Enable power on all stepper motors

From acd1b6e9c043086e16eacd0b178439d17f808cf2 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Sun, 11 Sep 2016 20:56:16 -0500
Subject: [PATCH 11/26] Rename delta_grid spacing for general nonlinear

---
 Marlin/Marlin.h        |  9 +++++----
 Marlin/Marlin_main.cpp | 21 +++++++++++----------
 2 files changed, 16 insertions(+), 14 deletions(-)

diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h
index e4fe48b0df..fbea4dc8b9 100644
--- a/Marlin/Marlin.h
+++ b/Marlin/Marlin.h
@@ -313,10 +313,6 @@ float code_value_temp_diff();
   extern float delta_diagonal_rod_trim_tower_3;
   void inverse_kinematics(const float cartesian[XYZ]);
   void recalc_delta_settings(float radius, float diagonal_rod);
-  #if ENABLED(AUTO_BED_LEVELING_FEATURE)
-    extern int delta_grid_spacing[2];
-    void adjust_delta(float cartesian[XYZ]);
-  #endif
 #elif IS_SCARA
   extern float delta[ABC];
   extern float axis_scaling[ABC];  // Build size scaling
@@ -324,6 +320,11 @@ float code_value_temp_diff();
   void forward_kinematics_SCARA(float f_scara[ABC]);
 #endif
 
+#if ENABLED(AUTO_BED_LEVELING_NONLINEAR)
+  extern int nonlinear_grid_spacing[2];
+  void adjust_delta(float cartesian[XYZ]);
+#endif
+
 #if ENABLED(Z_DUAL_ENDSTOPS)
   extern float z_endstop_adj;
 #endif
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 9c4da6b509..4badd7792b 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -486,11 +486,6 @@ static uint8_t target_extruder;
         delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND,
         delta_clip_start_height = Z_MAX_POS;
 
-  #if ENABLED(AUTO_BED_LEVELING_FEATURE)
-    int delta_grid_spacing[2] = { 0, 0 };
-    float bed_level[AUTO_BED_LEVELING_GRID_POINTS][AUTO_BED_LEVELING_GRID_POINTS];
-  #endif
-
   float delta_safe_distance_from_top();
   void set_cartesian_from_steppers();
 
@@ -500,6 +495,11 @@ static uint8_t target_extruder;
 
 #endif
 
+#if ENABLED(AUTO_BED_LEVELING_NONLINEAR)
+  int nonlinear_grid_spacing[2] = { 0 };
+  float bed_level[AUTO_BED_LEVELING_GRID_POINTS][AUTO_BED_LEVELING_GRID_POINTS];
+#endif
+
 #if IS_SCARA
   // Float constants for SCARA calculations
   const float L1 = SCARA_LINKAGE_1, L2 = SCARA_LINKAGE_2,
@@ -3442,8 +3442,9 @@ inline void gcode_G28() {
                   yGridSpacing = (back_probe_bed_position - front_probe_bed_position) / (auto_bed_leveling_grid_points - 1);
 
       #if ENABLED(AUTO_BED_LEVELING_NONLINEAR)
-        delta_grid_spacing[X_AXIS] = xGridSpacing;
-        delta_grid_spacing[Y_AXIS] = yGridSpacing;
+
+        nonlinear_grid_spacing[X_AXIS] = xGridSpacing;
+        nonlinear_grid_spacing[Y_AXIS] = yGridSpacing;
         float zoffset = zprobe_zoffset;
         if (code_seen('Z')) zoffset += code_value_axis_units(Z_AXIS);
 
@@ -7803,12 +7804,12 @@ void ok_to_send() {
 
     // Adjust print surface height by linear interpolation over the bed_level array.
     void adjust_delta(float cartesian[XYZ]) {
-      if (delta_grid_spacing[X_AXIS] == 0 || delta_grid_spacing[Y_AXIS] == 0) return; // G29 not done!
+      if (nonlinear_grid_spacing[X_AXIS] == 0 || nonlinear_grid_spacing[Y_AXIS] == 0) return; // G29 not done!
 
       int half = (AUTO_BED_LEVELING_GRID_POINTS - 1) / 2;
       float h1 = 0.001 - half, h2 = half - 0.001,
-            grid_x = max(h1, min(h2, RAW_X_POSITION(cartesian[X_AXIS]) / delta_grid_spacing[X_AXIS])),
-            grid_y = max(h1, min(h2, RAW_Y_POSITION(cartesian[Y_AXIS]) / delta_grid_spacing[Y_AXIS]));
+            grid_x = max(h1, min(h2, RAW_X_POSITION(cartesian[X_AXIS]) / nonlinear_grid_spacing[X_AXIS])),
+            grid_y = max(h1, min(h2, RAW_Y_POSITION(cartesian[Y_AXIS]) / nonlinear_grid_spacing[Y_AXIS]));
       int floor_x = floor(grid_x), floor_y = floor(grid_y);
       float ratio_x = grid_x - floor_x, ratio_y = grid_y - floor_y,
             z1 = bed_level[floor_x + half][floor_y + half],

From d4f21af6b3aa1003da78bd30b8b37d162b3025ef Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Sun, 11 Sep 2016 21:30:59 -0500
Subject: [PATCH 12/26] sync_plan_position_delta =>
 sync_plan_position_kinematic

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

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 4badd7792b..42b55bf781 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -660,14 +660,14 @@ inline void sync_plan_position() {
 inline void sync_plan_position_e() { planner.set_e_position_mm(current_position[E_AXIS]); }
 
 #if IS_KINEMATIC
-  inline void sync_plan_position_delta() {
+  inline void sync_plan_position_kinematic() {
     #if ENABLED(DEBUG_LEVELING_FEATURE)
-      if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position_delta", current_position);
+      if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position_kinematic", current_position);
     #endif
     inverse_kinematics(current_position);
     planner.set_position_mm(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
   }
-  #define SYNC_PLAN_POSITION_KINEMATIC() sync_plan_position_delta()
+  #define SYNC_PLAN_POSITION_KINEMATIC() sync_plan_position_kinematic()
 #else
   #define SYNC_PLAN_POSITION_KINEMATIC() sync_plan_position()
 #endif

From c109399bf6363741906a556092a72307638a7ce6 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Sun, 11 Sep 2016 21:40:44 -0500
Subject: [PATCH 13/26] Fix planner leveling and rename arguments
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Use lx, ly, lz for “logical” positions
---
 Marlin/planner.cpp | 110 ++++++++++++++++++++++++---------------------
 Marlin/planner.h   |  66 ++++++++++++++-------------
 2 files changed, 96 insertions(+), 80 deletions(-)

diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index f8dc5efd2b..93084792f1 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -523,30 +523,44 @@ void Planner::check_axes_activity() {
 
 #if PLANNER_LEVELING
 
-  void Planner::apply_leveling(
-    #if ENABLED(MESH_BED_LEVELING)
-      const float &x, const float &y
-    #else
-      float &x, float &y
-    #endif
-    , float &z
-  ) {
+  void Planner::apply_leveling(float &lx, float &ly, float &lz) {
     #if ENABLED(MESH_BED_LEVELING)
 
       if (mbl.active())
-        z += mbl.get_z(RAW_X_POSITION(x), RAW_Y_POSITION(y));
+        lz += mbl.get_z(RAW_X_POSITION(lx), RAW_Y_POSITION(ly));
 
-    #elif ENABLED(AUTO_BED_LEVELING_FEATURE)
+    #elif ENABLED(AUTO_BED_LEVELING_LINEAR)
 
-      float tx = RAW_X_POSITION(x) - (X_TILT_FULCRUM),
-            ty = RAW_Y_POSITION(y) - (Y_TILT_FULCRUM),
-            tz = RAW_Z_POSITION(z);
+      float dx = RAW_X_POSITION(lx) - (X_TILT_FULCRUM),
+            dy = RAW_Y_POSITION(ly) - (Y_TILT_FULCRUM),
+            dz = RAW_Z_POSITION(lz);
 
-      apply_rotation_xyz(bed_level_matrix, tx, ty, tz);
+      apply_rotation_xyz(bed_level_matrix, dx, dy, dz);
 
-      x = LOGICAL_X_POSITION(tx + X_TILT_FULCRUM);
-      y = LOGICAL_Y_POSITION(ty + Y_TILT_FULCRUM);
-      z = LOGICAL_Z_POSITION(tz);
+      lx = LOGICAL_X_POSITION(dx + X_TILT_FULCRUM);
+      ly = LOGICAL_Y_POSITION(dy + Y_TILT_FULCRUM);
+      lz = LOGICAL_Z_POSITION(dz);
+
+    #endif
+  }
+
+  void Planner::unapply_leveling(float &lx, float &ly, float &lz) {
+    #if ENABLED(MESH_BED_LEVELING)
+
+      if (mbl.active())
+        lz -= mbl.get_z(RAW_X_POSITION(lx), RAW_Y_POSITION(ly));
+
+    #elif ENABLED(AUTO_BED_LEVELING_LINEAR)
+
+      matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix);
+
+      float dx = lx - (X_TILT_FULCRUM), dy = ly - (Y_TILT_FULCRUM), dz = lz;
+
+      apply_rotation_xyz(inverse, dx, dy, dz);
+
+      lx = LOGICAL_X_POSITION(dx + X_TILT_FULCRUM);
+      ly = LOGICAL_Y_POSITION(dy + Y_TILT_FULCRUM);
+      lz = LOGICAL_Z_POSITION(dz);
 
     #endif
   }
@@ -562,15 +576,7 @@ void Planner::check_axes_activity() {
  *  fr_mm_s   - (target) speed of the move
  *  extruder  - target extruder
  */
-
-void Planner::buffer_line(
-  #if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
-    float x, float y, float z
-  #else
-    const float& x, const float& y, const float& z
-  #endif
-  , const float& e, float fr_mm_s, const uint8_t extruder
-) {
+void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, const uint8_t extruder) {
   // Calculate the buffer head after we push this byte
   int next_buffer_head = next_block_index(block_buffer_head);
 
@@ -578,17 +584,17 @@ void Planner::buffer_line(
   // Rest here until there is room in the buffer.
   while (block_buffer_tail == next_buffer_head) idle();
 
-  #if ENABLED(MESH_BED_LEVELING) || ENABLED(AUTO_BED_LEVELING_FEATURE)
-    apply_leveling(x, y, z);
+  #if PLANNER_LEVELING
+    apply_leveling(lx, ly, lz);
   #endif
 
   // The target position of the tool in absolute steps
   // Calculate target position in absolute steps
   //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
   long target[NUM_AXIS] = {
-    lround(x * axis_steps_per_mm[X_AXIS]),
-    lround(y * axis_steps_per_mm[Y_AXIS]),
-    lround(z * axis_steps_per_mm[Z_AXIS]),
+    lround(lx * axis_steps_per_mm[X_AXIS]),
+    lround(ly * axis_steps_per_mm[Y_AXIS]),
+    lround(lz * axis_steps_per_mm[Z_AXIS]),
     lround(e * axis_steps_per_mm[E_AXIS])
   };
 
@@ -598,11 +604,22 @@ void Planner::buffer_line(
 
   /*
   SERIAL_ECHO_START;
-  SERIAL_ECHOPAIR("Planner X:", x);
-  SERIAL_ECHOPAIR(" (", dx);
-  SERIAL_ECHOPAIR(") Y:", y);
+  SERIAL_ECHOPGM("Planner ", x);
+  #if IS_KINEMATIC
+    SERIAL_ECHOPAIR("A:", x);
+    SERIAL_ECHOPAIR(" (", dx);
+    SERIAL_ECHOPAIR(") B:", y);
+  #else
+    SERIAL_ECHOPAIR("X:", x);
+    SERIAL_ECHOPAIR(" (", dx);
+    SERIAL_ECHOPAIR(") Y:", y);
+  #endif
   SERIAL_ECHOPAIR(" (", dy);
-  SERIAL_ECHOPAIR(") Z:", z);
+  #elif ENABLED(DELTA)
+    SERIAL_ECHOPAIR(") C:", z);
+  #else
+    SERIAL_ECHOPAIR(") Z:", z);
+  #endif
   SERIAL_ECHOPAIR(" (", dz);
   SERIAL_ECHOLNPGM(")");
   //*/
@@ -671,7 +688,7 @@ void Planner::buffer_line(
   // For a mixing extruder, get a magnified step_event_count for each
   #if ENABLED(MIXING_EXTRUDER)
     for (uint8_t i = 0; i < MIXING_STEPPERS; i++)
-      block->mix_event_count[i] = (mixing_factor[i] < 0.0001) ? 0 : block->step_event_count / mixing_factor[i];
+      block->mix_event_count[i] = UNEAR_ZERO(mixing_factor[i]) ? 0 : block->step_event_count / mixing_factor[i];
   #endif
 
   #if FAN_COUNT > 0
@@ -1124,7 +1141,7 @@ void Planner::buffer_line(
       block->advance_rate = acc_dist ? advance / (float)acc_dist : 0;
     }
     /**
-      SERIAL_ECHO_START;
+     SERIAL_ECHO_START;
      SERIAL_ECHOPGM("advance :");
      SERIAL_ECHO(block->advance/256.0);
      SERIAL_ECHOPGM("advance rate :");
@@ -1152,22 +1169,15 @@ void Planner::buffer_line(
  *
  * On CORE machines stepper ABC will be translated from the given XYZ.
  */
-void Planner::set_position_mm(
-  #if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
-    float x, float y, float z
-  #else
-    const float& x, const float& y, const float& z
-  #endif
-  , const float& e
-) {
+void Planner::set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) {
 
-  #if ENABLED(MESH_BED_LEVELING) || ENABLED(AUTO_BED_LEVELING_FEATURE)
-    apply_leveling(x, y, z);
+  #if PLANNER_LEVELING
+    apply_leveling(lx, ly, lz);
   #endif
 
-  long nx = position[X_AXIS] = lround(x * axis_steps_per_mm[X_AXIS]),
-       ny = position[Y_AXIS] = lround(y * axis_steps_per_mm[Y_AXIS]),
-       nz = position[Z_AXIS] = lround(z * axis_steps_per_mm[Z_AXIS]),
+  long nx = position[X_AXIS] = lround(lx * axis_steps_per_mm[X_AXIS]),
+       ny = position[Y_AXIS] = lround(ly * axis_steps_per_mm[Y_AXIS]),
+       nz = position[Z_AXIS] = lround(lz * axis_steps_per_mm[Z_AXIS]),
        ne = position[E_AXIS] = lround(e * axis_steps_per_mm[E_AXIS]);
   stepper.set_position(nx, ny, nz, ne);
   previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
diff --git a/Marlin/planner.h b/Marlin/planner.h
index ecca0fd3fc..e38e9e5f03 100644
--- a/Marlin/planner.h
+++ b/Marlin/planner.h
@@ -202,39 +202,45 @@ class Planner {
     static bool is_full() { return (block_buffer_tail == BLOCK_MOD(block_buffer_head + 1)); }
 
     #if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
-
-      #if ENABLED(MESH_BED_LEVELING)
-        static void apply_leveling(const float &x, const float &y, float &z);
-      #else
-        static void apply_leveling(float &x, float &y, float &z);
-      #endif
-
-      /**
-       * Add a new linear movement to the buffer.
-       *
-       *  x,y,z,e   - target position in mm
-       *  fr_mm_s   - (target) speed of the move (mm/s)
-       *  extruder  - target extruder
-       */
-      static void buffer_line(float x, float y, float z, const float& e, float fr_mm_s, const uint8_t extruder);
-
-      /**
-       * Set the planner.position and individual stepper positions.
-       * Used by G92, G28, G29, and other procedures.
-       *
-       * Multiplies by axis_steps_per_mm[] and does necessary conversion
-       * for COREXY / COREXZ / COREYZ to set the corresponding stepper positions.
-       *
-       * Clears previous speed values.
-       */
-      static void set_position_mm(float x, float y, float z, const float& e);
-
+      #define ARG_X float lx
+      #define ARG_Y float ly
+      #define ARG_Z float lz
     #else
+      #define ARG_X const float &lx
+      #define ARG_Y const float &ly
+      #define ARG_Z const float &lz
+    #endif
 
-      static void buffer_line(const float& x, const float& y, const float& z, const float& e, float fr_mm_s, const uint8_t extruder);
-      static void set_position_mm(const float& x, const float& y, const float& z, const float& e);
+    #if PLANNER_LEVELING
 
-    #endif // AUTO_BED_LEVELING_FEATURE || MESH_BED_LEVELING
+      /**
+       * Apply leveling to transform a cartesian position
+       * as it will be given to the planner and steppers.
+       */
+      static void apply_leveling(float &lx, float &ly, float &lz);
+      static void unapply_leveling(float &lx, float &ly, float &lz);
+
+    #endif
+
+    /**
+     * Add a new linear movement to the buffer.
+     *
+     *  x,y,z,e   - target position in mm
+     *  fr_mm_s   - (target) speed of the move (mm/s)
+     *  extruder  - target extruder
+     */
+    static void buffer_line(ARG_X, ARG_Y, ARG_Z, const float& e, float fr_mm_s, const uint8_t extruder);
+
+    /**
+     * Set the planner.position and individual stepper positions.
+     * Used by G92, G28, G29, and other procedures.
+     *
+     * Multiplies by axis_steps_per_mm[] and does necessary conversion
+     * for COREXY / COREXZ / COREYZ to set the corresponding stepper positions.
+     *
+     * Clears previous speed values.
+     */
+    static void set_position_mm(ARG_X, ARG_Y, ARG_Z, const float& e);
 
     /**
      * Set the E position (mm) of the planner (and the E stepper)

From e529c6407e59e1ae0bea6ef830baee833c6b3fd5 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Sun, 11 Sep 2016 21:45:07 -0500
Subject: [PATCH 14/26] Rename set_cartesian_from_steppers, cartesian_position

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

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 42b55bf781..f0ab000e03 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -465,7 +465,7 @@ static uint8_t target_extruder;
   #define COS_60 0.5
 
   float delta[ABC],
-        cartesian_position[XYZ] = { 0 },
+        cartes[XYZ] = { 0 },
         endstop_adj[ABC] = { 0 };
 
   // these are the default values, can be overriden with M665
@@ -487,7 +487,7 @@ static uint8_t target_extruder;
         delta_clip_start_height = Z_MAX_POS;
 
   float delta_safe_distance_from_top();
-  void set_cartesian_from_steppers();
+  void get_cartesian_from_steppers();
 
 #else
 
@@ -509,8 +509,8 @@ static uint8_t target_extruder;
   float delta_segments_per_second = SCARA_SEGMENTS_PER_SECOND,
         delta[ABC],
         axis_scaling[ABC] = { 1, 1, 1 },    // Build size scaling, default to 1
-        cartesian_position[XYZ] = { 0 };
-  void set_cartesian_from_steppers() { }    // to be written later
+        cartes[XYZ] = { 0 };
+  void get_cartesian_from_steppers() { }    // to be written later
 #endif
 
 #if ENABLED(FILAMENT_WIDTH_SENSOR)
@@ -3412,8 +3412,8 @@ inline void gcode_G28() {
 
         // For DELTA/SCARA we need to apply forward kinematics.
         // This returns raw positions and we remap to the space.
-        set_cartesian_from_steppers();
-        LOOP_XYZ(i) current_position[i] = LOGICAL_POSITION(cartesian_position[i], i);
+        get_cartesian_from_steppers();
+        LOOP_XYZ(i) current_position[i] = LOGICAL_POSITION(cartes[i], i);
 
       #else
 
@@ -7741,7 +7741,7 @@ void ok_to_send() {
     // based on a Java function from
     // "Delta Robot Kinematics by Steve Graves" V3
 
-    // Result is in cartesian_position[].
+    // Result is in cartes[].
 
     //Create a vector in old coordinates along x axis of new coordinate
     float p12[3] = { delta_tower2_x - delta_tower1_x, delta_tower2_y - delta_tower1_y, z2 - z1 };
@@ -7785,16 +7785,16 @@ void ok_to_send() {
     //Now we can start from the origin in the old coords and
     //add vectors in the old coords that represent the
     //Xnew, Ynew and Znew to find the point in the old system
-    cartesian_position[X_AXIS] = delta_tower1_x + ex[0]*Xnew + ey[0]*Ynew - ez[0]*Znew;
-    cartesian_position[Y_AXIS] = delta_tower1_y + ex[1]*Xnew + ey[1]*Ynew - ez[1]*Znew;
-    cartesian_position[Z_AXIS] = z1             + ex[2]*Xnew + ey[2]*Ynew - ez[2]*Znew;
+    cartes[X_AXIS] = delta_tower1_x + ex[0]*Xnew + ey[0]*Ynew - ez[0]*Znew;
+    cartes[Y_AXIS] = delta_tower1_y + ex[1]*Xnew + ey[1]*Ynew - ez[1]*Znew;
+    cartes[Z_AXIS] = z1             + ex[2]*Xnew + ey[2]*Ynew - ez[2]*Znew;
   };
 
   void forward_kinematics_DELTA(float point[ABC]) {
     forward_kinematics_DELTA(point[A_AXIS], point[B_AXIS], point[C_AXIS]);
   }
 
-  void set_cartesian_from_steppers() {
+  void get_cartesian_from_steppers() {
     forward_kinematics_DELTA(stepper.get_axis_position_mm(A_AXIS),
                              stepper.get_axis_position_mm(B_AXIS),
                              stepper.get_axis_position_mm(C_AXIS));
@@ -7846,8 +7846,8 @@ void ok_to_send() {
 
 void set_current_from_steppers_for_axis(AxisEnum axis) {
   #if ENABLED(DELTA)
-    set_cartesian_from_steppers();
-    current_position[axis] = LOGICAL_POSITION(cartesian_position[axis], axis);
+    get_cartesian_from_steppers();
+    current_position[axis] = LOGICAL_POSITION(cartes[axis], axis);
   #elif ENABLED(AUTO_BED_LEVELING_FEATURE)
     vector_3 pos = untilted_stepper_position();
     current_position[axis] = axis == X_AXIS ? pos.x : axis == Y_AXIS ? pos.y : pos.z;

From d65f5d816f85f5f4cfb286ea5e874bbc0450173b Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Mon, 12 Sep 2016 03:48:29 -0500
Subject: [PATCH 15/26] Patch to fix kinematics

---
 Marlin/Marlin.h        |  24 ++++-----
 Marlin/Marlin_main.cpp | 107 +++++++++++++++++++++++------------------
 2 files changed, 73 insertions(+), 58 deletions(-)

diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h
index fbea4dc8b9..536a8cf79f 100644
--- a/Marlin/Marlin.h
+++ b/Marlin/Marlin.h
@@ -302,22 +302,24 @@ int code_value_int();
 float code_value_temp_abs();
 float code_value_temp_diff();
 
-#if ENABLED(DELTA)
+#if IS_KINEMATIC
   extern float delta[ABC];
-  extern float endstop_adj[ABC]; // axis[n].endstop_adj
-  extern float delta_radius;
-  extern float delta_diagonal_rod;
-  extern float delta_segments_per_second;
-  extern float delta_diagonal_rod_trim_tower_1;
-  extern float delta_diagonal_rod_trim_tower_2;
-  extern float delta_diagonal_rod_trim_tower_3;
   void inverse_kinematics(const float cartesian[XYZ]);
+#endif
+
+#if ENABLED(DELTA)
+  extern float delta[ABC],
+               endstop_adj[ABC],
+               delta_radius,
+               delta_diagonal_rod,
+               delta_segments_per_second,
+               delta_diagonal_rod_trim_tower_1,
+               delta_diagonal_rod_trim_tower_2,
+               delta_diagonal_rod_trim_tower_3;
   void recalc_delta_settings(float radius, float diagonal_rod);
 #elif IS_SCARA
-  extern float delta[ABC];
   extern float axis_scaling[ABC];  // Build size scaling
-  void inverse_kinematics(const float cartesian[XYZ]);
-  void forward_kinematics_SCARA(float f_scara[ABC]);
+  void forward_kinematics_SCARA(const float &a, const float &b);
 #endif
 
 #if ENABLED(AUTO_BED_LEVELING_NONLINEAR)
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index f0ab000e03..3e83ea606e 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -465,7 +465,6 @@ static uint8_t target_extruder;
   #define COS_60 0.5
 
   float delta[ABC],
-        cartes[XYZ] = { 0 },
         endstop_adj[ABC] = { 0 };
 
   // these are the default values, can be overriden with M665
@@ -487,7 +486,6 @@ static uint8_t target_extruder;
         delta_clip_start_height = Z_MAX_POS;
 
   float delta_safe_distance_from_top();
-  void get_cartesian_from_steppers();
 
 #else
 
@@ -508,11 +506,11 @@ static uint8_t target_extruder;
 
   float delta_segments_per_second = SCARA_SEGMENTS_PER_SECOND,
         delta[ABC],
-        axis_scaling[ABC] = { 1, 1, 1 },    // Build size scaling, default to 1
-        cartes[XYZ] = { 0 };
-  void get_cartesian_from_steppers() { }    // to be written later
+        axis_scaling[ABC] = { 1, 1, 1 };    // Build size scaling, default to 1
 #endif
 
+float cartes[XYZ] = { 0 };
+
 #if ENABLED(FILAMENT_WIDTH_SENSOR)
   bool filament_sensor = false;  //M405 turns on filament_sensor control, M406 turns it off
   float filament_width_nominal = DEFAULT_NOMINAL_FILAMENT_DIA,  // Nominal filament width. Change with M404
@@ -598,6 +596,8 @@ void stop();
 void get_available_commands();
 void process_next_command();
 void prepare_move_to_destination();
+
+void get_cartesian_from_steppers();
 void set_current_from_steppers_for_axis(AxisEnum axis);
 
 #if ENABLED(ARC_SUPPORT)
@@ -1347,7 +1347,7 @@ static void set_axis_is_at_home(AxisEnum axis) {
     }
   #endif
 
-  #if ENABLED(SCARA)
+  #if ENABLED(MORGAN_SCARA)
 
     if (axis == X_AXIS || axis == Y_AXIS) {
 
@@ -1362,19 +1362,19 @@ static void set_axis_is_at_home(AxisEnum axis) {
        * and calculates homing offset using forward kinematics
        */
       inverse_kinematics(homeposition);
-      forward_kinematics_SCARA(delta);
+      forward_kinematics_SCARA(delta[A_AXIS], delta[B_AXIS]);
 
-      // SERIAL_ECHOPAIR("Delta X=", delta[X_AXIS]);
-      // SERIAL_ECHOPGM(" Delta Y="); SERIAL_ECHOLN(delta[Y_AXIS]);
+      // SERIAL_ECHOPAIR("Delta X=", cartes[X_AXIS]);
+      // SERIAL_ECHOPGM(" Delta Y="); SERIAL_ECHOLN(cartes[Y_AXIS]);
 
-      current_position[axis] = LOGICAL_POSITION(delta[axis], axis);
+      current_position[axis] = LOGICAL_POSITION(cartes[axis], axis);
 
       /**
        * SCARA home positions are based on configuration since the actual
        * limits are determined by the inverse kinematic transform.
        */
-      soft_endstop_min[axis] = base_min_pos(axis); // + (delta[axis] - base_home_pos(axis));
-      soft_endstop_max[axis] = base_max_pos(axis); // + (delta[axis] - base_home_pos(axis));
+      soft_endstop_min[axis] = base_min_pos(axis); // + (cartes[axis] - base_home_pos(axis));
+      soft_endstop_max[axis] = base_max_pos(axis); // + (cartes[axis] - base_home_pos(axis));
     }
     else
   #endif
@@ -5089,7 +5089,7 @@ static void report_current_position() {
 
   stepper.report_positions();
 
-  #if ENABLED(SCARA)
+  #if IS_SCARA
     SERIAL_PROTOCOLPGM("SCARA Theta:");
     SERIAL_PROTOCOL(delta[X_AXIS]);
     SERIAL_PROTOCOLPGM("   Psi+Theta:");
@@ -5327,7 +5327,7 @@ inline void gcode_M206() {
     if (code_seen(axis_codes[i]))
       set_home_offset((AxisEnum)i, code_value_axis_units(i));
 
-  #if ENABLED(SCARA)
+  #if IS_SCARA
     if (code_seen('T')) set_home_offset(X_AXIS, code_value_axis_units(X_AXIS)); // Theta
     if (code_seen('P')) set_home_offset(Y_AXIS, code_value_axis_units(Y_AXIS)); // Psi
   #endif
@@ -5808,17 +5808,16 @@ inline void gcode_M303() {
   #endif
 }
 
-#if ENABLED(SCARA)
-  bool SCARA_move_to_cal(uint8_t delta_x, uint8_t delta_y) {
+#if ENABLED(MORGAN_SCARA)
+  bool SCARA_move_to_cal(uint8_t delta_a, uint8_t delta_b) {
     //SoftEndsEnabled = false;              // Ignore soft endstops during calibration
     //SERIAL_ECHOLNPGM(" Soft endstops disabled");
     if (IsRunning()) {
       //gcode_get_destination(); // For X Y Z E F
-      delta[X_AXIS] = delta_x;
-      delta[Y_AXIS] = delta_y;
-      forward_kinematics_SCARA(delta);
-      destination[X_AXIS] = delta[X_AXIS] / axis_scaling[X_AXIS];
-      destination[Y_AXIS] = delta[Y_AXIS] / axis_scaling[Y_AXIS];
+      forward_kinematics_SCARA(delta_a, delta_b);
+      destination[X_AXIS] = cartes[X_AXIS] / axis_scaling[X_AXIS];
+      destination[Y_AXIS] = cartes[Y_AXIS] / axis_scaling[Y_AXIS];
+      destination[Z_AXIS] = current_position[Z_AXIS];
       prepare_move_to_destination();
       //ok_to_send();
       return true;
@@ -7456,7 +7455,7 @@ void process_next_command() {
         gcode_M303();
         break;
 
-      #if ENABLED(SCARA)
+      #if ENABLED(MORGAN_SCARA)
         case 360:  // M360 SCARA Theta pos1
           if (gcode_M360()) return;
           break;
@@ -7794,12 +7793,6 @@ void ok_to_send() {
     forward_kinematics_DELTA(point[A_AXIS], point[B_AXIS], point[C_AXIS]);
   }
 
-  void get_cartesian_from_steppers() {
-    forward_kinematics_DELTA(stepper.get_axis_position_mm(A_AXIS),
-                             stepper.get_axis_position_mm(B_AXIS),
-                             stepper.get_axis_position_mm(C_AXIS));
-  }
-
   #if ENABLED(AUTO_BED_LEVELING_NONLINEAR)
 
     // Adjust print surface height by linear interpolation over the bed_level array.
@@ -8274,32 +8267,32 @@ void prepare_move_to_destination() {
 
 #endif // HAS_CONTROLLERFAN
 
-#if ENABLED(SCARA)
+#if IS_SCARA
 
-  void forward_kinematics_SCARA(float f_scara[ABC]) {
-    // Perform forward kinematics, and place results in delta[]
+  void forward_kinematics_SCARA(const float &a, const float &b) {
+    // Perform forward kinematics, and place results in cartes[]
     // The maths and first version has been done by QHARLEY . Integrated into masterbranch 06/2014 and slightly restructured by Joachim Cerny in June 2014
 
-    float x_sin, x_cos, y_sin, y_cos;
+    float a_sin, a_cos, b_sin, b_cos;
 
-    //SERIAL_ECHOPGM("f_delta x="); SERIAL_ECHO(f_scara[X_AXIS]);
-    //SERIAL_ECHOPGM(" y="); SERIAL_ECHO(f_scara[Y_AXIS]);
+    //SERIAL_ECHOPGM("f_delta x="); SERIAL_ECHO(a);
+    //SERIAL_ECHOPGM(" y="); SERIAL_ECHO(b);
 
-    x_sin = sin(RADIANS(f_scara[X_AXIS])) * L1;
-    x_cos = cos(RADIANS(f_scara[X_AXIS])) * L1;
-    y_sin = sin(RADIANS(f_scara[Y_AXIS])) * L2;
-    y_cos = cos(RADIANS(f_scara[Y_AXIS])) * L2;
+    a_sin = sin(RADIANS(a)) * L1;
+    a_cos = cos(RADIANS(a)) * L1;
+    b_sin = sin(RADIANS(b)) * L2;
+    b_cos = cos(RADIANS(b)) * L2;
 
-    //SERIAL_ECHOPGM(" x_sin="); SERIAL_ECHO(x_sin);
-    //SERIAL_ECHOPGM(" x_cos="); SERIAL_ECHO(x_cos);
-    //SERIAL_ECHOPGM(" y_sin="); SERIAL_ECHO(y_sin);
-    //SERIAL_ECHOPGM(" y_cos="); SERIAL_ECHOLN(y_cos);
+    //SERIAL_ECHOPGM(" a_sin="); SERIAL_ECHO(a_sin);
+    //SERIAL_ECHOPGM(" a_cos="); SERIAL_ECHO(a_cos);
+    //SERIAL_ECHOPGM(" b_sin="); SERIAL_ECHO(b_sin);
+    //SERIAL_ECHOPGM(" b_cos="); SERIAL_ECHOLN(b_cos);
 
-    delta[X_AXIS] = x_cos + y_cos + SCARA_OFFSET_X;  //theta
-    delta[Y_AXIS] = x_sin + y_sin + SCARA_OFFSET_Y;  //theta+phi
+    cartes[X_AXIS] = a_cos + b_cos + SCARA_OFFSET_X;  //theta
+    cartes[Y_AXIS] = a_sin + b_sin + SCARA_OFFSET_Y;  //theta+phi
 
-    //SERIAL_ECHOPGM(" delta[X_AXIS]="); SERIAL_ECHO(delta[X_AXIS]);
-    //SERIAL_ECHOPGM(" delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]);
+    //SERIAL_ECHOPGM(" cartes[X_AXIS]="); SERIAL_ECHO(cartes[X_AXIS]);
+    //SERIAL_ECHOPGM(" cartes[Y_AXIS]="); SERIAL_ECHOLN(cartes[Y_AXIS]);
   }
 
   void inverse_kinematics(const float cartesian[XYZ]) {
@@ -8343,7 +8336,27 @@ void prepare_move_to_destination() {
     //*/
   }
 
-#endif // MORGAN_SCARA
+#endif // IS_SCARA
+
+void get_cartesian_from_steppers() {
+  #if ENABLED(DELTA)
+    forward_kinematics_DELTA(
+      stepper.get_axis_position_mm(A_AXIS),
+      stepper.get_axis_position_mm(B_AXIS),
+      stepper.get_axis_position_mm(C_AXIS)
+    );
+  #elif IS_SCARA
+    forward_kinematics_SCARA(
+      stepper.get_axis_position_degrees(A_AXIS),
+      stepper.get_axis_position_degrees(B_AXIS)
+    );
+    cartes[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
+  #else
+    cartes[X_AXIS] = stepper.get_axis_position_mm(X_AXIS);
+    cartes[Y_AXIS] = stepper.get_axis_position_mm(Y_AXIS);
+    cartes[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
+  #endif
+}
 
 #if ENABLED(TEMP_STAT_LEDS)
 

From 82be65065c65917957c4d2452fb39f54ffcde405 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Mon, 12 Sep 2016 02:52:52 -0500
Subject: [PATCH 16/26] Fix up untilted_stepper_position and
 set_current_from_steppers_for_axis

---
 Marlin/Marlin_main.cpp | 16 +++++++++-------
 1 file changed, 9 insertions(+), 7 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 3e83ea606e..5f6ac563b9 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -2073,10 +2073,12 @@ static void clean_up_after_endstop_or_probe_move() {
      * using the home XY and Z0 position as the fulcrum.
      */
     vector_3 untilted_stepper_position() {
+      get_cartesian_from_steppers();
+
       vector_3 pos = vector_3(
-        RAW_X_POSITION(stepper.get_axis_position_mm(X_AXIS)) - X_TILT_FULCRUM,
-        RAW_Y_POSITION(stepper.get_axis_position_mm(Y_AXIS)) - Y_TILT_FULCRUM,
-        RAW_Z_POSITION(stepper.get_axis_position_mm(Z_AXIS))
+        cartes[X_AXIS] - X_TILT_FULCRUM,
+        cartes[Y_AXIS] - Y_TILT_FULCRUM,
+        cartes[Z_AXIS]
       );
 
       matrix_3x3 inverse = matrix_3x3::transpose(planner.bed_level_matrix);
@@ -7838,12 +7840,12 @@ void ok_to_send() {
 #endif // DELTA
 
 void set_current_from_steppers_for_axis(AxisEnum axis) {
-  #if ENABLED(DELTA)
-    get_cartesian_from_steppers();
-    current_position[axis] = LOGICAL_POSITION(cartes[axis], axis);
-  #elif ENABLED(AUTO_BED_LEVELING_FEATURE)
+  #if ENABLED(AUTO_BED_LEVELING_LINEAR)
     vector_3 pos = untilted_stepper_position();
     current_position[axis] = axis == X_AXIS ? pos.x : axis == Y_AXIS ? pos.y : pos.z;
+  #elif IS_KINEMATIC
+    get_cartesian_from_steppers();
+    current_position[axis] = LOGICAL_POSITION(cartes[axis], axis);
   #else
     current_position[axis] = stepper.get_axis_position_mm(axis); // CORE handled transparently
   #endif

From a9ed23225ff5d524297b064e04b6638ad2bb1669 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Sun, 11 Sep 2016 21:46:27 -0500
Subject: [PATCH 17/26] bed_level => bed_level_grid

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

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 5f6ac563b9..d12693ebf0 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -495,7 +495,7 @@ static uint8_t target_extruder;
 
 #if ENABLED(AUTO_BED_LEVELING_NONLINEAR)
   int nonlinear_grid_spacing[2] = { 0 };
-  float bed_level[AUTO_BED_LEVELING_GRID_POINTS][AUTO_BED_LEVELING_GRID_POINTS];
+  float bed_level_grid[AUTO_BED_LEVELING_GRID_POINTS][AUTO_BED_LEVELING_GRID_POINTS];
 #endif
 
 #if IS_SCARA
@@ -2104,12 +2104,12 @@ static void clean_up_after_endstop_or_probe_move() {
      * All DELTA leveling in the Marlin uses NONLINEAR_BED_LEVELING
      */
     static void extrapolate_one_point(int x, int y, int xdir, int ydir) {
-      if (bed_level[x][y] != 0.0) {
+      if (bed_level_grid[x][y] != 0.0) {
         return;  // Don't overwrite good values.
       }
-      float a = 2 * bed_level[x + xdir][y] - bed_level[x + xdir * 2][y]; // Left to right.
-      float b = 2 * bed_level[x][y + ydir] - bed_level[x][y + ydir * 2]; // Front to back.
-      float c = 2 * bed_level[x + xdir][y + ydir] - bed_level[x + xdir * 2][y + ydir * 2]; // Diagonal.
+      float a = 2 * bed_level_grid[x + xdir][y] - bed_level_grid[x + xdir * 2][y]; // Left to right.
+      float b = 2 * bed_level_grid[x][y + ydir] - bed_level_grid[x][y + ydir * 2]; // Front to back.
+      float c = 2 * bed_level_grid[x + xdir][y + ydir] - bed_level_grid[x + xdir * 2][y + ydir * 2]; // Diagonal.
       float median = c;  // Median is robust (ignores outliers).
       if (a < b) {
         if (b < c) median = b;
@@ -2119,7 +2119,7 @@ static void clean_up_after_endstop_or_probe_move() {
         if (c < b) median = b;
         if (a < c) median = a;
       }
-      bed_level[x][y] = median;
+      bed_level_grid[x][y] = median;
     }
 
     /**
@@ -2145,7 +2145,7 @@ static void clean_up_after_endstop_or_probe_move() {
     static void print_bed_level() {
       for (int y = 0; y < AUTO_BED_LEVELING_GRID_POINTS; y++) {
         for (int x = 0; x < AUTO_BED_LEVELING_GRID_POINTS; x++) {
-          SERIAL_PROTOCOL_F(bed_level[x][y], 2);
+          SERIAL_PROTOCOL_F(bed_level_grid[x][y], 2);
           SERIAL_PROTOCOLCHAR(' ');
         }
         SERIAL_EOL;
@@ -2161,7 +2161,7 @@ static void clean_up_after_endstop_or_probe_move() {
       #endif
       for (int y = 0; y < AUTO_BED_LEVELING_GRID_POINTS; y++) {
         for (int x = 0; x < AUTO_BED_LEVELING_GRID_POINTS; x++) {
-          bed_level[x][y] = 0.0;
+          bed_level_grid[x][y] = 0.0;
         }
       }
     }
@@ -3513,7 +3513,7 @@ inline void gcode_G28() {
 
           #elif ENABLED(AUTO_BED_LEVELING_NONLINEAR)
 
-            bed_level[xCount][yCount] = measured_z + zoffset;
+            bed_level_grid[xCount][yCount] = measured_z + zoffset;
 
           #endif
 
@@ -7807,10 +7807,10 @@ void ok_to_send() {
             grid_y = max(h1, min(h2, RAW_Y_POSITION(cartesian[Y_AXIS]) / nonlinear_grid_spacing[Y_AXIS]));
       int floor_x = floor(grid_x), floor_y = floor(grid_y);
       float ratio_x = grid_x - floor_x, ratio_y = grid_y - floor_y,
-            z1 = bed_level[floor_x + half][floor_y + half],
-            z2 = bed_level[floor_x + half][floor_y + half + 1],
-            z3 = bed_level[floor_x + half + 1][floor_y + half],
-            z4 = bed_level[floor_x + half + 1][floor_y + half + 1],
+            z1 = bed_level_grid[floor_x + half][floor_y + half],
+            z2 = bed_level_grid[floor_x + half][floor_y + half + 1],
+            z3 = bed_level_grid[floor_x + half + 1][floor_y + half],
+            z4 = bed_level_grid[floor_x + half + 1][floor_y + half + 1],
             left = (1 - ratio_y) * z1 + ratio_y * z2,
             right = (1 - ratio_y) * z3 + ratio_y * z4,
             offset = (1 - ratio_x) * left + ratio_x * right;

From 3236762e07f9eb7c8a6f384fea26348ceb4365c6 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Sun, 11 Sep 2016 22:42:54 -0500
Subject: [PATCH 18/26] Clean up sanity check for LCD_PROGRESS_BAR

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

diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h
index d04ae6992c..5a59670fbc 100644
--- a/Marlin/SanityCheck.h
+++ b/Marlin/SanityCheck.h
@@ -181,11 +181,9 @@
 #if ENABLED(LCD_PROGRESS_BAR)
   #if DISABLED(SDSUPPORT)
     #error "LCD_PROGRESS_BAR requires SDSUPPORT."
-  #endif
-  #if ENABLED(DOGLCD)
+  #elif ENABLED(DOGLCD)
     #error "LCD_PROGRESS_BAR does not apply to graphical displays."
-  #endif
-  #if ENABLED(FILAMENT_LCD_DISPLAY)
+  #elif ENABLED(FILAMENT_LCD_DISPLAY)
     #error "LCD_PROGRESS_BAR and FILAMENT_LCD_DISPLAY are not fully compatible. Comment out this line to use both."
   #endif
 #endif

From 40785ce8ac45d55552531bfb8d56964bfe54e8c5 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Sun, 11 Sep 2016 23:39:15 -0500
Subject: [PATCH 19/26] Use count method for kinematic sanity check

---
 Marlin/SanityCheck.h | 37 ++++++++++++++++++++++++++++++++-----
 1 file changed, 32 insertions(+), 5 deletions(-)

diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h
index 5a59670fbc..3922a30d0c 100644
--- a/Marlin/SanityCheck.h
+++ b/Marlin/SanityCheck.h
@@ -573,11 +573,38 @@
 /**
  * Don't set more than one kinematic type
  */
-#if (ENABLED(DELTA) && (ENABLED(MORGAN_SCARA) || ENABLED(MAKERARM_SCARA) || ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ))) \
- || (ENABLED(DELTA) && (ENABLED(MAKERARM_SCARA) || ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ))) \
- || (ENABLED(SCARA) && (ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ))) \
- || (ENABLED(COREXY) && (ENABLED(COREXZ) || ENABLED(COREYZ))) \
- || (ENABLED(COREXZ) && ENABLED(COREYZ))
+#define COUNT_KIN_1 0
+#if ENABLED(DELTA)
+  #define COUNT_KIN_2 INCREMENT(COUNT_KIN_1)
+#else
+  #define COUNT_KIN_2 COUNT_KIN_1
+#endif
+#if ENABLED(MORGAN_SCARA)
+  #define COUNT_KIN_3 INCREMENT(COUNT_KIN_2)
+#else
+  #define COUNT_KIN_3 COUNT_KIN_2
+#endif
+#if ENABLED(MAKERARM_SCARA)
+  #define COUNT_KIN_4 INCREMENT(COUNT_KIN_3)
+#else
+  #define COUNT_KIN_4 COUNT_KIN_3
+#endif
+#if ENABLED(COREXY)
+  #define COUNT_KIN_5 INCREMENT(COUNT_KIN_4)
+#else
+  #define COUNT_KIN_5 COUNT_KIN_4
+#endif
+#if ENABLED(COREXZ)
+  #define COUNT_KIN_6 INCREMENT(COUNT_KIN_5)
+#else
+  #define COUNT_KIN_6 COUNT_KIN_5
+#endif
+#if ENABLED(COREYZ)
+  #define COUNT_KIN_7 INCREMENT(COUNT_KIN_6)
+#else
+  #define COUNT_KIN_7 COUNT_KIN_6
+#endif
+#if COUNT_KIN_7 > 1
   #error "Please enable only one of DELTA, MORGAN_SCARA, MAKERARM_SCARA, COREXY, COREXZ, or COREYZ."
 #endif
 

From 41d8149bce6a9cc1da658a7a12feb40e0375374a Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Sun, 11 Sep 2016 23:33:09 -0500
Subject: [PATCH 20/26] Sanity check for more than 1 controller

---
 Marlin/SanityCheck.h | 133 +++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 133 insertions(+)

diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h
index 3922a30d0c..6a3433e387 100644
--- a/Marlin/SanityCheck.h
+++ b/Marlin/SanityCheck.h
@@ -811,3 +811,136 @@
     #error "I2C_SLAVE_ADDRESS can't be over 127. (Only 7 bits allowed.)"
   #endif
 #endif
+
+/**
+ * Make sure only one display is enabled
+ *
+ * Note: BQ_LCD_SMART_CONTROLLER => REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER
+ *       REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER => REPRAP_DISCOUNT_SMART_CONTROLLER
+ *       SAV_3DGLCD => U8GLIB_SH1106 => ULTIMAKERCONTROLLER
+ *       miniVIKI => ULTIMAKERCONTROLLER
+ *       VIKI2 => ULTIMAKERCONTROLLER
+ *       ELB_FULL_GRAPHIC_CONTROLLER => ULTIMAKERCONTROLLER
+ *       PANEL_ONE => ULTIMAKERCONTROLLER
+ */
+#define COUNT_LCD_1 0
+#if ENABLED(ULTIMAKERCONTROLLER) \
+    && DISABLED(SAV_3DGLCD) && DISABLED(miniVIKI) && DISABLED(VIKI2) \
+    && DISABLED(ELB_FULL_GRAPHIC_CONTROLLER) && DISABLED(PANEL_ONE)
+  #define COUNT_LCD_2 INCREMENT(COUNT_LCD_1)
+#else
+  #define COUNT_LCD_2 COUNT_LCD_1
+#endif
+#if ENABLED(REPRAP_DISCOUNT_SMART_CONTROLLER) && DISABLED(REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER)
+  #define COUNT_LCD_3 INCREMENT(COUNT_LCD_2)
+#else
+  #define COUNT_LCD_3 COUNT_LCD_2
+#endif
+#if ENABLED(REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER) && DISABLED(BQ_LCD_SMART_CONTROLLER)
+  #define COUNT_LCD_4 INCREMENT(COUNT_LCD_3)
+#else
+  #define COUNT_LCD_4 COUNT_LCD_3
+#endif
+#if ENABLED(CARTESIO_UI)
+  #define COUNT_LCD_5 INCREMENT(COUNT_LCD_4)
+#else
+  #define COUNT_LCD_5 COUNT_LCD_4
+#endif
+#if ENABLED(PANEL_ONE)
+  #define COUNT_LCD_6 INCREMENT(COUNT_LCD_5)
+#else
+  #define COUNT_LCD_6 COUNT_LCD_5
+#endif
+#if ENABLED(MAKRPANEL)
+  #define COUNT_LCD_7 INCREMENT(COUNT_LCD_6)
+#else
+  #define COUNT_LCD_7 COUNT_LCD_6
+#endif
+#if ENABLED(REPRAPWORLD_GRAPHICAL_LCD)
+  #define COUNT_LCD_8 INCREMENT(COUNT_LCD_7)
+#else
+  #define COUNT_LCD_8 COUNT_LCD_7
+#endif
+#if ENABLED(VIKI2)
+  #define COUNT_LCD_9 INCREMENT(COUNT_LCD_8)
+#else
+  #define COUNT_LCD_9 COUNT_LCD_8
+#endif
+#if ENABLED(miniVIKI)
+  #define COUNT_LCD_10 INCREMENT(COUNT_LCD_9)
+#else
+  #define COUNT_LCD_10 COUNT_LCD_9
+#endif
+#if ENABLED(ELB_FULL_GRAPHIC_CONTROLLER)
+  #define COUNT_LCD_11 INCREMENT(COUNT_LCD_10)
+#else
+  #define COUNT_LCD_11 COUNT_LCD_10
+#endif
+#if ENABLED(G3D_PANEL)
+  #define COUNT_LCD_12 INCREMENT(COUNT_LCD_11)
+#else
+  #define COUNT_LCD_12 COUNT_LCD_11
+#endif
+#if ENABLED(MINIPANEL)
+  #define COUNT_LCD_13 INCREMENT(COUNT_LCD_12)
+#else
+  #define COUNT_LCD_13 COUNT_LCD_12
+#endif
+#if ENABLED(REPRAPWORLD_KEYPAD)
+  #define COUNT_LCD_14 INCREMENT(COUNT_LCD_13)
+#else
+  #define COUNT_LCD_14 COUNT_LCD_13
+#endif
+#if ENABLED(RIGIDBOT_PANEL)
+  #define COUNT_LCD_15 INCREMENT(COUNT_LCD_14)
+#else
+  #define COUNT_LCD_15 COUNT_LCD_14
+#endif
+#if ENABLED(RA_CONTROL_PANEL)
+  #define COUNT_LCD_16 INCREMENT(COUNT_LCD_15)
+#else
+  #define COUNT_LCD_16 COUNT_LCD_15
+#endif
+#if ENABLED(LCD_I2C_SAINSMART_YWROBOT)
+  #define COUNT_LCD_17 INCREMENT(COUNT_LCD_16)
+#else
+  #define COUNT_LCD_17 COUNT_LCD_16
+#endif
+#if ENABLED(LCM1602)
+  #define COUNT_LCD_18 INCREMENT(COUNT_LCD_17)
+#else
+  #define COUNT_LCD_18 COUNT_LCD_17
+#endif
+#if ENABLED(LCD_I2C_PANELOLU2)
+  #define COUNT_LCD_19 INCREMENT(COUNT_LCD_18)
+#else
+  #define COUNT_LCD_19 COUNT_LCD_18
+#endif
+#if ENABLED(LCD_I2C_VIKI)
+  #define COUNT_LCD_20 INCREMENT(COUNT_LCD_19)
+#else
+  #define COUNT_LCD_20 COUNT_LCD_19
+#endif
+#if ENABLED(U8GLIB_SSD1306)
+  #define COUNT_LCD_21 INCREMENT(COUNT_LCD_20)
+#else
+  #define COUNT_LCD_21 COUNT_LCD_20
+#endif
+#if ENABLED(SAV_3DLCD)
+  #define COUNT_LCD_22 INCREMENT(COUNT_LCD_21)
+#else
+  #define COUNT_LCD_22 COUNT_LCD_21
+#endif
+#if ENABLED(BQ_LCD_SMART_CONTROLLER)
+  #define COUNT_LCD_23 INCREMENT(COUNT_LCD_22)
+#else
+  #define COUNT_LCD_23 COUNT_LCD_22
+#endif
+#if ENABLED(SAV_3DGLCD)
+  #define COUNT_LCD_24 INCREMENT(COUNT_LCD_23)
+#else
+  #define COUNT_LCD_24 COUNT_LCD_23
+#endif
+#if COUNT_LCD_24 > 1
+  #error "Please select no more than one LCD controller option."
+#endif

From 4f75ce695786ae3494c1ee1d1799f0c4566334d2 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Mon, 12 Sep 2016 03:58:40 -0500
Subject: [PATCH 21/26] Unify reset_bed_level for linear/non-linear

---
 Marlin/Marlin.h        |  1 -
 Marlin/Marlin_main.cpp | 62 +++++++++++++++++-------------------------
 2 files changed, 25 insertions(+), 38 deletions(-)

diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h
index 536a8cf79f..d34445ac43 100644
--- a/Marlin/Marlin.h
+++ b/Marlin/Marlin.h
@@ -220,7 +220,6 @@ void disable_all_steppers();
 void FlushSerialRequestResend();
 void ok_to_send();
 
-void reset_bed_level();
 void kill(const char*);
 
 void quickstop_stepper();
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index d12693ebf0..a1f18224fd 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -1955,10 +1955,6 @@ static void clean_up_after_endstop_or_probe_move() {
     // Prevent stepper_inactive_time from running out and EXTRUDER_RUNOUT_PREVENT from extruding
     refresh_cmd_timeout();
 
-    #if ENABLED(AUTO_BED_LEVELING_LINEAR)
-      planner.bed_level_matrix.set_to_identity();
-    #endif
-
     #if ENABLED(PROBE_DOUBLE_TOUCH)
 
       // Do a first probe at the fast speed
@@ -2152,22 +2148,23 @@ static void clean_up_after_endstop_or_probe_move() {
       }
     }
 
-    /**
-     * Reset calibration results to zero.
-     */
-    void reset_bed_level() {
-      #if ENABLED(DEBUG_LEVELING_FEATURE)
-        if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("reset_bed_level");
-      #endif
-      for (int y = 0; y < AUTO_BED_LEVELING_GRID_POINTS; y++) {
-        for (int x = 0; x < AUTO_BED_LEVELING_GRID_POINTS; x++) {
-          bed_level_grid[x][y] = 0.0;
-        }
-      }
-    }
-
   #endif // DELTA
 
+  /**
+   * Reset calibration results to zero.
+   */
+  void reset_bed_level() {
+    #if ENABLED(DEBUG_LEVELING_FEATURE)
+      if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("reset_bed_level");
+    #endif
+    #if ENABLED(AUTO_BED_LEVELING_LINEAR)
+      planner.bed_level_matrix.set_to_identity();
+    #elif ENABLED(AUTO_BED_LEVELING_NONLINEAR)
+      memset(bed_level_grid, 0, sizeof(bed_level_grid));
+      nonlinear_grid_spacing[X_AXIS] = nonlinear_grid_spacing[Y_AXIS] = 0;
+    #endif
+  }
+
 #endif // AUTO_BED_LEVELING_FEATURE
 
 /**
@@ -2848,11 +2845,7 @@ inline void gcode_G28() {
   stepper.synchronize();
 
   // For auto bed leveling, clear the level matrix
-  #if ENABLED(AUTO_BED_LEVELING_LINEAR)
-    planner.bed_level_matrix.set_to_identity();
-  #endif
-
-  #if ENABLED(AUTO_BED_LEVELING_NONLINEAR)
+  #if ENABLED(AUTO_BED_LEVELING_FEATURE)
     reset_bed_level();
   #endif
 
@@ -3396,11 +3389,9 @@ inline void gcode_G28() {
 
     if (!dryrun) {
 
-      #if ENABLED(AUTO_BED_LEVELING_LINEAR)
-        // Reset the bed_level_matrix because leveling
-        // needs to be done without leveling enabled.
-        planner.bed_level_matrix.set_to_identity();
-      #endif
+      // Reset the bed_level_matrix because leveling
+      // needs to be done without leveling enabled.
+      reset_bed_level();
 
       //
       // Re-orient the current position without leveling
@@ -3408,10 +3399,6 @@ inline void gcode_G28() {
       //
       #if IS_KINEMATIC
 
-        #if ENABLED(AUTO_BED_LEVELING_NONLINEAR)
-          reset_bed_level();
-        #endif
-
         // For DELTA/SCARA we need to apply forward kinematics.
         // This returns raw positions and we remap to the space.
         get_cartesian_from_steppers();
@@ -3757,6 +3744,10 @@ inline void gcode_G28() {
    */
   inline void gcode_G30() {
 
+    #if ENABLED(AUTO_BED_LEVELING_FEATURE)
+      reset_bed_level();
+    #endif
+
     setup_for_endstop_or_probe_move();
 
     // TODO: clear the leveling matrix or the planner will be set incorrectly
@@ -4199,12 +4190,9 @@ inline void gcode_M42() {
     if (verbose_level > 2)
       SERIAL_PROTOCOLLNPGM("Positioning the probe...");
 
-    #if ENABLED(AUTO_BED_LEVELING_NONLINEAR)
-      // we don't do bed level correction in M48 because we want the raw data when we probe
+    // we don't do bed level correction in M48 because we want the raw data when we probe
+    #if ENABLED(AUTO_BED_LEVELING_FEATURE)
       reset_bed_level();
-    #elif ENABLED(AUTO_BED_LEVELING_LINEAR)
-      // we don't do bed level correction in M48 because we want the raw data when we probe
-      planner.bed_level_matrix.set_to_identity();
     #endif
 
     setup_for_endstop_or_probe_move();

From 0e38bea79dd89c084783590cb9df2c153ce0907c Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Mon, 12 Sep 2016 04:02:17 -0500
Subject: [PATCH 22/26] Shrink 3-point leveling code

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

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index a1f18224fd..70c5929bdd 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -3518,23 +3518,21 @@ inline void gcode_G28() {
       #endif
 
       // Probe at 3 arbitrary points
-      float z_at_pt_1 = probe_pt( LOGICAL_X_POSITION(ABL_PROBE_PT_1_X),
-                                  LOGICAL_Y_POSITION(ABL_PROBE_PT_1_Y),
-                                  stow_probe_after_each, verbose_level),
-            z_at_pt_2 = probe_pt( LOGICAL_X_POSITION(ABL_PROBE_PT_2_X),
-                                  LOGICAL_Y_POSITION(ABL_PROBE_PT_2_Y),
-                                  stow_probe_after_each, verbose_level),
-            z_at_pt_3 = probe_pt( LOGICAL_X_POSITION(ABL_PROBE_PT_3_X),
-                                  LOGICAL_Y_POSITION(ABL_PROBE_PT_3_Y),
-                                  stow_probe_after_each, verbose_level);
+      vector_3 points[3] = {
+        vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, 0),
+        vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, 0),
+        vector_3(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, 0)
+      };
+
+      for (uint8_t i = 0; i < 3; ++i)
+        points[i].z = probe_pt(
+          LOGICAL_X_POSITION(points[i].x),
+          LOGICAL_Y_POSITION(points[i].y),
+          stow_probe_after_each, verbose_level
+        );
 
       if (!dryrun) {
-        vector_3 pt1 = vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, z_at_pt_1),
-                 pt2 = vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, z_at_pt_2),
-                 pt3 = vector_3(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, z_at_pt_3);
-
-        vector_3 planeNormal = vector_3::cross(pt1 - pt2, pt3 - pt2).get_normal();
-
+        vector_3 planeNormal = vector_3::cross(points[0] - points[1], points[2] - points[1]).get_normal();
         if (planeNormal.z < 0) {
           planeNormal.x *= -1;
           planeNormal.y *= -1;

From 788a16fc46214a7e08d841ce25c11d83a3b4d532 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Mon, 12 Sep 2016 17:49:35 -0500
Subject: [PATCH 23/26] Clean up serial out code

---
 Marlin/Marlin_main.cpp | 190 ++++++++++++++++-------------------------
 Marlin/language.h      |   6 +-
 2 files changed, 78 insertions(+), 118 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 70c5929bdd..630b6f188c 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -764,8 +764,7 @@ void enqueue_and_echo_command_now(const char* cmd) {
 bool enqueue_and_echo_command(const char* cmd, bool say_ok/*=false*/) {
   if (_enqueuecommand(cmd, say_ok)) {
     SERIAL_ECHO_START;
-    SERIAL_ECHOPGM(MSG_Enqueueing);
-    SERIAL_ECHO(cmd);
+    SERIAL_ECHOPAIR(MSG_Enqueueing, cmd);
     SERIAL_ECHOLNPGM("\"");
     return true;
   }
@@ -1354,8 +1353,8 @@ static void set_axis_is_at_home(AxisEnum axis) {
       float homeposition[XYZ];
       LOOP_XYZ(i) homeposition[i] = LOGICAL_POSITION(base_home_pos(i), i);
 
-      // SERIAL_ECHOPGM("homeposition[x]= "); SERIAL_ECHO(homeposition[0]);
-      // SERIAL_ECHOPGM("homeposition[y]= "); SERIAL_ECHOLN(homeposition[1]);
+      // SERIAL_ECHOPAIR("homeposition X:", homeposition[X_AXIS]);
+      // SERIAL_ECHOLNPAIR(" Y:", homeposition[Y_AXIS]);
 
       /**
        * Works out real Homeposition angles using inverse kinematics,
@@ -1364,8 +1363,8 @@ static void set_axis_is_at_home(AxisEnum axis) {
       inverse_kinematics(homeposition);
       forward_kinematics_SCARA(delta[A_AXIS], delta[B_AXIS]);
 
-      // SERIAL_ECHOPAIR("Delta X=", cartes[X_AXIS]);
-      // SERIAL_ECHOPGM(" Delta Y="); SERIAL_ECHOLN(cartes[Y_AXIS]);
+      // SERIAL_ECHOPAIR("Cartesian X:", cartes[X_AXIS]);
+      // SERIAL_ECHOLNPAIR(" Y:", cartes[Y_AXIS]);
 
       current_position[axis] = LOGICAL_POSITION(cartes[axis], axis);
 
@@ -2019,17 +2018,11 @@ static void clean_up_after_endstop_or_probe_move() {
     feedrate_mm_s = XY_PROBE_FEEDRATE_MM_S;
     do_blocking_move_to_xy(x - (X_PROBE_OFFSET_FROM_EXTRUDER), y - (Y_PROBE_OFFSET_FROM_EXTRUDER));
 
-    #if ENABLED(DEBUG_LEVELING_FEATURE)
-      if (DEBUGGING(LEVELING)) SERIAL_ECHOPGM("> ");
-    #endif
     if (DEPLOY_PROBE()) return NAN;
 
     float measured_z = run_z_probe();
 
     if (stow) {
-      #if ENABLED(DEBUG_LEVELING_FEATURE)
-        if (DEBUGGING(LEVELING)) SERIAL_ECHOPGM("> ");
-      #endif
       if (STOW_PROBE()) return NAN;
     }
     else {
@@ -2203,12 +2196,7 @@ static void homeaxis(AxisEnum axis) {
 
   // Homing Z towards the bed? Deploy the Z probe or endstop.
   #if HOMING_Z_WITH_PROBE
-    if (axis == Z_AXIS) {
-      #if ENABLED(DEBUG_LEVELING_FEATURE)
-        if (DEBUGGING(LEVELING)) SERIAL_ECHOPGM("> ");
-      #endif
-      if (DEPLOY_PROBE()) return;
-    }
+    if (axis == Z_AXIS && DEPLOY_PROBE()) return;
   #endif
 
   // Set a flag for Z motor locking
@@ -2286,12 +2274,7 @@ static void homeaxis(AxisEnum axis) {
 
   // Put away the Z probe
   #if HOMING_Z_WITH_PROBE
-    if (axis == Z_AXIS) {
-      #if ENABLED(DEBUG_LEVELING_FEATURE)
-        if (DEBUGGING(LEVELING)) SERIAL_ECHOPGM("> ");
-      #endif
-      if (STOW_PROBE()) return;
-    }
+    if (axis == Z_AXIS && STOW_PROBE()) return;
   #endif
 
   #if ENABLED(DEBUG_LEVELING_FEATURE)
@@ -2416,8 +2399,7 @@ void gcode_get_destination() {
 
 void unknown_command_error() {
   SERIAL_ECHO_START;
-  SERIAL_ECHOPGM(MSG_UNKNOWN_COMMAND);
-  SERIAL_ECHO(current_command);
+  SERIAL_ECHOPAIR(MSG_UNKNOWN_COMMAND, current_command);
   SERIAL_ECHOLNPGM("\"");
 }
 
@@ -3713,10 +3695,7 @@ inline void gcode_G28() {
 
     #ifdef Z_PROBE_END_SCRIPT
       #if ENABLED(DEBUG_LEVELING_FEATURE)
-        if (DEBUGGING(LEVELING)) {
-          SERIAL_ECHOPGM("Z Probe End Script: ");
-          SERIAL_ECHOLNPGM(Z_PROBE_END_SCRIPT);
-        }
+        if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPAIR("Z Probe End Script: ", Z_PROBE_END_SCRIPT);
       #endif
       enqueue_and_echo_commands_P(PSTR(Z_PROBE_END_SCRIPT));
       stepper.synchronize();
@@ -4002,8 +3981,7 @@ inline void gcode_M31() {
   lcd_setstatus(buffer);
 
   SERIAL_ECHO_START;
-  SERIAL_ECHOPGM("Print time: ");
-  SERIAL_ECHOLN(buffer);
+  SERIAL_ECHOLNPAIR("Print time: ", buffer);
 
   thermalManager.autotempShutdown();
 }
@@ -5358,8 +5336,7 @@ inline void gcode_M206() {
         endstop_adj[i] = code_value_axis_units(i);
         #if ENABLED(DEBUG_LEVELING_FEATURE)
           if (DEBUGGING(LEVELING)) {
-            SERIAL_ECHOPGM("endstop_adj[");
-            SERIAL_ECHO(axis_codes[i]);
+            SERIAL_ECHOPAIR("endstop_adj[", axis_codes[i]);
             SERIAL_ECHOLNPAIR("] = ", endstop_adj[i]);
           }
         #endif
@@ -5442,16 +5419,17 @@ inline void gcode_M211() {
     if (code_seen('S')) soft_endstops_enabled = code_value_bool();
   #endif
   #if ENABLED(min_software_endstops) || ENABLED(max_software_endstops)
-    SERIAL_ECHOPGM(MSG_SOFT_ENDSTOPS ": ");
+    SERIAL_ECHOPGM(MSG_SOFT_ENDSTOPS);
     serialprintPGM(soft_endstops_enabled ? PSTR(MSG_ON) : PSTR(MSG_OFF));
   #else
-    SERIAL_ECHOPGM(MSG_SOFT_ENDSTOPS ": " MSG_OFF);
+    SERIAL_ECHOPGM(MSG_SOFT_ENDSTOPS);
+    SERIAL_ECHOPGM(MSG_OFF);
   #endif
-  SERIAL_ECHOPGM("  " MSG_SOFT_MIN ": ");
+  SERIAL_ECHOPGM(MSG_SOFT_MIN);
   SERIAL_ECHOPAIR(    MSG_X, soft_endstop_min[X_AXIS]);
   SERIAL_ECHOPAIR(" " MSG_Y, soft_endstop_min[Y_AXIS]);
   SERIAL_ECHOPAIR(" " MSG_Z, soft_endstop_min[Z_AXIS]);
-  SERIAL_ECHOPGM("  " MSG_SOFT_MAX ": ");
+  SERIAL_ECHOPGM(MSG_SOFT_MAX);
   SERIAL_ECHOPAIR(    MSG_X, soft_endstop_max[X_AXIS]);
   SERIAL_ECHOPAIR(" " MSG_Y, soft_endstop_max[Y_AXIS]);
   SERIAL_ECHOLNPAIR(" " MSG_Z, soft_endstop_max[Z_AXIS]);
@@ -5569,17 +5547,14 @@ inline void gcode_M226() {
         MOVE_SERVO(servo_index, code_value_int());
       else {
         SERIAL_ECHO_START;
-        SERIAL_ECHOPGM(" Servo ");
-        SERIAL_ECHO(servo_index);
-        SERIAL_ECHOPGM(": ");
-        SERIAL_ECHOLN(servo[servo_index].read());
+        SERIAL_ECHOPAIR(" Servo ", servo_index);
+        SERIAL_ECHOLNPAIR(": ", servo[servo_index].read());
       }
     }
     else {
       SERIAL_ERROR_START;
-      SERIAL_ERROR("Servo ");
-      SERIAL_ERROR(servo_index);
-      SERIAL_ERRORLN(" out of range");
+      SERIAL_ECHOPAIR("Servo ", servo_index);
+      SERIAL_ECHOLNPGM(" out of range");
     }
   }
 
@@ -5635,19 +5610,14 @@ inline void gcode_M226() {
       thermalManager.updatePID();
       SERIAL_ECHO_START;
       #if ENABLED(PID_PARAMS_PER_HOTEND)
-        SERIAL_ECHOPGM(" e:"); // specify extruder in serial output
-        SERIAL_ECHO(e);
+        SERIAL_ECHOPAIR(" e:", e); // specify extruder in serial output
       #endif // PID_PARAMS_PER_HOTEND
-      SERIAL_ECHOPGM(" p:");
-      SERIAL_ECHO(PID_PARAM(Kp, e));
-      SERIAL_ECHOPGM(" i:");
-      SERIAL_ECHO(unscalePID_i(PID_PARAM(Ki, e)));
-      SERIAL_ECHOPGM(" d:");
-      SERIAL_ECHO(unscalePID_d(PID_PARAM(Kd, e)));
+      SERIAL_ECHOPAIR(" p:", PID_PARAM(Kp, e));
+      SERIAL_ECHOPAIR(" i:", unscalePID_i(PID_PARAM(Ki, e)));
+      SERIAL_ECHOPAIR(" d:", unscalePID_d(PID_PARAM(Kd, e)));
       #if ENABLED(PID_EXTRUSION_SCALING)
-        SERIAL_ECHOPGM(" c:");
         //Kc does not have scaling applied above, or in resetting defaults
-        SERIAL_ECHO(PID_PARAM(Kc, e));
+        SERIAL_ECHOPAIR(" c:", PID_PARAM(Kc, e));
       #endif
       SERIAL_EOL;
     }
@@ -5669,12 +5639,9 @@ inline void gcode_M226() {
     thermalManager.updatePID();
 
     SERIAL_ECHO_START;
-    SERIAL_ECHOPGM(" p:");
-    SERIAL_ECHO(thermalManager.bedKp);
-    SERIAL_ECHOPGM(" i:");
-    SERIAL_ECHO(unscalePID_i(thermalManager.bedKi));
-    SERIAL_ECHOPGM(" d:");
-    SERIAL_ECHOLN(unscalePID_d(thermalManager.bedKd));
+    SERIAL_ECHOPAIR(" p:", thermalManager.bedKp);
+    SERIAL_ECHOPAIR(" i:", unscalePID_i(thermalManager.bedKi));
+    SERIAL_ECHOLNPAIR(" d:", unscalePID_d(thermalManager.bedKd));
   }
 
 #endif // PIDTEMPBED
@@ -6138,11 +6105,9 @@ inline void gcode_M503() {
         SERIAL_ECHO(zprobe_zoffset);
       }
       else {
-        SERIAL_ECHOPGM(MSG_Z_MIN);
-        SERIAL_ECHO(Z_PROBE_OFFSET_RANGE_MIN);
+        SERIAL_ECHOPAIR(MSG_Z_MIN, Z_PROBE_OFFSET_RANGE_MIN);
         SERIAL_CHAR(' ');
-        SERIAL_ECHOPGM(MSG_Z_MAX);
-        SERIAL_ECHO(Z_PROBE_OFFSET_RANGE_MAX);
+        SERIAL_ECHOPAIR(MSG_Z_MAX, Z_PROBE_OFFSET_RANGE_MAX);
       }
     }
     else {
@@ -6863,8 +6828,7 @@ void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool n
     #endif // HOTENDS <= 1
 
     SERIAL_ECHO_START;
-    SERIAL_ECHOPGM(MSG_ACTIVE_EXTRUDER);
-    SERIAL_PROTOCOLLN((int)active_extruder);
+    SERIAL_ECHOLNPAIR(MSG_ACTIVE_EXTRUDER, (int)active_extruder);
 
   #endif //!MIXING_EXTRUDER || MIXING_VIRTUAL_TOOLS <= 1
 }
@@ -7685,13 +7649,13 @@ void ok_to_send() {
                           - sq(delta_tower3_y - cartesian[Y_AXIS])
                          ) + cartesian[Z_AXIS];
     /**
-    SERIAL_ECHOPGM("cartesian x="); SERIAL_ECHO(cartesian[X_AXIS]);
-    SERIAL_ECHOPGM(" y="); SERIAL_ECHO(cartesian[Y_AXIS]);
-    SERIAL_ECHOPGM(" z="); SERIAL_ECHOLN(cartesian[Z_AXIS]);
+    SERIAL_ECHOPAIR("cartesian x=", cartesian[X_AXIS]);
+    SERIAL_ECHOPAIR(" y=", cartesian[Y_AXIS]);
+    SERIAL_ECHOLNPAIR(" z=", cartesian[Z_AXIS]);
 
-    SERIAL_ECHOPGM("delta a="); SERIAL_ECHO(delta[A_AXIS]);
-    SERIAL_ECHOPGM(" b="); SERIAL_ECHO(delta[B_AXIS]);
-    SERIAL_ECHOPGM(" c="); SERIAL_ECHOLN(delta[C_AXIS]);
+    SERIAL_ECHOPAIR("delta a=", delta[A_AXIS]);
+    SERIAL_ECHOPAIR(" b=", delta[B_AXIS]);
+    SERIAL_ECHOLNPAIR(" c=", delta[C_AXIS]);
     */
   }
 
@@ -7806,19 +7770,19 @@ void ok_to_send() {
       delta[Z_AXIS] += offset;
 
       /**
-      SERIAL_ECHOPGM("grid_x="); SERIAL_ECHO(grid_x);
-      SERIAL_ECHOPGM(" grid_y="); SERIAL_ECHO(grid_y);
-      SERIAL_ECHOPGM(" floor_x="); SERIAL_ECHO(floor_x);
-      SERIAL_ECHOPGM(" floor_y="); SERIAL_ECHO(floor_y);
-      SERIAL_ECHOPGM(" ratio_x="); SERIAL_ECHO(ratio_x);
-      SERIAL_ECHOPGM(" ratio_y="); SERIAL_ECHO(ratio_y);
-      SERIAL_ECHOPGM(" z1="); SERIAL_ECHO(z1);
-      SERIAL_ECHOPGM(" z2="); SERIAL_ECHO(z2);
-      SERIAL_ECHOPGM(" z3="); SERIAL_ECHO(z3);
-      SERIAL_ECHOPGM(" z4="); SERIAL_ECHO(z4);
-      SERIAL_ECHOPGM(" left="); SERIAL_ECHO(left);
-      SERIAL_ECHOPGM(" right="); SERIAL_ECHO(right);
-      SERIAL_ECHOPGM(" offset="); SERIAL_ECHOLN(offset);
+      SERIAL_ECHOPAIR("grid_x=", grid_x);
+      SERIAL_ECHOPAIR(" grid_y=", grid_y);
+      SERIAL_ECHOPAIR(" floor_x=", floor_x);
+      SERIAL_ECHOPAIR(" floor_y=", floor_y);
+      SERIAL_ECHOPAIR(" ratio_x=", ratio_x);
+      SERIAL_ECHOPAIR(" ratio_y=", ratio_y);
+      SERIAL_ECHOPAIR(" z1=", z1);
+      SERIAL_ECHOPAIR(" z2=", z2);
+      SERIAL_ECHOPAIR(" z3=", z3);
+      SERIAL_ECHOPAIR(" z4=", z4);
+      SERIAL_ECHOPAIR(" left=", left);
+      SERIAL_ECHOPAIR(" right=", right);
+      SERIAL_ECHOLNPAIR(" offset=", offset);
       */
     }
   #endif // AUTO_BED_LEVELING_NONLINEAR
@@ -7910,9 +7874,9 @@ void mesh_line_to_destination(float fr_mm_s, uint8_t x_splits = 0xff, uint8_t y_
     int steps = max(1, int(delta_segments_per_second * seconds));
     float inv_steps = 1.0/steps;
 
-    // SERIAL_ECHOPGM("mm="); SERIAL_ECHO(cartesian_mm);
-    // SERIAL_ECHOPGM(" seconds="); SERIAL_ECHO(seconds);
-    // SERIAL_ECHOPGM(" steps="); SERIAL_ECHOLN(steps);
+    // SERIAL_ECHOPAIR("mm=", cartesian_mm);
+    // SERIAL_ECHOPAIR(" seconds=", seconds);
+    // SERIAL_ECHOLNPAIR(" steps=", steps);
 
     for (int s = 1; s <= steps; s++) {
 
@@ -8263,24 +8227,24 @@ void prepare_move_to_destination() {
 
     float a_sin, a_cos, b_sin, b_cos;
 
-    //SERIAL_ECHOPGM("f_delta x="); SERIAL_ECHO(a);
-    //SERIAL_ECHOPGM(" y="); SERIAL_ECHO(b);
-
     a_sin = sin(RADIANS(a)) * L1;
     a_cos = cos(RADIANS(a)) * L1;
     b_sin = sin(RADIANS(b)) * L2;
     b_cos = cos(RADIANS(b)) * L2;
 
-    //SERIAL_ECHOPGM(" a_sin="); SERIAL_ECHO(a_sin);
-    //SERIAL_ECHOPGM(" a_cos="); SERIAL_ECHO(a_cos);
-    //SERIAL_ECHOPGM(" b_sin="); SERIAL_ECHO(b_sin);
-    //SERIAL_ECHOPGM(" b_cos="); SERIAL_ECHOLN(b_cos);
-
     cartes[X_AXIS] = a_cos + b_cos + SCARA_OFFSET_X;  //theta
     cartes[Y_AXIS] = a_sin + b_sin + SCARA_OFFSET_Y;  //theta+phi
 
-    //SERIAL_ECHOPGM(" cartes[X_AXIS]="); SERIAL_ECHO(cartes[X_AXIS]);
-    //SERIAL_ECHOPGM(" cartes[Y_AXIS]="); SERIAL_ECHOLN(cartes[Y_AXIS]);
+    /*
+      SERIAL_ECHOPAIR("f_delta x=", a);
+      SERIAL_ECHOPAIR(" y=", b);
+      SERIAL_ECHOPAIR(" a_sin=", a_sin);
+      SERIAL_ECHOPAIR(" a_cos=", a_cos);
+      SERIAL_ECHOPAIR(" b_sin=", b_sin);
+      SERIAL_ECHOLNPAIR(" b_cos=", b_cos);
+      SERIAL_ECHOPAIR(" cartes[X_AXIS]=", cartes[X_AXIS]);
+      SERIAL_ECHOLNPAIR(" cartes[Y_AXIS]=", cartes[Y_AXIS]);
+    //*/
   }
 
   void inverse_kinematics(const float cartesian[XYZ]) {
@@ -8771,25 +8735,21 @@ void setup() {
   MCUSR = 0;
 
   SERIAL_ECHOPGM(MSG_MARLIN);
-  SERIAL_ECHOLNPGM(" " SHORT_BUILD_VERSION);
+  SERIAL_CHAR(' ');
+  SERIAL_ECHOLNPGM(SHORT_BUILD_VERSION);
+  SERIAL_EOL;
 
-  #ifdef STRING_DISTRIBUTION_DATE
-    #ifdef STRING_CONFIG_H_AUTHOR
-      SERIAL_ECHO_START;
-      SERIAL_ECHOPGM(MSG_CONFIGURATION_VER);
-      SERIAL_ECHOPGM(STRING_DISTRIBUTION_DATE);
-      SERIAL_ECHOPGM(MSG_AUTHOR);
-      SERIAL_ECHOLNPGM(STRING_CONFIG_H_AUTHOR);
-      SERIAL_ECHOPGM("Compiled: ");
-      SERIAL_ECHOLNPGM(__DATE__);
-    #endif // STRING_CONFIG_H_AUTHOR
-  #endif // STRING_DISTRIBUTION_DATE
+  #if defined(STRING_DISTRIBUTION_DATE) && defined(STRING_CONFIG_H_AUTHOR)
+    SERIAL_ECHO_START;
+    SERIAL_ECHOPGM(MSG_CONFIGURATION_VER);
+    SERIAL_ECHOPGM(STRING_DISTRIBUTION_DATE);
+    SERIAL_ECHOLNPGM(MSG_AUTHOR STRING_CONFIG_H_AUTHOR);
+    SERIAL_ECHOLNPGM("Compiled: " __DATE__);
+  #endif
 
   SERIAL_ECHO_START;
-  SERIAL_ECHOPGM(MSG_FREE_MEMORY);
-  SERIAL_ECHO(freeMemory());
-  SERIAL_ECHOPGM(MSG_PLANNER_BUFFER_BYTES);
-  SERIAL_ECHOLN((int)sizeof(block_t)*BLOCK_BUFFER_SIZE);
+  SERIAL_ECHOPAIR(MSG_FREE_MEMORY, freeMemory());
+  SERIAL_ECHOLNPAIR(MSG_PLANNER_BUFFER_BYTES, (int)sizeof(block_t)*BLOCK_BUFFER_SIZE);
 
   // Send "ok" after commands by default
   for (int8_t i = 0; i < BUFSIZE; i++) send_ok[i] = true;
diff --git a/Marlin/language.h b/Marlin/language.h
index e6c1b8efd5..6311e6c600 100644
--- a/Marlin/language.h
+++ b/Marlin/language.h
@@ -157,9 +157,9 @@
 #define MSG_ENDSTOP_OPEN                    "open"
 #define MSG_HOTEND_OFFSET                   "Hotend offsets:"
 #define MSG_DUPLICATION_MODE                "Duplication mode: "
-#define MSG_SOFT_ENDSTOPS                   "Soft endstops"
-#define MSG_SOFT_MIN                        "Min"
-#define MSG_SOFT_MAX                        "Max"
+#define MSG_SOFT_ENDSTOPS                   "Soft endstops: "
+#define MSG_SOFT_MIN                        "  Min: "
+#define MSG_SOFT_MAX                        "  Max: "
 
 #define MSG_SD_CANT_OPEN_SUBDIR             "Cannot open subdir "
 #define MSG_SD_INIT_FAIL                    "SD init fail"

From 61284cbd8c87795605ee2d9fff5fd9d703b2582c Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Mon, 12 Sep 2016 21:57:07 -0500
Subject: [PATCH 24/26] Various code style adjustments

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

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 630b6f188c..69d26a5391 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -2092,23 +2092,14 @@ static void clean_up_after_endstop_or_probe_move() {
     /**
      * All DELTA leveling in the Marlin uses NONLINEAR_BED_LEVELING
      */
-    static void extrapolate_one_point(int x, int y, int xdir, int ydir) {
-      if (bed_level_grid[x][y] != 0.0) {
-        return;  // Don't overwrite good values.
-      }
-      float a = 2 * bed_level_grid[x + xdir][y] - bed_level_grid[x + xdir * 2][y]; // Left to right.
-      float b = 2 * bed_level_grid[x][y + ydir] - bed_level_grid[x][y + ydir * 2]; // Front to back.
-      float c = 2 * bed_level_grid[x + xdir][y + ydir] - bed_level_grid[x + xdir * 2][y + ydir * 2]; // Diagonal.
-      float median = c;  // Median is robust (ignores outliers).
-      if (a < b) {
-        if (b < c) median = b;
-        if (c < a) median = a;
-      }
-      else {  // b <= a
-        if (c < b) median = b;
-        if (a < c) median = a;
-      }
-      bed_level_grid[x][y] = median;
+    static void extrapolate_one_point(uint8_t x, uint8_t y, int xdir, int ydir) {
+      if (bed_level_grid[x][y]) return;  // Don't overwrite good values.
+      float a = 2 * bed_level_grid[x + xdir][y] - bed_level_grid[x + xdir * 2][y], // Left to right.
+            b = 2 * bed_level_grid[x][y + ydir] - bed_level_grid[x][y + ydir * 2], // Front to back.
+            c = 2 * bed_level_grid[x + xdir][y + ydir] - bed_level_grid[x + xdir * 2][y + ydir * 2]; // Diagonal.
+      // Median is robust (ignores outliers).
+      bed_level_grid[x][y] = (a < b) ? ((b < c) ? b : (c < a) ? a : c)
+                                     : ((c < b) ? b : (a < c) ? a : c);
     }
 
     /**
@@ -2116,9 +2107,9 @@ static void clean_up_after_endstop_or_probe_move() {
      * using linear extrapolation, away from the center.
      */
     static void extrapolate_unprobed_bed_level() {
-      int half = (AUTO_BED_LEVELING_GRID_POINTS - 1) / 2;
-      for (int y = 0; y <= half; y++) {
-        for (int x = 0; x <= half; x++) {
+      uint8_t half = (AUTO_BED_LEVELING_GRID_POINTS - 1) / 2;
+      for (uint8_t y = 0; y <= half; y++) {
+        for (uint8_t x = 0; x <= half; x++) {
           if (x + y < 3) continue;
           extrapolate_one_point(half - x, half - y, x > 1 ? +1 : 0, y > 1 ? +1 : 0);
           extrapolate_one_point(half + x, half - y, x > 1 ? -1 : 0, y > 1 ? +1 : 0);
@@ -2132,8 +2123,8 @@ static void clean_up_after_endstop_or_probe_move() {
      * Print calibration results for plotting or manual frame adjustment.
      */
     static void print_bed_level() {
-      for (int y = 0; y < AUTO_BED_LEVELING_GRID_POINTS; y++) {
-        for (int x = 0; x < AUTO_BED_LEVELING_GRID_POINTS; x++) {
+      for (uint8_t y = 0; y < AUTO_BED_LEVELING_GRID_POINTS; y++) {
+        for (uint8_t x = 0; x < AUTO_BED_LEVELING_GRID_POINTS; x++) {
           SERIAL_PROTOCOL_F(bed_level_grid[x][y], 2);
           SERIAL_PROTOCOLCHAR(' ');
         }

From b1a60e8954c12174970d66fa42bf282b8464322e Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Tue, 13 Sep 2016 02:11:45 -0500
Subject: [PATCH 25/26] Reformat, rearrange, tweak, and document

- Add comments to several functions
- Add an option to use Fast SQRT for Delta IK
- Group related functions together
- Outdent some leveling-related functions
---
 Marlin/Marlin_main.cpp | 1094 ++++++++++++++++++++++------------------
 1 file changed, 593 insertions(+), 501 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 69d26a5391..7e894f51fd 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -2055,85 +2055,6 @@ static void clean_up_after_endstop_or_probe_move() {
 
 #if ENABLED(AUTO_BED_LEVELING_FEATURE)
 
-  #if ENABLED(AUTO_BED_LEVELING_LINEAR)
-
-    /**
-     * Get the stepper positions, apply the rotation matrix
-     * using the home XY and Z0 position as the fulcrum.
-     */
-    vector_3 untilted_stepper_position() {
-      get_cartesian_from_steppers();
-
-      vector_3 pos = vector_3(
-        cartes[X_AXIS] - X_TILT_FULCRUM,
-        cartes[Y_AXIS] - Y_TILT_FULCRUM,
-        cartes[Z_AXIS]
-      );
-
-      matrix_3x3 inverse = matrix_3x3::transpose(planner.bed_level_matrix);
-
-      //pos.debug("untilted_stepper_position offset");
-      //bed_level_matrix.debug("untilted_stepper_position");
-      //inverse.debug("in untilted_stepper_position");
-
-      pos.apply_rotation(inverse);
-
-      pos.x = LOGICAL_X_POSITION(pos.x + X_TILT_FULCRUM);
-      pos.y = LOGICAL_Y_POSITION(pos.y + Y_TILT_FULCRUM);
-      pos.z = LOGICAL_Z_POSITION(pos.z);
-
-      //pos.debug("after rotation and reorientation");
-
-      return pos;
-    }
-
-  #elif ENABLED(AUTO_BED_LEVELING_NONLINEAR)
-
-    /**
-     * All DELTA leveling in the Marlin uses NONLINEAR_BED_LEVELING
-     */
-    static void extrapolate_one_point(uint8_t x, uint8_t y, int xdir, int ydir) {
-      if (bed_level_grid[x][y]) return;  // Don't overwrite good values.
-      float a = 2 * bed_level_grid[x + xdir][y] - bed_level_grid[x + xdir * 2][y], // Left to right.
-            b = 2 * bed_level_grid[x][y + ydir] - bed_level_grid[x][y + ydir * 2], // Front to back.
-            c = 2 * bed_level_grid[x + xdir][y + ydir] - bed_level_grid[x + xdir * 2][y + ydir * 2]; // Diagonal.
-      // Median is robust (ignores outliers).
-      bed_level_grid[x][y] = (a < b) ? ((b < c) ? b : (c < a) ? a : c)
-                                     : ((c < b) ? b : (a < c) ? a : c);
-    }
-
-    /**
-     * Fill in the unprobed points (corners of circular print surface)
-     * using linear extrapolation, away from the center.
-     */
-    static void extrapolate_unprobed_bed_level() {
-      uint8_t half = (AUTO_BED_LEVELING_GRID_POINTS - 1) / 2;
-      for (uint8_t y = 0; y <= half; y++) {
-        for (uint8_t x = 0; x <= half; x++) {
-          if (x + y < 3) continue;
-          extrapolate_one_point(half - x, half - y, x > 1 ? +1 : 0, y > 1 ? +1 : 0);
-          extrapolate_one_point(half + x, half - y, x > 1 ? -1 : 0, y > 1 ? +1 : 0);
-          extrapolate_one_point(half - x, half + y, x > 1 ? +1 : 0, y > 1 ? -1 : 0);
-          extrapolate_one_point(half + x, half + y, x > 1 ? -1 : 0, y > 1 ? -1 : 0);
-        }
-      }
-    }
-
-    /**
-     * Print calibration results for plotting or manual frame adjustment.
-     */
-    static void print_bed_level() {
-      for (uint8_t y = 0; y < AUTO_BED_LEVELING_GRID_POINTS; y++) {
-        for (uint8_t x = 0; x < AUTO_BED_LEVELING_GRID_POINTS; x++) {
-          SERIAL_PROTOCOL_F(bed_level_grid[x][y], 2);
-          SERIAL_PROTOCOLCHAR(' ');
-        }
-        SERIAL_EOL;
-      }
-    }
-
-  #endif // DELTA
-
   /**
    * Reset calibration results to zero.
    */
@@ -2151,6 +2072,85 @@ static void clean_up_after_endstop_or_probe_move() {
 
 #endif // AUTO_BED_LEVELING_FEATURE
 
+#if ENABLED(AUTO_BED_LEVELING_LINEAR)
+
+  /**
+   * Get the stepper positions, apply the rotation matrix
+   * using the home XY and Z0 position as the fulcrum.
+   */
+  vector_3 untilted_stepper_position() {
+    get_cartesian_from_steppers();
+
+    vector_3 pos = vector_3(
+      cartes[X_AXIS] - X_TILT_FULCRUM,
+      cartes[Y_AXIS] - Y_TILT_FULCRUM,
+      cartes[Z_AXIS]
+    );
+
+    matrix_3x3 inverse = matrix_3x3::transpose(planner.bed_level_matrix);
+
+    //pos.debug("untilted_stepper_position offset");
+    //bed_level_matrix.debug("untilted_stepper_position");
+    //inverse.debug("in untilted_stepper_position");
+
+    pos.apply_rotation(inverse);
+
+    pos.x = LOGICAL_X_POSITION(pos.x + X_TILT_FULCRUM);
+    pos.y = LOGICAL_Y_POSITION(pos.y + Y_TILT_FULCRUM);
+    pos.z = LOGICAL_Z_POSITION(pos.z);
+
+    //pos.debug("after rotation and reorientation");
+
+    return pos;
+  }
+
+#elif ENABLED(AUTO_BED_LEVELING_NONLINEAR)
+
+  /**
+   * All DELTA leveling in the Marlin uses NONLINEAR_BED_LEVELING
+   */
+  static void extrapolate_one_point(uint8_t x, uint8_t y, int xdir, int ydir) {
+    if (bed_level_grid[x][y]) return;  // Don't overwrite good values.
+    float a = 2 * bed_level_grid[x + xdir][y] - bed_level_grid[x + xdir * 2][y], // Left to right.
+          b = 2 * bed_level_grid[x][y + ydir] - bed_level_grid[x][y + ydir * 2], // Front to back.
+          c = 2 * bed_level_grid[x + xdir][y + ydir] - bed_level_grid[x + xdir * 2][y + ydir * 2]; // Diagonal.
+    // Median is robust (ignores outliers).
+    bed_level_grid[x][y] = (a < b) ? ((b < c) ? b : (c < a) ? a : c)
+                                   : ((c < b) ? b : (a < c) ? a : c);
+  }
+
+  /**
+   * Fill in the unprobed points (corners of circular print surface)
+   * using linear extrapolation, away from the center.
+   */
+  static void extrapolate_unprobed_bed_level() {
+    uint8_t half = (AUTO_BED_LEVELING_GRID_POINTS - 1) / 2;
+    for (uint8_t y = 0; y <= half; y++) {
+      for (uint8_t x = 0; x <= half; x++) {
+        if (x + y < 3) continue;
+        extrapolate_one_point(half - x, half - y, x > 1 ? +1 : 0, y > 1 ? +1 : 0);
+        extrapolate_one_point(half + x, half - y, x > 1 ? -1 : 0, y > 1 ? +1 : 0);
+        extrapolate_one_point(half - x, half + y, x > 1 ? +1 : 0, y > 1 ? -1 : 0);
+        extrapolate_one_point(half + x, half + y, x > 1 ? -1 : 0, y > 1 ? -1 : 0);
+      }
+    }
+  }
+
+  /**
+   * Print calibration results for plotting or manual frame adjustment.
+   */
+  static void print_bed_level() {
+    for (uint8_t y = 0; y < AUTO_BED_LEVELING_GRID_POINTS; y++) {
+      for (uint8_t x = 0; x < AUTO_BED_LEVELING_GRID_POINTS; x++) {
+        SERIAL_PROTOCOL_F(bed_level_grid[x][y], 2);
+        SERIAL_PROTOCOLCHAR(' ');
+      }
+      SERIAL_EOL;
+    }
+  }
+
+#endif // AUTO_BED_LEVELING_NONLINEAR
+
 /**
  * Home an individual axis
  */
@@ -2441,6 +2441,10 @@ bool position_is_reachable(float target[XYZ]) {
   #endif
 }
 
+/**************************************************
+ ***************** GCode Handlers *****************
+ **************************************************/
+
 /**
  * G0, G1: Coordinated movement of X Y Z E axes
  */
@@ -2589,16 +2593,12 @@ inline void gcode_G4() {
   /**
    * G20: Set input mode to inches
    */
-  inline void gcode_G20() {
-    set_input_linear_units(LINEARUNIT_INCH);
-  }
+  inline void gcode_G20() { set_input_linear_units(LINEARUNIT_INCH); }
 
   /**
    * G21: Set input mode to millimeters
    */
-  inline void gcode_G21() {
-    set_input_linear_units(LINEARUNIT_MM);
-  }
+  inline void gcode_G21() { set_input_linear_units(LINEARUNIT_MM); }
 #endif
 
 #if ENABLED(NOZZLE_PARK_FEATURE)
@@ -3431,12 +3431,12 @@ inline void gcode_G28() {
       #endif // AUTO_BED_LEVELING_LINEAR
 
       int probePointCounter = 0;
-      bool zig = auto_bed_leveling_grid_points & 1; //always end at [RIGHT_PROBE_BED_POSITION, BACK_PROBE_BED_POSITION]
+      uint8_t zig = auto_bed_leveling_grid_points & 1; //always end at [RIGHT_PROBE_BED_POSITION, BACK_PROBE_BED_POSITION]
 
-      for (int yCount = 0; yCount < auto_bed_leveling_grid_points; yCount++) {
+      for (uint8_t yCount = 0; yCount < auto_bed_leveling_grid_points; yCount++) {
         float yBase = front_probe_bed_position + yGridSpacing * yCount,
               yProbe = floor(yBase + (yBase < 0 ? 0 : 0.5));
-        int xStart, xStop, xInc;
+        int8_t xStart, xStop, xInc;
 
         if (zig) {
           xStart = 0;
@@ -3451,7 +3451,7 @@ inline void gcode_G28() {
 
         zig = !zig;
 
-        for (int xCount = xStart; xCount != xStop; xCount += xInc) {
+        for (uint8_t xCount = xStart; xCount != xStop; xCount += xInc) {
           float xBase = left_probe_bed_position + xGridSpacing * xCount,
                 xProbe = floor(xBase + (xBase < 0 ? 0 : 0.5));
 
@@ -3514,7 +3514,7 @@ inline void gcode_G28() {
         planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
       }
 
-    #endif // !AUTO_BED_LEVELING_GRID
+    #endif // AUTO_BED_LEVELING_3POINT
 
     // Raise to _Z_PROBE_DEPLOY_HEIGHT. Stow the probe.
     if (STOW_PROBE()) return;
@@ -3527,74 +3527,96 @@ inline void gcode_G28() {
     #endif
 
     // Calculate leveling, print reports, correct the position
-    #if ENABLED(AUTO_BED_LEVELING_GRID)
-      #if ENABLED(AUTO_BED_LEVELING_NONLINEAR)
+    #if ENABLED(AUTO_BED_LEVELING_NONLINEAR)
 
-        if (!dryrun) extrapolate_unprobed_bed_level();
-        print_bed_level();
+      if (!dryrun) extrapolate_unprobed_bed_level();
+      print_bed_level();
 
-      #elif ENABLED(AUTO_BED_LEVELING_LINEAR)
+    #elif ENABLED(AUTO_BED_LEVELING_LINEAR)
 
-        // solve lsq problem
-        double plane_equation_coefficients[3];
-        qr_solve(plane_equation_coefficients, abl2, 3, eqnAMatrix, eqnBVector);
+      // solve lsq problem
+      double plane_equation_coefficients[3];
+      qr_solve(plane_equation_coefficients, abl2, 3, eqnAMatrix, eqnBVector);
 
-        mean /= abl2;
+      mean /= abl2;
 
-        if (verbose_level) {
-          SERIAL_PROTOCOLPGM("Eqn coefficients: a: ");
-          SERIAL_PROTOCOL_F(plane_equation_coefficients[0], 8);
-          SERIAL_PROTOCOLPGM(" b: ");
-          SERIAL_PROTOCOL_F(plane_equation_coefficients[1], 8);
-          SERIAL_PROTOCOLPGM(" d: ");
-          SERIAL_PROTOCOL_F(plane_equation_coefficients[2], 8);
+      if (verbose_level) {
+        SERIAL_PROTOCOLPGM("Eqn coefficients: a: ");
+        SERIAL_PROTOCOL_F(plane_equation_coefficients[0], 8);
+        SERIAL_PROTOCOLPGM(" b: ");
+        SERIAL_PROTOCOL_F(plane_equation_coefficients[1], 8);
+        SERIAL_PROTOCOLPGM(" d: ");
+        SERIAL_PROTOCOL_F(plane_equation_coefficients[2], 8);
+        SERIAL_EOL;
+        if (verbose_level > 2) {
+          SERIAL_PROTOCOLPGM("Mean of sampled points: ");
+          SERIAL_PROTOCOL_F(mean, 8);
           SERIAL_EOL;
-          if (verbose_level > 2) {
-            SERIAL_PROTOCOLPGM("Mean of sampled points: ");
-            SERIAL_PROTOCOL_F(mean, 8);
-            SERIAL_EOL;
-          }
         }
+      }
 
-        // Create the matrix but don't correct the position yet
-        if (!dryrun) {
-          planner.bed_level_matrix = matrix_3x3::create_look_at(
-            vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1)
-          );
-        }
+      // Create the matrix but don't correct the position yet
+      if (!dryrun) {
+        planner.bed_level_matrix = matrix_3x3::create_look_at(
+          vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1)
+        );
+      }
 
-        // Show the Topography map if enabled
-        if (do_topography_map) {
+      // Show the Topography map if enabled
+      if (do_topography_map) {
 
-          SERIAL_PROTOCOLLNPGM("\nBed Height Topography:\n"
-                                 "   +--- BACK --+\n"
-                                 "   |           |\n"
-                                 " L |    (+)    | R\n"
-                                 " E |           | I\n"
-                                 " F | (-) N (+) | G\n"
-                                 " T |           | H\n"
-                                 "   |    (-)    | T\n"
-                                 "   |           |\n"
-                                 "   O-- FRONT --+\n"
-                                 " (0,0)");
+        SERIAL_PROTOCOLLNPGM("\nBed Height Topography:\n"
+                               "   +--- BACK --+\n"
+                               "   |           |\n"
+                               " L |    (+)    | R\n"
+                               " E |           | I\n"
+                               " F | (-) N (+) | G\n"
+                               " T |           | H\n"
+                               "   |    (-)    | T\n"
+                               "   |           |\n"
+                               "   O-- FRONT --+\n"
+                               " (0,0)");
 
-          float min_diff = 999;
+        float min_diff = 999;
+
+        for (int8_t yy = auto_bed_leveling_grid_points - 1; yy >= 0; yy--) {
+          for (uint8_t xx = 0; xx < auto_bed_leveling_grid_points; xx++) {
+            int ind = indexIntoAB[xx][yy];
+            float diff = eqnBVector[ind] - mean,
+                  x_tmp = eqnAMatrix[ind + 0 * abl2],
+                  y_tmp = eqnAMatrix[ind + 1 * abl2],
+                  z_tmp = 0;
+
+            apply_rotation_xyz(planner.bed_level_matrix, x_tmp, y_tmp, z_tmp);
+
+            NOMORE(min_diff, eqnBVector[ind] - z_tmp);
+
+            if (diff >= 0.0)
+              SERIAL_PROTOCOLPGM(" +");   // Include + for column alignment
+            else
+              SERIAL_PROTOCOLCHAR(' ');
+            SERIAL_PROTOCOL_F(diff, 5);
+          } // xx
+          SERIAL_EOL;
+        } // yy
+        SERIAL_EOL;
+
+        if (verbose_level > 3) {
+          SERIAL_PROTOCOLLNPGM("\nCorrected Bed Height vs. Bed Topology:");
 
           for (int yy = auto_bed_leveling_grid_points - 1; yy >= 0; yy--) {
             for (int xx = 0; xx < auto_bed_leveling_grid_points; xx++) {
               int ind = indexIntoAB[xx][yy];
-              float diff = eqnBVector[ind] - mean;
-
               float x_tmp = eqnAMatrix[ind + 0 * abl2],
                     y_tmp = eqnAMatrix[ind + 1 * abl2],
                     z_tmp = 0;
 
               apply_rotation_xyz(planner.bed_level_matrix, x_tmp, y_tmp, z_tmp);
 
-              NOMORE(min_diff, eqnBVector[ind] - z_tmp);
-
+              float diff = eqnBVector[ind] - z_tmp - min_diff;
               if (diff >= 0.0)
-                SERIAL_PROTOCOLPGM(" +");   // Include + for column alignment
+                SERIAL_PROTOCOLPGM(" +");
+              // Include + for column alignment
               else
                 SERIAL_PROTOCOLCHAR(' ');
               SERIAL_PROTOCOL_F(diff, 5);
@@ -3602,38 +3624,8 @@ inline void gcode_G28() {
             SERIAL_EOL;
           } // yy
           SERIAL_EOL;
-
-          if (verbose_level > 3) {
-            SERIAL_PROTOCOLLNPGM("\nCorrected Bed Height vs. Bed Topology:");
-
-            for (int yy = auto_bed_leveling_grid_points - 1; yy >= 0; yy--) {
-              for (int xx = 0; xx < auto_bed_leveling_grid_points; xx++) {
-                int ind = indexIntoAB[xx][yy];
-                float x_tmp = eqnAMatrix[ind + 0 * abl2],
-                      y_tmp = eqnAMatrix[ind + 1 * abl2],
-                      z_tmp = 0;
-
-                apply_rotation_xyz(planner.bed_level_matrix, x_tmp, y_tmp, z_tmp);
-
-                float diff = eqnBVector[ind] - z_tmp - min_diff;
-                if (diff >= 0.0)
-                  SERIAL_PROTOCOLPGM(" +");
-                // Include + for column alignment
-                else
-                  SERIAL_PROTOCOLCHAR(' ');
-                SERIAL_PROTOCOL_F(diff, 5);
-              } // xx
-              SERIAL_EOL;
-            } // yy
-            SERIAL_EOL;
-          }
-        } //do_topography_map
-
-      #endif // AUTO_BED_LEVELING_LINEAR
-
-    #endif // AUTO_BED_LEVELING_GRID
-
-    #if ENABLED(AUTO_BED_LEVELING_LINEAR)
+        }
+      } //do_topography_map
 
       if (verbose_level > 0)
         planner.bed_level_matrix.debug("\n\nBed Level Correction Matrix:");
@@ -3682,7 +3674,7 @@ inline void gcode_G28() {
         #endif
       }
 
-    #endif // !DELTA
+    #endif // AUTO_BED_LEVELING_LINEAR
 
     #ifdef Z_PROBE_END_SCRIPT
       #if ENABLED(DEBUG_LEVELING_FEATURE)
@@ -3703,7 +3695,7 @@ inline void gcode_G28() {
     KEEPALIVE_STATE(IN_HANDLER);
   }
 
-#endif //AUTO_BED_LEVELING_FEATURE
+#endif // AUTO_BED_LEVELING_FEATURE
 
 #if HAS_BED_PROBE
 
@@ -3886,23 +3878,17 @@ inline void gcode_M17() {
   /**
    * M21: Init SD Card
    */
-  inline void gcode_M21() {
-    card.initsd();
-  }
+  inline void gcode_M21() { card.initsd(); }
 
   /**
    * M22: Release SD Card
    */
-  inline void gcode_M22() {
-    card.release();
-  }
+  inline void gcode_M22() { card.release(); }
 
   /**
    * M23: Open a file
    */
-  inline void gcode_M23() {
-    card.openFile(current_command_args, true);
-  }
+  inline void gcode_M23() { card.openFile(current_command_args, true); }
 
   /**
    * M24: Start SD Print
@@ -3915,9 +3901,7 @@ inline void gcode_M17() {
   /**
    * M25: Pause SD Print
    */
-  inline void gcode_M25() {
-    card.pauseSDPrint();
-  }
+  inline void gcode_M25() { card.pauseSDPrint(); }
 
   /**
    * M26: Set SD Card file index
@@ -3930,16 +3914,12 @@ inline void gcode_M17() {
   /**
    * M27: Get SD Card status
    */
-  inline void gcode_M27() {
-    card.getStatus();
-  }
+  inline void gcode_M27() { card.getStatus(); }
 
   /**
    * M28: Start SD Write
    */
-  inline void gcode_M28() {
-    card.openFile(current_command_args, false);
-  }
+  inline void gcode_M28() { card.openFile(current_command_args, false); }
 
   /**
    * M29: Stop SD Write
@@ -3959,7 +3939,7 @@ inline void gcode_M17() {
     }
   }
 
-#endif //SDSUPPORT
+#endif // SDSUPPORT
 
 /**
  * M31: Get the time since the start of SD Print (or last M109)
@@ -4318,7 +4298,8 @@ inline void gcode_M77() { print_job_timer.stop(); }
     // "M78 S78" will reset the statistics
     if (code_seen('S') && code_value_int() == 78)
       print_job_timer.initStats();
-    else print_job_timer.showStats();
+    else
+      print_job_timer.showStats();
   }
 #endif
 
@@ -4885,7 +4866,7 @@ inline void gcode_M140() {
     }
   }
 
-#endif
+#endif // ULTIPANEL
 
 #if ENABLED(TEMPERATURE_UNITS_SUPPORT)
   /**
@@ -4959,7 +4940,6 @@ inline void gcode_M81() {
   #endif
 }
 
-
 /**
  * M82: Set E codes absolute (default)
  */
@@ -5485,7 +5465,6 @@ inline void gcode_M221() {
 inline void gcode_M226() {
   if (code_seen('P')) {
     int pin_number = code_value_int();
-
     int pin_state = code_seen('S') ? code_value_int() : -1; // required pin state - default is inverted
 
     if (pin_state >= -1 && pin_state <= 1) {
@@ -6536,6 +6515,10 @@ inline void invalid_extruder_error(const uint8_t &e) {
   SERIAL_ECHOLN(MSG_INVALID_EXTRUDER);
 }
 
+/**
+ * Perform a tool-change, which may result in moving the
+ * previous tool out of the way and the new tool into place.
+ */
 void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool no_move/*=false*/) {
   #if ENABLED(MIXING_EXTRUDER) && MIXING_VIRTUAL_TOOLS > 1
 
@@ -7562,6 +7545,10 @@ ExitUnknownCommand:
   ok_to_send();
 }
 
+/**
+ * Send a "Resend: nnn" message to the host to
+ * indicate that a command needs to be re-sent.
+ */
 void FlushSerialRequestResend() {
   //char command_queue[cmd_queue_index_r][100]="Resend:";
   MYSERIAL.flush();
@@ -7570,6 +7557,15 @@ void FlushSerialRequestResend() {
   ok_to_send();
 }
 
+/**
+ * Send an "ok" message to the host, indicating
+ * that a command was successfully processed.
+ *
+ * If ADVANCED_OK is enabled also include:
+ *   N<int>  Line number of the command, if any
+ *   P<int>  Planner space remaining
+ *   B<int>  Block queue space remaining
+ */
 void ok_to_send() {
   refresh_cmd_timeout();
   if (!send_ok[cmd_queue_index_r]) return;
@@ -7590,6 +7586,9 @@ void ok_to_send() {
 
 #if ENABLED(min_software_endstops) || ENABLED(max_software_endstops)
 
+  /**
+   * Constrain the given coordinates to the software endstops.
+   */
   void clamp_to_software_endstops(float target[XYZ]) {
     #if ENABLED(min_software_endstops)
       NOLESS(target[X_AXIS], soft_endstop_min[X_AXIS]);
@@ -7607,6 +7606,10 @@ void ok_to_send() {
 
 #if ENABLED(DELTA)
 
+  /**
+   * Recalculate factors used for delta kinematics whenever
+   * settings have been changed (e.g., by M665).
+   */
   void recalc_delta_settings(float radius, float diagonal_rod) {
     delta_tower1_x = -SIN_60 * (radius + DELTA_RADIUS_TRIM_TOWER_1);  // front left tower
     delta_tower1_y = -COS_60 * (radius + DELTA_RADIUS_TRIM_TOWER_1);
@@ -7619,37 +7622,85 @@ void ok_to_send() {
     delta_diagonal_rod_2_tower_3 = sq(diagonal_rod + delta_diagonal_rod_trim_tower_3);
   }
 
-  void inverse_kinematics(const float in_cartesian[XYZ]) {
+  #if ENABLED(DELTA_FAST_SQRT)
+    /**
+     * Fast inverse sqrt from Quake III Arena
+     * See: https://en.wikipedia.org/wiki/Fast_inverse_square_root
+     */
+    float Q_rsqrt(float number) {
+      long i;
+      float x2, y;
+      const float threehalfs = 1.5f;
+      x2 = number * 0.5f;
+      y  = number;
+      i  = * ( long * ) &y;                       // evil floating point bit level hacking
+      i  = 0x5f3759df - ( i >> 1 );               // what the f***?
+      y  = * ( float * ) &i;
+      y  = y * ( threehalfs - ( x2 * y * y ) );   // 1st iteration
+      // y  = y * ( threehalfs - ( x2 * y * y ) );   // 2nd iteration, this can be removed
+      return y;
+    }
+
+    #define _SQRT(n) (1.0f / Q_rsqrt(n))
+
+  #else
+
+    #define _SQRT(n) sqrt(n)
+
+  #endif
+
+  /**
+   * Delta Inverse Kinematics
+   *
+   * Calculate the tower positions for a given logical
+   * position, storing the result in the delta[] array.
+   *
+   * This is an expensive calculation, requiring 3 square
+   * roots per segmented linear move, and strains the limits
+   * of a Mega2560 with a Graphical Display.
+   *
+   * Suggested optimizations include:
+   *
+   * - Disable the home_offset (M206) and/or position_shift (G92)
+   *   features to remove up to 12 float additions.
+   *
+   * - Use a fast-inverse-sqrt function and add the reciprocal.
+   *   (see above)
+   */
+  void inverse_kinematics(const float logical[XYZ]) {
 
     const float cartesian[XYZ] = {
-      RAW_X_POSITION(in_cartesian[X_AXIS]),
-      RAW_Y_POSITION(in_cartesian[Y_AXIS]),
-      RAW_Z_POSITION(in_cartesian[Z_AXIS])
+      RAW_X_POSITION(logical[X_AXIS]),
+      RAW_Y_POSITION(logical[Y_AXIS]),
+      RAW_Z_POSITION(logical[Z_AXIS])
     };
 
-    delta[A_AXIS] = sqrt(delta_diagonal_rod_2_tower_1
-                          - sq(delta_tower1_x - cartesian[X_AXIS])
-                          - sq(delta_tower1_y - cartesian[Y_AXIS])
-                         ) + cartesian[Z_AXIS];
-    delta[B_AXIS] = sqrt(delta_diagonal_rod_2_tower_2
-                          - sq(delta_tower2_x - cartesian[X_AXIS])
-                          - sq(delta_tower2_y - cartesian[Y_AXIS])
-                         ) + cartesian[Z_AXIS];
-    delta[C_AXIS] = sqrt(delta_diagonal_rod_2_tower_3
-                          - sq(delta_tower3_x - cartesian[X_AXIS])
-                          - sq(delta_tower3_y - cartesian[Y_AXIS])
-                         ) + cartesian[Z_AXIS];
-    /**
-    SERIAL_ECHOPAIR("cartesian x=", cartesian[X_AXIS]);
-    SERIAL_ECHOPAIR(" y=", cartesian[Y_AXIS]);
-    SERIAL_ECHOLNPAIR(" z=", cartesian[Z_AXIS]);
+    // Macro to obtain the Z position of an individual tower
+    #define DELTA_Z(T) cartesian[Z_AXIS] + _SQRT( \
+      delta_diagonal_rod_2_tower_##T - HYPOT2(    \
+          delta_tower##T##_x - cartesian[X_AXIS], \
+          delta_tower##T##_y - cartesian[Y_AXIS]  \
+        )                                         \
+      )
 
-    SERIAL_ECHOPAIR("delta a=", delta[A_AXIS]);
-    SERIAL_ECHOPAIR(" b=", delta[B_AXIS]);
-    SERIAL_ECHOLNPAIR(" c=", delta[C_AXIS]);
-    */
+    delta[A_AXIS] = DELTA_Z(1);
+    delta[B_AXIS] = DELTA_Z(2);
+    delta[C_AXIS] = DELTA_Z(3);
+
+    /*
+      SERIAL_ECHOPAIR("cartesian X:", cartesian[X_AXIS]);
+      SERIAL_ECHOPAIR(" Y:", cartesian[Y_AXIS]);
+      SERIAL_ECHOLNPAIR(" Z:", cartesian[Z_AXIS]);
+      SERIAL_ECHOPAIR("delta A:", delta[A_AXIS]);
+      SERIAL_ECHOPAIR(" B:", delta[B_AXIS]);
+      SERIAL_ECHOLNPAIR(" C:", delta[C_AXIS]);
+    //*/
   }
 
+  /**
+   * Calculate the highest Z position where the
+   * effector has the full range of XY motion.
+   */
   float delta_safe_distance_from_top() {
     float cartesian[XYZ] = {
       LOGICAL_X_POSITION(0),
@@ -7663,73 +7714,80 @@ void ok_to_send() {
     return abs(distance - delta[A_AXIS]);
   }
 
+  /**
+   * Delta Forward Kinematics
+   *
+   * See the Wikipedia article "Trilateration"
+   * https://en.wikipedia.org/wiki/Trilateration
+   *
+   * Establish a new coordinate system in the plane of the
+   * three carriage points. This system has its origin at
+   * tower1, with tower2 on the X axis. Tower3 is in the X-Y
+   * plane with a Z component of zero.
+   * We will define unit vectors in this coordinate system
+   * in our original coordinate system. Then when we calculate
+   * the Xnew, Ynew and Znew values, we can translate back into
+   * the original system by moving along those unit vectors
+   * by the corresponding values.
+   *
+   * Variable names matched to Marlin, c-version, and avoid the
+   * use of any vector library.
+   *
+   * by Andreas Hardtung 2016-06-07
+   * based on a Java function from "Delta Robot Kinematics V3"
+   * by Steve Graves
+   *
+   * The result is stored in the cartes[] array.
+   */
   void forward_kinematics_DELTA(float z1, float z2, float z3) {
-    //As discussed in Wikipedia "Trilateration"
-    //we are establishing a new coordinate
-    //system in the plane of the three carriage points.
-    //This system will have the origin at tower1 and
-    //tower2 is on the x axis. tower3 is in the X-Y
-    //plane with a Z component of zero. We will define unit
-    //vectors in this coordinate system in our original
-    //coordinate system. Then when we calculate the
-    //Xnew, Ynew and Znew values, we can translate back into
-    //the original system by moving along those unit vectors
-    //by the corresponding values.
-    // https://en.wikipedia.org/wiki/Trilateration
-
-    // Variable names matched to Marlin, c-version
-    // and avoiding a vector library
-    // by Andreas Hardtung 2016-06-7
-    // based on a Java function from
-    // "Delta Robot Kinematics by Steve Graves" V3
-
-    // Result is in cartes[].
-
-    //Create a vector in old coordinates along x axis of new coordinate
+    // Create a vector in old coordinates along x axis of new coordinate
     float p12[3] = { delta_tower2_x - delta_tower1_x, delta_tower2_y - delta_tower1_y, z2 - z1 };
 
-    //Get the Magnitude of vector.
-    float d = sqrt( p12[0]*p12[0] + p12[1]*p12[1] + p12[2]*p12[2] );
+    // Get the Magnitude of vector.
+    float d = sqrt( sq(p12[0]) + sq(p12[1]) + sq(p12[2]) );
 
-    //Create unit vector by dividing by magnitude.
-    float ex[3] = { p12[0]/d, p12[1]/d, p12[2]/d };
+    // Create unit vector by dividing by magnitude.
+    float ex[3] = { p12[0] / d, p12[1] / d, p12[2] / d };
 
-    //Now find vector from the origin of the new system to the third point.
+    // Get the vector from the origin of the new system to the third point.
     float p13[3] = { delta_tower3_x - delta_tower1_x, delta_tower3_y - delta_tower1_y, z3 - z1 };
 
-    //Now use dot product to find the component of this vector on the X axis.
-    float i = ex[0]*p13[0] + ex[1]*p13[1] + ex[2]*p13[2];
+    // Use the dot product to find the component of this vector on the X axis.
+    float i = ex[0] * p13[0] + ex[1] * p13[1] + ex[2] * p13[2];
 
-    //Now create a vector along the x axis that represents the x component of p13.
-    float iex[3] = { ex[0]*i,  ex[1]*i,  ex[2]*i  };
+    // Create a vector along the x axis that represents the x component of p13.
+    float iex[3] = { ex[0] * i, ex[1] * i, ex[2] * i };
 
-    //Now subtract the X component away from the original vector leaving only the Y component. We use the
-    //variable that will be the unit vector after we scale it.
-    float ey[3] = { p13[0] - iex[0], p13[1] - iex[1], p13[2] - iex[2]};
+    // Subtract the X component from the original vector leaving only Y. We use the
+    // variable that will be the unit vector after we scale it.
+    float ey[3] = { p13[0] - iex[0], p13[1] - iex[1], p13[2] - iex[2] };
 
-    //The magnitude of Y component
-    float j = sqrt(sq(ey[0]) + sq(ey[1]) + sq(ey[2]));
+    // The magnitude of Y component
+    float j = sqrt( sq(ey[0]) + sq(ey[1]) + sq(ey[2]) );
 
-    //Now make vector a unit vector
+    // Convert to a unit vector
     ey[0] /= j; ey[1] /= j;  ey[2] /= j;
 
-    //The cross product of the unit x and y is the unit z
-    //float[] ez = vectorCrossProd(ex, ey);
-    float ez[3] = { ex[1]*ey[2] - ex[2]*ey[1], ex[2]*ey[0] - ex[0]*ey[2], ex[0]*ey[1] - ex[1]*ey[0] };
+    // The cross product of the unit x and y is the unit z
+    // float[] ez = vectorCrossProd(ex, ey);
+    float ez[3] = {
+      ex[1] * ey[2] - ex[2] * ey[1],
+      ex[2] * ey[0] - ex[0] * ey[2],
+      ex[0] * ey[1] - ex[1] * ey[0]
+    };
 
-    //Now we have the d, i and j values defined in Wikipedia.
-    //We can plug them into the equations defined in
-    //Wikipedia for Xnew, Ynew and Znew
-    float Xnew = (delta_diagonal_rod_2_tower_1 - delta_diagonal_rod_2_tower_2 + d*d)/(d*2);
-    float Ynew = ((delta_diagonal_rod_2_tower_1 - delta_diagonal_rod_2_tower_3 + i*i + j*j)/2 - i*Xnew) /j;
-    float Znew = sqrt(delta_diagonal_rod_2_tower_1 - Xnew*Xnew - Ynew*Ynew);
+    // We now have the d, i and j values defined in Wikipedia.
+    // Plug them into the equations defined in Wikipedia for Xnew, Ynew and Znew
+    float Xnew = (delta_diagonal_rod_2_tower_1 - delta_diagonal_rod_2_tower_2 + sq(d)) / (d * 2),
+          Ynew = ((delta_diagonal_rod_2_tower_1 - delta_diagonal_rod_2_tower_3 + HYPOT2(i, j)) / 2 - i * Xnew) / j,
+          Znew = sqrt(delta_diagonal_rod_2_tower_1 - HYPOT2(Xnew, Ynew));
 
-    //Now we can start from the origin in the old coords and
-    //add vectors in the old coords that represent the
-    //Xnew, Ynew and Znew to find the point in the old system
-    cartes[X_AXIS] = delta_tower1_x + ex[0]*Xnew + ey[0]*Ynew - ez[0]*Znew;
-    cartes[Y_AXIS] = delta_tower1_y + ex[1]*Xnew + ey[1]*Ynew - ez[1]*Znew;
-    cartes[Z_AXIS] = z1             + ex[2]*Xnew + ey[2]*Ynew - ez[2]*Znew;
+    // Start from the origin of the old coordinates and add vectors in the
+    // old coords that represent the Xnew, Ynew and Znew to find the point
+    // in the old system.
+    cartes[X_AXIS] = delta_tower1_x + ex[0] * Xnew + ey[0] * Ynew - ez[0] * Znew;
+    cartes[Y_AXIS] = delta_tower1_y + ex[1] * Xnew + ey[1] * Ynew - ez[1] * Znew;
+    cartes[Z_AXIS] =             z1 + ex[2] * Xnew + ey[2] * Ynew - ez[2] * Znew;
   };
 
   void forward_kinematics_DELTA(float point[ABC]) {
@@ -7780,6 +7838,42 @@ void ok_to_send() {
 
 #endif // DELTA
 
+/**
+ * Get the stepper positions in the cartes[] array.
+ * Forward kinematics are applied for DELTA and SCARA.
+ *
+ * The result is in the current coordinate space with
+ * leveling applied. The coordinates need to be run through
+ * unapply_leveling to obtain the "ideal" coordinates
+ * suitable for current_position, etc.
+ */
+void get_cartesian_from_steppers() {
+  #if ENABLED(DELTA)
+    forward_kinematics_DELTA(
+      stepper.get_axis_position_mm(A_AXIS),
+      stepper.get_axis_position_mm(B_AXIS),
+      stepper.get_axis_position_mm(C_AXIS)
+    );
+  #elif IS_SCARA
+    forward_kinematics_SCARA(
+      stepper.get_axis_position_degrees(A_AXIS),
+      stepper.get_axis_position_degrees(B_AXIS)
+    );
+    cartes[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
+  #else
+    cartes[X_AXIS] = stepper.get_axis_position_mm(X_AXIS);
+    cartes[Y_AXIS] = stepper.get_axis_position_mm(Y_AXIS);
+    cartes[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
+  #endif
+}
+
+/**
+ * Set the current_position for an axis based on
+ * the stepper positions, removing any leveling that
+ * may have been applied.
+ *
+ * << INCOMPLETE! Still needs to unapply leveling! >>
+ */
 void set_current_from_steppers_for_axis(AxisEnum axis) {
   #if ENABLED(AUTO_BED_LEVELING_LINEAR)
     vector_3 pos = untilted_stepper_position();
@@ -7794,65 +7888,75 @@ void set_current_from_steppers_for_axis(AxisEnum axis) {
 
 #if ENABLED(MESH_BED_LEVELING)
 
-// This function is used to split lines on mesh borders so each segment is only part of one mesh area
-void mesh_line_to_destination(float fr_mm_s, uint8_t x_splits = 0xff, uint8_t y_splits = 0xff) {
-  int cx1 = mbl.cell_index_x(RAW_CURRENT_POSITION(X_AXIS)),
-      cy1 = mbl.cell_index_y(RAW_CURRENT_POSITION(Y_AXIS)),
-      cx2 = mbl.cell_index_x(RAW_X_POSITION(destination[X_AXIS])),
-      cy2 = mbl.cell_index_y(RAW_Y_POSITION(destination[Y_AXIS]));
-  NOMORE(cx1, MESH_NUM_X_POINTS - 2);
-  NOMORE(cy1, MESH_NUM_Y_POINTS - 2);
-  NOMORE(cx2, MESH_NUM_X_POINTS - 2);
-  NOMORE(cy2, MESH_NUM_Y_POINTS - 2);
+  /**
+   * Prepare a mesh-leveled linear move in a Cartesian setup,
+   * splitting the move where it crosses mesh borders.
+   */
+  void mesh_line_to_destination(float fr_mm_s, uint8_t x_splits = 0xff, uint8_t y_splits = 0xff) {
+    int cx1 = mbl.cell_index_x(RAW_CURRENT_POSITION(X_AXIS)),
+        cy1 = mbl.cell_index_y(RAW_CURRENT_POSITION(Y_AXIS)),
+        cx2 = mbl.cell_index_x(RAW_X_POSITION(destination[X_AXIS])),
+        cy2 = mbl.cell_index_y(RAW_Y_POSITION(destination[Y_AXIS]));
+    NOMORE(cx1, MESH_NUM_X_POINTS - 2);
+    NOMORE(cy1, MESH_NUM_Y_POINTS - 2);
+    NOMORE(cx2, MESH_NUM_X_POINTS - 2);
+    NOMORE(cy2, MESH_NUM_Y_POINTS - 2);
 
-  if (cx1 == cx2 && cy1 == cy2) {
-    // Start and end on same mesh square
-    line_to_destination(fr_mm_s);
-    set_current_to_destination();
-    return;
+    if (cx1 == cx2 && cy1 == cy2) {
+      // Start and end on same mesh square
+      line_to_destination(fr_mm_s);
+      set_current_to_destination();
+      return;
+    }
+
+    #define MBL_SEGMENT_END(A) (current_position[A ##_AXIS] + (destination[A ##_AXIS] - current_position[A ##_AXIS]) * normalized_dist)
+
+    float normalized_dist, end[NUM_AXIS];
+
+    // Split at the left/front border of the right/top square
+    int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2);
+    if (cx2 != cx1 && TEST(x_splits, gcx)) {
+      memcpy(end, destination, sizeof(end));
+      destination[X_AXIS] = LOGICAL_X_POSITION(mbl.get_probe_x(gcx));
+      normalized_dist = (destination[X_AXIS] - current_position[X_AXIS]) / (end[X_AXIS] - current_position[X_AXIS]);
+      destination[Y_AXIS] = MBL_SEGMENT_END(Y);
+      CBI(x_splits, gcx);
+    }
+    else if (cy2 != cy1 && TEST(y_splits, gcy)) {
+      memcpy(end, destination, sizeof(end));
+      destination[Y_AXIS] = LOGICAL_Y_POSITION(mbl.get_probe_y(gcy));
+      normalized_dist = (destination[Y_AXIS] - current_position[Y_AXIS]) / (end[Y_AXIS] - current_position[Y_AXIS]);
+      destination[X_AXIS] = MBL_SEGMENT_END(X);
+      CBI(y_splits, gcy);
+    }
+    else {
+      // Already split on a border
+      line_to_destination(fr_mm_s);
+      set_current_to_destination();
+      return;
+    }
+
+    destination[Z_AXIS] = MBL_SEGMENT_END(Z);
+    destination[E_AXIS] = MBL_SEGMENT_END(E);
+
+    // Do the split and look for more borders
+    mesh_line_to_destination(fr_mm_s, x_splits, y_splits);
+
+    // Restore destination from stack
+    memcpy(destination, end, sizeof(end));
+    mesh_line_to_destination(fr_mm_s, x_splits, y_splits);
   }
 
-  #define MBL_SEGMENT_END(A) (current_position[A ##_AXIS] + (destination[A ##_AXIS] - current_position[A ##_AXIS]) * normalized_dist)
-
-  float normalized_dist, end[NUM_AXIS];
-
-  // Split at the left/front border of the right/top square
-  int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2);
-  if (cx2 != cx1 && TEST(x_splits, gcx)) {
-    memcpy(end, destination, sizeof(end));
-    destination[X_AXIS] = LOGICAL_X_POSITION(mbl.get_probe_x(gcx));
-    normalized_dist = (destination[X_AXIS] - current_position[X_AXIS]) / (end[X_AXIS] - current_position[X_AXIS]);
-    destination[Y_AXIS] = MBL_SEGMENT_END(Y);
-    CBI(x_splits, gcx);
-  }
-  else if (cy2 != cy1 && TEST(y_splits, gcy)) {
-    memcpy(end, destination, sizeof(end));
-    destination[Y_AXIS] = LOGICAL_Y_POSITION(mbl.get_probe_y(gcy));
-    normalized_dist = (destination[Y_AXIS] - current_position[Y_AXIS]) / (end[Y_AXIS] - current_position[Y_AXIS]);
-    destination[X_AXIS] = MBL_SEGMENT_END(X);
-    CBI(y_splits, gcy);
-  }
-  else {
-    // Already split on a border
-    line_to_destination(fr_mm_s);
-    set_current_to_destination();
-    return;
-  }
-
-  destination[Z_AXIS] = MBL_SEGMENT_END(Z);
-  destination[E_AXIS] = MBL_SEGMENT_END(E);
-
-  // Do the split and look for more borders
-  mesh_line_to_destination(fr_mm_s, x_splits, y_splits);
-
-  // Restore destination from stack
-  memcpy(destination, end, sizeof(end));
-  mesh_line_to_destination(fr_mm_s, x_splits, y_splits);
-}
 #endif  // MESH_BED_LEVELING
 
 #if IS_KINEMATIC
 
+  /**
+   * Prepare a linear move in a DELTA or SCARA setup.
+   *
+   * This calls planner.buffer_line several times, adding
+   * small incremental moves for DELTA or SCARA.
+   */
   inline bool prepare_kinematic_move_to(float target[NUM_AXIS]) {
     float difference[NUM_AXIS];
     LOOP_XYZE(i) difference[i] = target[i] - current_position[i];
@@ -7890,10 +7994,37 @@ void mesh_line_to_destination(float fr_mm_s, uint8_t x_splits = 0xff, uint8_t y_
     return true;
   }
 
-#endif // IS_KINEMATIC
+#else
+
+  /**
+   * Prepare a linear move in a Cartesian setup.
+   * If Mesh Bed Leveling is enabled, perform a mesh move.
+   */
+  inline bool prepare_move_to_destination_cartesian() {
+    // Do not use feedrate_percentage for E or Z only moves
+    if (current_position[X_AXIS] == destination[X_AXIS] && current_position[Y_AXIS] == destination[Y_AXIS]) {
+      line_to_destination();
+    }
+    else {
+      #if ENABLED(MESH_BED_LEVELING)
+        if (mbl.active()) {
+          mesh_line_to_destination(MMS_SCALED(feedrate_mm_s));
+          return false;
+        }
+        else
+      #endif
+          line_to_destination(MMS_SCALED(feedrate_mm_s));
+    }
+    return true;
+  }
+
+#endif // !IS_KINEMATIC
 
 #if ENABLED(DUAL_X_CARRIAGE)
 
+  /**
+   * Prepare a linear move in a dual X axis setup
+   */
   inline bool prepare_move_to_destination_dualx() {
     if (active_extruder_parked) {
       if (dual_x_carriage_mode == DXC_DUPLICATION_MODE && active_extruder == 0) {
@@ -7936,63 +8067,35 @@ void mesh_line_to_destination(float fr_mm_s, uint8_t x_splits = 0xff, uint8_t y_
 
 #endif // DUAL_X_CARRIAGE
 
-#if !IS_KINEMATIC
-
-  inline bool prepare_move_to_destination_cartesian() {
-    // Do not use feedrate_percentage for E or Z only moves
-    if (current_position[X_AXIS] == destination[X_AXIS] && current_position[Y_AXIS] == destination[Y_AXIS]) {
-      line_to_destination();
-    }
-    else {
-      #if ENABLED(MESH_BED_LEVELING)
-        if (mbl.active()) {
-          mesh_line_to_destination(MMS_SCALED(feedrate_mm_s));
-          return false;
-        }
-        else
-      #endif
-          line_to_destination(MMS_SCALED(feedrate_mm_s));
-    }
-    return true;
-  }
-
-#endif // !IS_KINEMATIC
-
-#if ENABLED(PREVENT_COLD_EXTRUSION)
-
-  inline void prevent_dangerous_extrude(float& curr_e, float& dest_e) {
-    if (DEBUGGING(DRYRUN)) return;
-    float de = dest_e - curr_e;
-    if (de) {
-      if (thermalManager.tooColdToExtrude(active_extruder)) {
-        curr_e = dest_e; // Behave as if the move really took place, but ignore E part
-        SERIAL_ECHO_START;
-        SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
-      }
-      #if ENABLED(PREVENT_LENGTHY_EXTRUDE)
-        if (labs(de) > EXTRUDE_MAXLENGTH) {
-          curr_e = dest_e; // Behave as if the move really took place, but ignore E part
-          SERIAL_ECHO_START;
-          SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
-        }
-      #endif
-    }
-  }
-
-#endif // PREVENT_COLD_EXTRUSION
-
 /**
  * Prepare a single move and get ready for the next one
  *
- * (This may call planner.buffer_line several times to put
- *  smaller moves into the planner for DELTA or SCARA.)
+ * This may result in several calls to planner.buffer_line to
+ * do smaller moves for DELTA, SCARA, mesh moves, etc.
  */
 void prepare_move_to_destination() {
   clamp_to_software_endstops(destination);
   refresh_cmd_timeout();
 
   #if ENABLED(PREVENT_COLD_EXTRUSION)
-    prevent_dangerous_extrude(current_position[E_AXIS], destination[E_AXIS]);
+
+    if (!DEBUGGING(DRYRUN)) {
+      if (destination[E_AXIS] != current_position[E_AXIS]) {
+        if (thermalManager.tooColdToExtrude(active_extruder)) {
+          current_position[E_AXIS] = destination[E_AXIS]; // Behave as if the move really took place, but ignore E part
+          SERIAL_ECHO_START;
+          SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
+        }
+        #if ENABLED(PREVENT_LENGTHY_EXTRUDE)
+          if (labs(destination[E_AXIS] - current_position[E_AXIS]) > EXTRUDE_MAXLENGTH) {
+            current_position[E_AXIS] = destination[E_AXIS]; // Behave as if the move really took place, but ignore E part
+            SERIAL_ECHO_START;
+            SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
+          }
+        #endif
+      }
+    }
+
   #endif
 
   #if IS_KINEMATIC
@@ -8281,26 +8384,6 @@ void prepare_move_to_destination() {
 
 #endif // IS_SCARA
 
-void get_cartesian_from_steppers() {
-  #if ENABLED(DELTA)
-    forward_kinematics_DELTA(
-      stepper.get_axis_position_mm(A_AXIS),
-      stepper.get_axis_position_mm(B_AXIS),
-      stepper.get_axis_position_mm(C_AXIS)
-    );
-  #elif IS_SCARA
-    forward_kinematics_SCARA(
-      stepper.get_axis_position_degrees(A_AXIS),
-      stepper.get_axis_position_degrees(B_AXIS)
-    );
-    cartes[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
-  #else
-    cartes[X_AXIS] = stepper.get_axis_position_mm(X_AXIS);
-    cartes[Y_AXIS] = stepper.get_axis_position_mm(Y_AXIS);
-    cartes[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
-  #endif
-}
-
 #if ENABLED(TEMP_STAT_LEDS)
 
   static bool red_led = false;
@@ -8327,6 +8410,91 @@ void get_cartesian_from_steppers() {
 
 #endif
 
+#if ENABLED(FILAMENT_RUNOUT_SENSOR)
+
+  void handle_filament_runout() {
+    if (!filament_ran_out) {
+      filament_ran_out = true;
+      enqueue_and_echo_commands_P(PSTR(FILAMENT_RUNOUT_SCRIPT));
+      stepper.synchronize();
+    }
+  }
+
+#endif // FILAMENT_RUNOUT_SENSOR
+
+#if ENABLED(FAST_PWM_FAN)
+
+  void setPwmFrequency(uint8_t pin, int val) {
+    val &= 0x07;
+    switch (digitalPinToTimer(pin)) {
+      #if defined(TCCR0A)
+        case TIMER0A:
+        case TIMER0B:
+          // TCCR0B &= ~(_BV(CS00) | _BV(CS01) | _BV(CS02));
+          // TCCR0B |= val;
+          break;
+      #endif
+      #if defined(TCCR1A)
+        case TIMER1A:
+        case TIMER1B:
+          // TCCR1B &= ~(_BV(CS10) | _BV(CS11) | _BV(CS12));
+          // TCCR1B |= val;
+          break;
+      #endif
+      #if defined(TCCR2)
+        case TIMER2:
+        case TIMER2:
+          TCCR2 &= ~(_BV(CS10) | _BV(CS11) | _BV(CS12));
+          TCCR2 |= val;
+          break;
+      #endif
+      #if defined(TCCR2A)
+        case TIMER2A:
+        case TIMER2B:
+          TCCR2B &= ~(_BV(CS20) | _BV(CS21) | _BV(CS22));
+          TCCR2B |= val;
+          break;
+      #endif
+      #if defined(TCCR3A)
+        case TIMER3A:
+        case TIMER3B:
+        case TIMER3C:
+          TCCR3B &= ~(_BV(CS30) | _BV(CS31) | _BV(CS32));
+          TCCR3B |= val;
+          break;
+      #endif
+      #if defined(TCCR4A)
+        case TIMER4A:
+        case TIMER4B:
+        case TIMER4C:
+          TCCR4B &= ~(_BV(CS40) | _BV(CS41) | _BV(CS42));
+          TCCR4B |= val;
+          break;
+      #endif
+      #if defined(TCCR5A)
+        case TIMER5A:
+        case TIMER5B:
+        case TIMER5C:
+          TCCR5B &= ~(_BV(CS50) | _BV(CS51) | _BV(CS52));
+          TCCR5B |= val;
+          break;
+      #endif
+    }
+  }
+
+#endif // FAST_PWM_FAN
+
+float calculate_volumetric_multiplier(float diameter) {
+  if (!volumetric_enabled || diameter == 0) return 1.0;
+  float d2 = diameter * 0.5;
+  return 1.0 / (M_PI * d2 * d2);
+}
+
+void calculate_volumetric_multipliers() {
+  for (uint8_t i = 0; i < COUNT(filament_size); i++)
+    volumetric_multiplier[i] = calculate_volumetric_multiplier(filament_size[i]);
+}
+
 void enable_all_steppers() {
   enable_x();
   enable_y();
@@ -8347,33 +8515,6 @@ void disable_all_steppers() {
   disable_e3();
 }
 
-/**
- * Standard idle routine keeps the machine alive
- */
-void idle(
-  #if ENABLED(FILAMENT_CHANGE_FEATURE)
-    bool no_stepper_sleep/*=false*/
-  #endif
-) {
-  lcd_update();
-  host_keepalive();
-  manage_inactivity(
-    #if ENABLED(FILAMENT_CHANGE_FEATURE)
-      no_stepper_sleep
-    #endif
-  );
-
-  thermalManager.manage_heater();
-
-  #if ENABLED(PRINTCOUNTER)
-    print_job_timer.tick();
-  #endif
-
-  #if HAS_BUZZER && PIN_EXISTS(BEEPER)
-    buzzer.tick();
-  #endif
-}
-
 /**
  * Manage several activities:
  *  - Check for Filament Runout
@@ -8551,6 +8692,37 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) {
   planner.check_axes_activity();
 }
 
+/**
+ * Standard idle routine keeps the machine alive
+ */
+void idle(
+  #if ENABLED(FILAMENT_CHANGE_FEATURE)
+    bool no_stepper_sleep/*=false*/
+  #endif
+) {
+  lcd_update();
+  host_keepalive();
+  manage_inactivity(
+    #if ENABLED(FILAMENT_CHANGE_FEATURE)
+      no_stepper_sleep
+    #endif
+  );
+
+  thermalManager.manage_heater();
+
+  #if ENABLED(PRINTCOUNTER)
+    print_job_timer.tick();
+  #endif
+
+  #if HAS_BUZZER && PIN_EXISTS(BEEPER)
+    buzzer.tick();
+  #endif
+}
+
+/**
+ * Kill all activity and lock the machine.
+ * After this the machine will need to be reset.
+ */
 void kill(const char* lcd_msg) {
   SERIAL_ERROR_START;
   SERIAL_ERRORLNPGM(MSG_ERR_KILLED);
@@ -8579,79 +8751,10 @@ void kill(const char* lcd_msg) {
   } // Wait for reset
 }
 
-#if ENABLED(FILAMENT_RUNOUT_SENSOR)
-
-  void handle_filament_runout() {
-    if (!filament_ran_out) {
-      filament_ran_out = true;
-      enqueue_and_echo_commands_P(PSTR(FILAMENT_RUNOUT_SCRIPT));
-      stepper.synchronize();
-    }
-  }
-
-#endif // FILAMENT_RUNOUT_SENSOR
-
-#if ENABLED(FAST_PWM_FAN)
-
-  void setPwmFrequency(uint8_t pin, int val) {
-    val &= 0x07;
-    switch (digitalPinToTimer(pin)) {
-      #if defined(TCCR0A)
-        case TIMER0A:
-        case TIMER0B:
-          // TCCR0B &= ~(_BV(CS00) | _BV(CS01) | _BV(CS02));
-          // TCCR0B |= val;
-          break;
-      #endif
-      #if defined(TCCR1A)
-        case TIMER1A:
-        case TIMER1B:
-          // TCCR1B &= ~(_BV(CS10) | _BV(CS11) | _BV(CS12));
-          // TCCR1B |= val;
-          break;
-      #endif
-      #if defined(TCCR2)
-        case TIMER2:
-        case TIMER2:
-          TCCR2 &= ~(_BV(CS10) | _BV(CS11) | _BV(CS12));
-          TCCR2 |= val;
-          break;
-      #endif
-      #if defined(TCCR2A)
-        case TIMER2A:
-        case TIMER2B:
-          TCCR2B &= ~(_BV(CS20) | _BV(CS21) | _BV(CS22));
-          TCCR2B |= val;
-          break;
-      #endif
-      #if defined(TCCR3A)
-        case TIMER3A:
-        case TIMER3B:
-        case TIMER3C:
-          TCCR3B &= ~(_BV(CS30) | _BV(CS31) | _BV(CS32));
-          TCCR3B |= val;
-          break;
-      #endif
-      #if defined(TCCR4A)
-        case TIMER4A:
-        case TIMER4B:
-        case TIMER4C:
-          TCCR4B &= ~(_BV(CS40) | _BV(CS41) | _BV(CS42));
-          TCCR4B |= val;
-          break;
-      #endif
-      #if defined(TCCR5A)
-        case TIMER5A:
-        case TIMER5B:
-        case TIMER5C:
-          TCCR5B &= ~(_BV(CS50) | _BV(CS51) | _BV(CS52));
-          TCCR5B |= val;
-          break;
-      #endif
-    }
-  }
-#endif // FAST_PWM_FAN
-
+/**
+ * Turn off heaters and stop the print in progress
+ * After a stop the machine may be resumed with M999
+ */
 void stop() {
   thermalManager.disable_all_heaters();
   if (IsRunning()) {
@@ -8663,17 +8766,6 @@ void stop() {
   }
 }
 
-float calculate_volumetric_multiplier(float diameter) {
-  if (!volumetric_enabled || diameter == 0) return 1.0;
-  float d2 = diameter * 0.5;
-  return 1.0 / (M_PI * d2 * d2);
-}
-
-void calculate_volumetric_multipliers() {
-  for (uint8_t i = 0; i < COUNT(filament_size); i++)
-    volumetric_multiplier[i] = calculate_volumetric_multiplier(filament_size[i]);
-}
-
 /**
  * Marlin entry-point: Set up before the program loop
  *  - Set up the kill pin, filament runout, power hold

From 9b0931ef74f297897d59f32bcdc01af4902bb0f7 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Tue, 13 Sep 2016 03:26:23 -0500
Subject: [PATCH 26/26] Apply fixed EXTRUDER_RUNOUT_PREVENT

---
 Marlin/Configuration_adv.h                      | 16 +++++++++-------
 Marlin/Marlin_main.cpp                          | 17 ++++++++---------
 .../Cartesio/Configuration_adv.h                | 16 +++++++++-------
 .../Felix/Configuration_adv.h                   | 16 +++++++++-------
 .../Hephestos/Configuration_adv.h               | 16 +++++++++-------
 .../Hephestos_2/Configuration_adv.h             | 16 +++++++++-------
 .../K8200/Configuration_adv.h                   | 16 +++++++++-------
 .../K8400/Configuration_adv.h                   | 16 +++++++++-------
 .../RigidBot/Configuration_adv.h                | 16 +++++++++-------
 .../SCARA/Configuration_adv.h                   | 16 +++++++++-------
 .../TAZ4/Configuration_adv.h                    | 16 +++++++++-------
 .../WITBOX/Configuration_adv.h                  | 16 +++++++++-------
 .../delta/biv2.5/Configuration_adv.h            | 16 +++++++++-------
 .../delta/generic/Configuration_adv.h           | 16 +++++++++-------
 .../delta/kossel_mini/Configuration_adv.h       | 16 +++++++++-------
 .../delta/kossel_pro/Configuration_adv.h        | 16 +++++++++-------
 .../delta/kossel_xl/Configuration_adv.h         | 16 +++++++++-------
 .../makibox/Configuration_adv.h                 | 16 +++++++++-------
 .../tvrrug/Round2/Configuration_adv.h           | 16 +++++++++-------
 19 files changed, 170 insertions(+), 135 deletions(-)

diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h
index f6df89a850..cdf223d11d 100644
--- a/Marlin/Configuration_adv.h
+++ b/Marlin/Configuration_adv.h
@@ -168,14 +168,16 @@
 
 // @section extruder
 
-//  extruder run-out prevention.
-//if the machine is idle, and the temperature over MINTEMP, every couple of SECONDS some filament is extruded
+// Extruder runout prevention.
+// If the machine is idle and the temperature over MINTEMP
+// then extrude some filament every couple of SECONDS.
 //#define EXTRUDER_RUNOUT_PREVENT
-#define EXTRUDER_RUNOUT_MINTEMP 190
-#define EXTRUDER_RUNOUT_SECONDS 30
-#define EXTRUDER_RUNOUT_ESTEPS 14   // mm filament
-#define EXTRUDER_RUNOUT_SPEED 1500  // extrusion speed
-#define EXTRUDER_RUNOUT_EXTRUDE 100
+#if ENABLED(EXTRUDER_RUNOUT_PREVENT)
+  #define EXTRUDER_RUNOUT_MINTEMP 190
+  #define EXTRUDER_RUNOUT_SECONDS 30
+  #define EXTRUDER_RUNOUT_SPEED 1500  // mm/m
+  #define EXTRUDER_RUNOUT_EXTRUDE 5   // mm
+#endif
 
 // @section temperature
 
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 7e894f51fd..bbc92e9f6a 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -8608,11 +8608,11 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) {
   #if ENABLED(EXTRUDER_RUNOUT_PREVENT)
     if (ELAPSED(ms, previous_cmd_ms + (EXTRUDER_RUNOUT_SECONDS) * 1000UL)
       && thermalManager.degHotend(active_extruder) > EXTRUDER_RUNOUT_MINTEMP) {
+      bool oldstatus;
       #if ENABLED(SWITCHING_EXTRUDER)
-        bool oldstatus = E0_ENABLE_READ;
+        oldstatus = E0_ENABLE_READ;
         enable_e0();
       #else // !SWITCHING_EXTRUDER
-        bool oldstatus;
         switch (active_extruder) {
           case 0:
             oldstatus = E0_ENABLE_READ;
@@ -8639,15 +8639,14 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) {
         }
       #endif // !SWITCHING_EXTRUDER
 
-      float oldepos = current_position[E_AXIS], oldedes = destination[E_AXIS];
-      planner.buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS],
-                       destination[E_AXIS] + (EXTRUDER_RUNOUT_EXTRUDE) * (EXTRUDER_RUNOUT_ESTEPS) * planner.steps_to_mm[E_AXIS],
-                       MMM_TO_MMS(EXTRUDER_RUNOUT_SPEED) * (EXTRUDER_RUNOUT_ESTEPS) * planner.steps_to_mm[E_AXIS], active_extruder);
-      current_position[E_AXIS] = oldepos;
-      destination[E_AXIS] = oldedes;
-      planner.set_e_position_mm(oldepos);
       previous_cmd_ms = ms; // refresh_cmd_timeout()
+      planner.buffer_line(
+        current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS],
+        current_position[E_AXIS] + EXTRUDER_RUNOUT_EXTRUDE,
+        MMM_TO_MMS(EXTRUDER_RUNOUT_SPEED), active_extruder
+      );
       stepper.synchronize();
+      planner.set_e_position_mm(current_position[E_AXIS]);
       #if ENABLED(SWITCHING_EXTRUDER)
         E0_ENABLE_WRITE(oldstatus);
       #else
diff --git a/Marlin/example_configurations/Cartesio/Configuration_adv.h b/Marlin/example_configurations/Cartesio/Configuration_adv.h
index 0763b33823..fb5a2ebca1 100644
--- a/Marlin/example_configurations/Cartesio/Configuration_adv.h
+++ b/Marlin/example_configurations/Cartesio/Configuration_adv.h
@@ -168,14 +168,16 @@
 
 // @section extruder
 
-//  extruder run-out prevention.
-//if the machine is idle, and the temperature over MINTEMP, every couple of SECONDS some filament is extruded
+// Extruder runout prevention.
+// If the machine is idle and the temperature over MINTEMP
+// then extrude some filament every couple of SECONDS.
 //#define EXTRUDER_RUNOUT_PREVENT
-#define EXTRUDER_RUNOUT_MINTEMP 190
-#define EXTRUDER_RUNOUT_SECONDS 30
-#define EXTRUDER_RUNOUT_ESTEPS 14   // mm filament
-#define EXTRUDER_RUNOUT_SPEED 1500  // extrusion speed
-#define EXTRUDER_RUNOUT_EXTRUDE 100
+#if ENABLED(EXTRUDER_RUNOUT_PREVENT)
+  #define EXTRUDER_RUNOUT_MINTEMP 190
+  #define EXTRUDER_RUNOUT_SECONDS 30
+  #define EXTRUDER_RUNOUT_SPEED 1500  // mm/m
+  #define EXTRUDER_RUNOUT_EXTRUDE 5   // mm
+#endif
 
 // @section temperature
 
diff --git a/Marlin/example_configurations/Felix/Configuration_adv.h b/Marlin/example_configurations/Felix/Configuration_adv.h
index a24bca1a26..6ebe50e926 100644
--- a/Marlin/example_configurations/Felix/Configuration_adv.h
+++ b/Marlin/example_configurations/Felix/Configuration_adv.h
@@ -168,14 +168,16 @@
 
 // @section extruder
 
-//  extruder run-out prevention.
-//if the machine is idle, and the temperature over MINTEMP, every couple of SECONDS some filament is extruded
+// Extruder runout prevention.
+// If the machine is idle and the temperature over MINTEMP
+// then extrude some filament every couple of SECONDS.
 //#define EXTRUDER_RUNOUT_PREVENT
-#define EXTRUDER_RUNOUT_MINTEMP 190
-#define EXTRUDER_RUNOUT_SECONDS 30
-#define EXTRUDER_RUNOUT_ESTEPS 14   // mm filament
-#define EXTRUDER_RUNOUT_SPEED 1500  // extrusion speed
-#define EXTRUDER_RUNOUT_EXTRUDE 100
+#if ENABLED(EXTRUDER_RUNOUT_PREVENT)
+  #define EXTRUDER_RUNOUT_MINTEMP 190
+  #define EXTRUDER_RUNOUT_SECONDS 30
+  #define EXTRUDER_RUNOUT_SPEED 1500  // mm/m
+  #define EXTRUDER_RUNOUT_EXTRUDE 5   // mm
+#endif
 
 // @section temperature
 
diff --git a/Marlin/example_configurations/Hephestos/Configuration_adv.h b/Marlin/example_configurations/Hephestos/Configuration_adv.h
index 7b826b5277..71139893fc 100644
--- a/Marlin/example_configurations/Hephestos/Configuration_adv.h
+++ b/Marlin/example_configurations/Hephestos/Configuration_adv.h
@@ -168,14 +168,16 @@
 
 // @section extruder
 
-//  extruder run-out prevention.
-//if the machine is idle, and the temperature over MINTEMP, every couple of SECONDS some filament is extruded
+// Extruder runout prevention.
+// If the machine is idle and the temperature over MINTEMP
+// then extrude some filament every couple of SECONDS.
 //#define EXTRUDER_RUNOUT_PREVENT
-#define EXTRUDER_RUNOUT_MINTEMP 190
-#define EXTRUDER_RUNOUT_SECONDS 30
-#define EXTRUDER_RUNOUT_ESTEPS 14   // mm filament
-#define EXTRUDER_RUNOUT_SPEED 1500  // extrusion speed
-#define EXTRUDER_RUNOUT_EXTRUDE 100
+#if ENABLED(EXTRUDER_RUNOUT_PREVENT)
+  #define EXTRUDER_RUNOUT_MINTEMP 190
+  #define EXTRUDER_RUNOUT_SECONDS 30
+  #define EXTRUDER_RUNOUT_SPEED 1500  // mm/m
+  #define EXTRUDER_RUNOUT_EXTRUDE 5   // mm
+#endif
 
 // @section temperature
 
diff --git a/Marlin/example_configurations/Hephestos_2/Configuration_adv.h b/Marlin/example_configurations/Hephestos_2/Configuration_adv.h
index deb6262928..4a426c6ac7 100644
--- a/Marlin/example_configurations/Hephestos_2/Configuration_adv.h
+++ b/Marlin/example_configurations/Hephestos_2/Configuration_adv.h
@@ -168,14 +168,16 @@
 
 // @section extruder
 
-//  extruder run-out prevention.
-//if the machine is idle, and the temperature over MINTEMP, every couple of SECONDS some filament is extruded
+// Extruder runout prevention.
+// If the machine is idle and the temperature over MINTEMP
+// then extrude some filament every couple of SECONDS.
 //#define EXTRUDER_RUNOUT_PREVENT
-#define EXTRUDER_RUNOUT_MINTEMP 190
-#define EXTRUDER_RUNOUT_SECONDS 30
-#define EXTRUDER_RUNOUT_ESTEPS 14   // mm filament
-#define EXTRUDER_RUNOUT_SPEED 1500  // extrusion speed
-#define EXTRUDER_RUNOUT_EXTRUDE 100
+#if ENABLED(EXTRUDER_RUNOUT_PREVENT)
+  #define EXTRUDER_RUNOUT_MINTEMP 190
+  #define EXTRUDER_RUNOUT_SECONDS 30
+  #define EXTRUDER_RUNOUT_SPEED 1500  // mm/m
+  #define EXTRUDER_RUNOUT_EXTRUDE 5   // mm
+#endif
 
 // @section temperature
 
diff --git a/Marlin/example_configurations/K8200/Configuration_adv.h b/Marlin/example_configurations/K8200/Configuration_adv.h
index d57fcea93d..34d12ec229 100644
--- a/Marlin/example_configurations/K8200/Configuration_adv.h
+++ b/Marlin/example_configurations/K8200/Configuration_adv.h
@@ -174,14 +174,16 @@
 
 // @section extruder
 
-//  extruder run-out prevention.
-//if the machine is idle, and the temperature over MINTEMP, every couple of SECONDS some filament is extruded
+// Extruder runout prevention.
+// If the machine is idle and the temperature over MINTEMP
+// then extrude some filament every couple of SECONDS.
 //#define EXTRUDER_RUNOUT_PREVENT
-#define EXTRUDER_RUNOUT_MINTEMP 190
-#define EXTRUDER_RUNOUT_SECONDS 30
-#define EXTRUDER_RUNOUT_ESTEPS 14   // mm filament
-#define EXTRUDER_RUNOUT_SPEED 1500  // extrusion speed
-#define EXTRUDER_RUNOUT_EXTRUDE 100
+#if ENABLED(EXTRUDER_RUNOUT_PREVENT)
+  #define EXTRUDER_RUNOUT_MINTEMP 190
+  #define EXTRUDER_RUNOUT_SECONDS 30
+  #define EXTRUDER_RUNOUT_SPEED 1500  // mm/m
+  #define EXTRUDER_RUNOUT_EXTRUDE 5   // mm
+#endif
 
 // @section temperature
 
diff --git a/Marlin/example_configurations/K8400/Configuration_adv.h b/Marlin/example_configurations/K8400/Configuration_adv.h
index 7a84269818..463f17382f 100644
--- a/Marlin/example_configurations/K8400/Configuration_adv.h
+++ b/Marlin/example_configurations/K8400/Configuration_adv.h
@@ -168,14 +168,16 @@
 
 // @section extruder
 
-//  extruder run-out prevention.
-//if the machine is idle, and the temperature over MINTEMP, every couple of SECONDS some filament is extruded
+// Extruder runout prevention.
+// If the machine is idle and the temperature over MINTEMP
+// then extrude some filament every couple of SECONDS.
 //#define EXTRUDER_RUNOUT_PREVENT
-#define EXTRUDER_RUNOUT_MINTEMP 190
-#define EXTRUDER_RUNOUT_SECONDS 30
-#define EXTRUDER_RUNOUT_ESTEPS 14   // mm filament
-#define EXTRUDER_RUNOUT_SPEED 1500  // extrusion speed
-#define EXTRUDER_RUNOUT_EXTRUDE 100
+#if ENABLED(EXTRUDER_RUNOUT_PREVENT)
+  #define EXTRUDER_RUNOUT_MINTEMP 190
+  #define EXTRUDER_RUNOUT_SECONDS 30
+  #define EXTRUDER_RUNOUT_SPEED 1500  // mm/m
+  #define EXTRUDER_RUNOUT_EXTRUDE 5   // mm
+#endif
 
 // @section temperature
 
diff --git a/Marlin/example_configurations/RigidBot/Configuration_adv.h b/Marlin/example_configurations/RigidBot/Configuration_adv.h
index faebf06ffa..191f1b8246 100644
--- a/Marlin/example_configurations/RigidBot/Configuration_adv.h
+++ b/Marlin/example_configurations/RigidBot/Configuration_adv.h
@@ -168,14 +168,16 @@
 
 // @section extruder
 
-//  extruder run-out prevention.
-//if the machine is idle, and the temperature over MINTEMP, every couple of SECONDS some filament is extruded
+// Extruder runout prevention.
+// If the machine is idle and the temperature over MINTEMP
+// then extrude some filament every couple of SECONDS.
 //#define EXTRUDER_RUNOUT_PREVENT
-#define EXTRUDER_RUNOUT_MINTEMP 190
-#define EXTRUDER_RUNOUT_SECONDS 30
-#define EXTRUDER_RUNOUT_ESTEPS 14   // mm filament
-#define EXTRUDER_RUNOUT_SPEED 1500  // extrusion speed
-#define EXTRUDER_RUNOUT_EXTRUDE 100
+#if ENABLED(EXTRUDER_RUNOUT_PREVENT)
+  #define EXTRUDER_RUNOUT_MINTEMP 190
+  #define EXTRUDER_RUNOUT_SECONDS 30
+  #define EXTRUDER_RUNOUT_SPEED 1500  // mm/m
+  #define EXTRUDER_RUNOUT_EXTRUDE 5   // mm
+#endif
 
 // @section temperature
 
diff --git a/Marlin/example_configurations/SCARA/Configuration_adv.h b/Marlin/example_configurations/SCARA/Configuration_adv.h
index a6d33e8aa8..7bc8880251 100644
--- a/Marlin/example_configurations/SCARA/Configuration_adv.h
+++ b/Marlin/example_configurations/SCARA/Configuration_adv.h
@@ -168,14 +168,16 @@
 
 // @section extruder
 
-//  extruder run-out prevention.
-//if the machine is idle, and the temperature over MINTEMP, every couple of SECONDS some filament is extruded
+// Extruder runout prevention.
+// If the machine is idle and the temperature over MINTEMP
+// then extrude some filament every couple of SECONDS.
 //#define EXTRUDER_RUNOUT_PREVENT
-#define EXTRUDER_RUNOUT_MINTEMP 180
-#define EXTRUDER_RUNOUT_SECONDS 30
-#define EXTRUDER_RUNOUT_ESTEPS 14   // mm filament
-#define EXTRUDER_RUNOUT_SPEED 180   // extrusion speed
-#define EXTRUDER_RUNOUT_EXTRUDE 100
+#if ENABLED(EXTRUDER_RUNOUT_PREVENT)
+  #define EXTRUDER_RUNOUT_MINTEMP 190
+  #define EXTRUDER_RUNOUT_SECONDS 30
+  #define EXTRUDER_RUNOUT_SPEED 180  // mm/m
+  #define EXTRUDER_RUNOUT_EXTRUDE 5  // mm
+#endif
 
 // @section temperature
 
diff --git a/Marlin/example_configurations/TAZ4/Configuration_adv.h b/Marlin/example_configurations/TAZ4/Configuration_adv.h
index 6252c27964..7d420646a1 100644
--- a/Marlin/example_configurations/TAZ4/Configuration_adv.h
+++ b/Marlin/example_configurations/TAZ4/Configuration_adv.h
@@ -168,14 +168,16 @@
 
 // @section extruder
 
-//  extruder run-out prevention.
-//if the machine is idle, and the temperature over MINTEMP, every couple of SECONDS some filament is extruded
+// Extruder runout prevention.
+// If the machine is idle and the temperature over MINTEMP
+// then extrude some filament every couple of SECONDS.
 //#define EXTRUDER_RUNOUT_PREVENT
-#define EXTRUDER_RUNOUT_MINTEMP 190
-#define EXTRUDER_RUNOUT_SECONDS 30
-#define EXTRUDER_RUNOUT_ESTEPS 14   // mm filament
-#define EXTRUDER_RUNOUT_SPEED 1500  // extrusion speed
-#define EXTRUDER_RUNOUT_EXTRUDE 100
+#if ENABLED(EXTRUDER_RUNOUT_PREVENT)
+  #define EXTRUDER_RUNOUT_MINTEMP 190
+  #define EXTRUDER_RUNOUT_SECONDS 30
+  #define EXTRUDER_RUNOUT_SPEED 1500  // mm/m
+  #define EXTRUDER_RUNOUT_EXTRUDE 5   // mm
+#endif
 
 // @section temperature
 
diff --git a/Marlin/example_configurations/WITBOX/Configuration_adv.h b/Marlin/example_configurations/WITBOX/Configuration_adv.h
index 7b826b5277..71139893fc 100644
--- a/Marlin/example_configurations/WITBOX/Configuration_adv.h
+++ b/Marlin/example_configurations/WITBOX/Configuration_adv.h
@@ -168,14 +168,16 @@
 
 // @section extruder
 
-//  extruder run-out prevention.
-//if the machine is idle, and the temperature over MINTEMP, every couple of SECONDS some filament is extruded
+// Extruder runout prevention.
+// If the machine is idle and the temperature over MINTEMP
+// then extrude some filament every couple of SECONDS.
 //#define EXTRUDER_RUNOUT_PREVENT
-#define EXTRUDER_RUNOUT_MINTEMP 190
-#define EXTRUDER_RUNOUT_SECONDS 30
-#define EXTRUDER_RUNOUT_ESTEPS 14   // mm filament
-#define EXTRUDER_RUNOUT_SPEED 1500  // extrusion speed
-#define EXTRUDER_RUNOUT_EXTRUDE 100
+#if ENABLED(EXTRUDER_RUNOUT_PREVENT)
+  #define EXTRUDER_RUNOUT_MINTEMP 190
+  #define EXTRUDER_RUNOUT_SECONDS 30
+  #define EXTRUDER_RUNOUT_SPEED 1500  // mm/m
+  #define EXTRUDER_RUNOUT_EXTRUDE 5   // mm
+#endif
 
 // @section temperature
 
diff --git a/Marlin/example_configurations/delta/biv2.5/Configuration_adv.h b/Marlin/example_configurations/delta/biv2.5/Configuration_adv.h
index bbafe89f6e..b4efc0016d 100644
--- a/Marlin/example_configurations/delta/biv2.5/Configuration_adv.h
+++ b/Marlin/example_configurations/delta/biv2.5/Configuration_adv.h
@@ -168,14 +168,16 @@
 
 // @section extruder
 
-//  extruder run-out prevention.
-//if the machine is idle, and the temperature over MINTEMP, every couple of SECONDS some filament is extruded
+// Extruder runout prevention.
+// If the machine is idle and the temperature over MINTEMP
+// then extrude some filament every couple of SECONDS.
 //#define EXTRUDER_RUNOUT_PREVENT
-#define EXTRUDER_RUNOUT_MINTEMP 190
-#define EXTRUDER_RUNOUT_SECONDS 30
-#define EXTRUDER_RUNOUT_ESTEPS 14   // mm filament
-#define EXTRUDER_RUNOUT_SPEED 1500  // extrusion speed
-#define EXTRUDER_RUNOUT_EXTRUDE 100
+#if ENABLED(EXTRUDER_RUNOUT_PREVENT)
+  #define EXTRUDER_RUNOUT_MINTEMP 190
+  #define EXTRUDER_RUNOUT_SECONDS 30
+  #define EXTRUDER_RUNOUT_SPEED 1500  // mm/m
+  #define EXTRUDER_RUNOUT_EXTRUDE 5   // mm
+#endif
 
 // @section temperature
 
diff --git a/Marlin/example_configurations/delta/generic/Configuration_adv.h b/Marlin/example_configurations/delta/generic/Configuration_adv.h
index 3345f32ec4..2960f84138 100644
--- a/Marlin/example_configurations/delta/generic/Configuration_adv.h
+++ b/Marlin/example_configurations/delta/generic/Configuration_adv.h
@@ -168,14 +168,16 @@
 
 // @section extruder
 
-//  extruder run-out prevention.
-//if the machine is idle, and the temperature over MINTEMP, every couple of SECONDS some filament is extruded
+// Extruder runout prevention.
+// If the machine is idle and the temperature over MINTEMP
+// then extrude some filament every couple of SECONDS.
 //#define EXTRUDER_RUNOUT_PREVENT
-#define EXTRUDER_RUNOUT_MINTEMP 190
-#define EXTRUDER_RUNOUT_SECONDS 30
-#define EXTRUDER_RUNOUT_ESTEPS 14   // mm filament
-#define EXTRUDER_RUNOUT_SPEED 1500  // extrusion speed
-#define EXTRUDER_RUNOUT_EXTRUDE 100
+#if ENABLED(EXTRUDER_RUNOUT_PREVENT)
+  #define EXTRUDER_RUNOUT_MINTEMP 190
+  #define EXTRUDER_RUNOUT_SECONDS 30
+  #define EXTRUDER_RUNOUT_SPEED 1500  // mm/m
+  #define EXTRUDER_RUNOUT_EXTRUDE 5   // mm
+#endif
 
 // @section temperature
 
diff --git a/Marlin/example_configurations/delta/kossel_mini/Configuration_adv.h b/Marlin/example_configurations/delta/kossel_mini/Configuration_adv.h
index 97a1712c8a..9936c54e52 100644
--- a/Marlin/example_configurations/delta/kossel_mini/Configuration_adv.h
+++ b/Marlin/example_configurations/delta/kossel_mini/Configuration_adv.h
@@ -168,14 +168,16 @@
 
 // @section extruder
 
-//  extruder run-out prevention.
-//if the machine is idle, and the temperature over MINTEMP, every couple of SECONDS some filament is extruded
+// Extruder runout prevention.
+// If the machine is idle and the temperature over MINTEMP
+// then extrude some filament every couple of SECONDS.
 //#define EXTRUDER_RUNOUT_PREVENT
-#define EXTRUDER_RUNOUT_MINTEMP 190
-#define EXTRUDER_RUNOUT_SECONDS 30
-#define EXTRUDER_RUNOUT_ESTEPS 14   // mm filament
-#define EXTRUDER_RUNOUT_SPEED 1500  // extrusion speed
-#define EXTRUDER_RUNOUT_EXTRUDE 100
+#if ENABLED(EXTRUDER_RUNOUT_PREVENT)
+  #define EXTRUDER_RUNOUT_MINTEMP 190
+  #define EXTRUDER_RUNOUT_SECONDS 30
+  #define EXTRUDER_RUNOUT_SPEED 1500  // mm/m
+  #define EXTRUDER_RUNOUT_EXTRUDE 5   // mm
+#endif
 
 // @section temperature
 
diff --git a/Marlin/example_configurations/delta/kossel_pro/Configuration_adv.h b/Marlin/example_configurations/delta/kossel_pro/Configuration_adv.h
index 2203ee7942..9c82b1fb00 100644
--- a/Marlin/example_configurations/delta/kossel_pro/Configuration_adv.h
+++ b/Marlin/example_configurations/delta/kossel_pro/Configuration_adv.h
@@ -173,14 +173,16 @@
 
 // @section extruder
 
-//  extruder run-out prevention.
-//if the machine is idle, and the temperature over MINTEMP, every couple of SECONDS some filament is extruded
+// Extruder runout prevention.
+// If the machine is idle and the temperature over MINTEMP
+// then extrude some filament every couple of SECONDS.
 //#define EXTRUDER_RUNOUT_PREVENT
-#define EXTRUDER_RUNOUT_MINTEMP 190
-#define EXTRUDER_RUNOUT_SECONDS 30
-#define EXTRUDER_RUNOUT_ESTEPS 14   // mm filament
-#define EXTRUDER_RUNOUT_SPEED 1500  // extrusion speed
-#define EXTRUDER_RUNOUT_EXTRUDE 100
+#if ENABLED(EXTRUDER_RUNOUT_PREVENT)
+  #define EXTRUDER_RUNOUT_MINTEMP 190
+  #define EXTRUDER_RUNOUT_SECONDS 30
+  #define EXTRUDER_RUNOUT_SPEED 1500  // mm/m
+  #define EXTRUDER_RUNOUT_EXTRUDE 5   // mm
+#endif
 
 // @section temperature
 
diff --git a/Marlin/example_configurations/delta/kossel_xl/Configuration_adv.h b/Marlin/example_configurations/delta/kossel_xl/Configuration_adv.h
index 4532819852..fc68cbcefc 100644
--- a/Marlin/example_configurations/delta/kossel_xl/Configuration_adv.h
+++ b/Marlin/example_configurations/delta/kossel_xl/Configuration_adv.h
@@ -168,14 +168,16 @@
 
 // @section extruder
 
-//  extruder run-out prevention.
-//if the machine is idle, and the temperature over MINTEMP, every couple of SECONDS some filament is extruded
+// Extruder runout prevention.
+// If the machine is idle and the temperature over MINTEMP
+// then extrude some filament every couple of SECONDS.
 //#define EXTRUDER_RUNOUT_PREVENT
-#define EXTRUDER_RUNOUT_MINTEMP 190
-#define EXTRUDER_RUNOUT_SECONDS 30
-#define EXTRUDER_RUNOUT_ESTEPS 14   // mm filament
-#define EXTRUDER_RUNOUT_SPEED 1500  // extrusion speed
-#define EXTRUDER_RUNOUT_EXTRUDE 100
+#if ENABLED(EXTRUDER_RUNOUT_PREVENT)
+  #define EXTRUDER_RUNOUT_MINTEMP 190
+  #define EXTRUDER_RUNOUT_SECONDS 30
+  #define EXTRUDER_RUNOUT_SPEED 1500  // mm/m
+  #define EXTRUDER_RUNOUT_EXTRUDE 5   // mm
+#endif
 
 // @section temperature
 
diff --git a/Marlin/example_configurations/makibox/Configuration_adv.h b/Marlin/example_configurations/makibox/Configuration_adv.h
index addfc7acdf..7e9d002f87 100644
--- a/Marlin/example_configurations/makibox/Configuration_adv.h
+++ b/Marlin/example_configurations/makibox/Configuration_adv.h
@@ -168,14 +168,16 @@
 
 // @section extruder
 
-//  extruder run-out prevention.
-//if the machine is idle, and the temperature over MINTEMP, every couple of SECONDS some filament is extruded
+// Extruder runout prevention.
+// If the machine is idle and the temperature over MINTEMP
+// then extrude some filament every couple of SECONDS.
 //#define EXTRUDER_RUNOUT_PREVENT
-#define EXTRUDER_RUNOUT_MINTEMP 190
-#define EXTRUDER_RUNOUT_SECONDS 30
-#define EXTRUDER_RUNOUT_ESTEPS 14   // mm filament
-#define EXTRUDER_RUNOUT_SPEED 1500  // extrusion speed
-#define EXTRUDER_RUNOUT_EXTRUDE 100
+#if ENABLED(EXTRUDER_RUNOUT_PREVENT)
+  #define EXTRUDER_RUNOUT_MINTEMP 190
+  #define EXTRUDER_RUNOUT_SECONDS 30
+  #define EXTRUDER_RUNOUT_SPEED 1500  // mm/m
+  #define EXTRUDER_RUNOUT_EXTRUDE 5   // mm
+#endif
 
 // @section temperature
 
diff --git a/Marlin/example_configurations/tvrrug/Round2/Configuration_adv.h b/Marlin/example_configurations/tvrrug/Round2/Configuration_adv.h
index 1e8e626cc0..e6f1a7f25b 100644
--- a/Marlin/example_configurations/tvrrug/Round2/Configuration_adv.h
+++ b/Marlin/example_configurations/tvrrug/Round2/Configuration_adv.h
@@ -168,14 +168,16 @@
 
 // @section extruder
 
-//  extruder run-out prevention.
-//if the machine is idle, and the temperature over MINTEMP, every couple of SECONDS some filament is extruded
+// Extruder runout prevention.
+// If the machine is idle and the temperature over MINTEMP
+// then extrude some filament every couple of SECONDS.
 //#define EXTRUDER_RUNOUT_PREVENT
-#define EXTRUDER_RUNOUT_MINTEMP 190
-#define EXTRUDER_RUNOUT_SECONDS 30
-#define EXTRUDER_RUNOUT_ESTEPS 14   // mm filament
-#define EXTRUDER_RUNOUT_SPEED 1500  // extrusion speed
-#define EXTRUDER_RUNOUT_EXTRUDE 100
+#if ENABLED(EXTRUDER_RUNOUT_PREVENT)
+  #define EXTRUDER_RUNOUT_MINTEMP 190
+  #define EXTRUDER_RUNOUT_SECONDS 30
+  #define EXTRUDER_RUNOUT_SPEED 1500  // mm/m
+  #define EXTRUDER_RUNOUT_EXTRUDE 5   // mm
+#endif
 
 // @section temperature