From 9f3ff14008fbcdc82e12a4cdf57129fde29219b6 Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Sat, 28 Mar 2015 04:31:51 -0500
Subject: [PATCH 01/37] Enabled separate Z Probe and Z Axis endstop use at same
 time. Typo fixes in comments in existing code.

---
 Marlin/Configuration.h | 28 +++++++++++++++++++++++
 Marlin/pins.h          |  4 ++++
 Marlin/pins_RAMPS_13.h |  1 +
 Marlin/stepper.cpp     | 52 +++++++++++++++++++++++++++++++++++++++++-
 4 files changed, 84 insertions(+), 1 deletion(-)

diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index ad7ec45909..ff6a57c239 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -317,6 +317,7 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 const bool X_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic of the endstop.
 const bool Y_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic of the endstop.
 const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic of the endstop.
+
 //#define DISABLE_MAX_ENDSTOPS
 //#define DISABLE_MIN_ENDSTOPS
 
@@ -483,6 +484,33 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 
   #endif
 
+// If you have are a Z Probe in addition to endstop(s) for Z Homing, uncomment the #define Z_PROBE_AND_ENDSTOP line below and configure Z Probe settings.
+// Only use this if you have both a Z PROBE and Z HOMING ENDSTOP(S). If you are using Z_SAFE_HOMING above, then you probably don't need this unless you want to make use of
+// a non-default pin for your Z Probe.
+// Note: It's expected that your Z Probe triggers in the direction towards your bed. If your Z Probe does not trigger when traveling towards you bed, it will trigger when it's moving
+// away from the bed.
+
+//  #define Z_PROBE_AND_ENDSTOP
+
+  #ifdef Z_PROBE_AND_ENDSTOP
+
+// As of 3-28-2015, there are NO Z Probe pins defined in any board config files.
+// Z_PROBE_PIN is for the signal pin only. RAMPS 1.3/1.4 boards may be able to use the 5V, Ground and the D1 pin in the Aux 1 section of the RAMPS board for the signal.
+// The D1 pin in Aux 1 on RAMPS maps to the Arduino D1 pin. The Z_PROBE_PIN is setting the pin to use on the Arduino. Since the D1 pin on the RAMPS maps to D1 on Arduino, this works.
+// If you have RAMPS 1.3/1.4 and want to use the RAMPS D1 pin, set Z_PROBE_PIN to 1 and use ground and 5v next to it as needed. Check the RAMPS 1.3/1.4 pinout diagram for details.
+// WARNING: Setting the wrong pin may have unexpected and disastrous outcomes. Use with caution and do your homework.
+  #define Z_PROBE_PIN -1
+
+// Mechanical endstop with COM to ground and NC to Signal uses "false" here (most common setup).
+    const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic of the endstop.
+
+// The pullups are needed if you directly connect a mechanical endstop between the signal and ground pins.
+    #define ENDSTOPPULLUP_ZPROBE
+
+// If you want to enable the Z Probe pin, but disable its use, uncomment the line below.
+//    #define DISABLE_Z_PROBE_ENDSTOP
+  #endif
+
 #endif // ENABLE_AUTO_BED_LEVELING
 
 
diff --git a/Marlin/pins.h b/Marlin/pins.h
index 939dab5e66..8f013d5d01 100644
--- a/Marlin/pins.h
+++ b/Marlin/pins.h
@@ -178,6 +178,10 @@
   #define Z_MIN_PIN          -1
 #endif
 
+#ifdef DISABLE_Z_PROBE_ENDSTOP
+  #define Z_PROBE_PIN        -1
+#endif
+
 #ifdef DISABLE_XMAX_ENDSTOP
   #undef X_MAX_PIN
   #define X_MAX_PIN          -1
diff --git a/Marlin/pins_RAMPS_13.h b/Marlin/pins_RAMPS_13.h
index 71287f6832..ece70005b3 100644
--- a/Marlin/pins_RAMPS_13.h
+++ b/Marlin/pins_RAMPS_13.h
@@ -34,6 +34,7 @@
 #define Z_ENABLE_PIN       62
 #define Z_MIN_PIN          18
 #define Z_MAX_PIN          19
+#define Z_PROBE_PIN        -1
 
 #define Y2_STEP_PIN        36
 #define Y2_DIR_PIN         34
diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index 40d5a36eb4..bf83c927e5 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -97,6 +97,9 @@ static bool old_x_min_endstop = false,
               old_z2_min_endstop = false,
               old_z2_max_endstop = false;
             #endif
+            #if defined Z_PROBE_AND_ENDSTOP
+              old_z_probe_endstop = false;
+            #endif
 
 static bool check_endstops = true;
 
@@ -520,6 +523,26 @@ ISR(TIMER1_COMPA_vect) {
             old_z2_min_endstop = z2_min_endstop;
           #endif
         #endif
+
+        #if defined(Z_PROBE_PIN) && Z_PROBE_PIN > -1
+          UPDATE_ENDSTOP(z, Z, probe, PROBE);
+          bool z_probe_endstop(READ(Z_PROBE_PIN) != Z_MIN_ENDSTOP_INVERTING);
+          if(z_probe_endstop && old_z_probe_endstop)
+          {
+        	  endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
+        	  endstop_z_hit=true;
+
+//        	  if (z_probe_endstop && old_z_probe_endstop) SERIAL_ECHOLN("z_probe_endstop = true");
+
+
+        	  if (!(performing_homing)) //if not performing home
+        	  {
+        		  step_events_completed = current_block->step_event_count;
+        	  }
+          }
+          old_z_probe_endstop = z_probe_endstop;
+          old_z2_probe_endstop = z2_probe_endstop;
+        #endif
       }
     }
     else { // +direction
@@ -554,6 +577,26 @@ ISR(TIMER1_COMPA_vect) {
             old_z2_max_endstop = z2_max_endstop;
           #endif
         #endif
+
+        #if defined(Z_PROBE_PIN) && Z_PROBE_PIN > -1
+          UPDATE_ENDSTOP(z, Z, probe, PROBE);
+          bool z_probe_endstop(READ(Z_PROBE_PIN) != Z_MAX_ENDSTOP_INVERTING);
+          if(z_probe_endstop && old_z_probe_endstop)
+          {
+        	  endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
+        	  endstop_z_hit=true;
+
+//        	  if (z_probe_endstop && old_z_probe_endstop) SERIAL_ECHOLN("z_probe_endstop = true");
+
+
+        	  if (!(performing_homing)) //if not performing home
+        	  {
+        		  step_events_completed = current_block->step_event_count;
+        	  }
+          }
+          old_z_probe_endstop = z_probe_endstop;
+          old_z2_probe_endstop = z2_probe_endstop;
+        #endif
       }
     }
 
@@ -635,7 +678,7 @@ ISR(TIMER1_COMPA_vect) {
       step_events_completed++;
       if (step_events_completed >= current_block->step_event_count) break;
     }
-    // Calculare new timer value
+    // Calculate new timer value
     unsigned short timer;
     unsigned short step_rate;
     if (step_events_completed <= (unsigned long int)current_block->accelerate_until) {
@@ -918,6 +961,13 @@ void st_init() {
     #endif
   #endif  
   
+#if defined(Z_PROBE_PIN) && Z_PROBE_PIN >= 0
+  SET_INPUT(Z_PROBE_PIN);
+  #ifdef ENDSTOPPULLUP_ZPROBE
+    WRITE(Z_PROBE_PIN,HIGH);
+  #endif
+#endif
+
   #define AXIS_INIT(axis, AXIS, PIN) \
     AXIS ##_STEP_INIT; \
     AXIS ##_STEP_WRITE(INVERT_## PIN ##_STEP_PIN); \

From 2979b40a7a3ef2937aa900c06b733ee3f6b4373a Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Sat, 28 Mar 2015 04:41:03 -0500
Subject: [PATCH 02/37] Fixed typo in Z Probe and Endstop section.

---
 Marlin/Configuration.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index ff6a57c239..21141938ba 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -484,7 +484,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 
   #endif
 
-// If you have are a Z Probe in addition to endstop(s) for Z Homing, uncomment the #define Z_PROBE_AND_ENDSTOP line below and configure Z Probe settings.
+// If you have a Z Probe in addition to endstop(s) for Z Homing, uncomment the #define Z_PROBE_AND_ENDSTOP line below and configure Z Probe settings.
 // Only use this if you have both a Z PROBE and Z HOMING ENDSTOP(S). If you are using Z_SAFE_HOMING above, then you probably don't need this unless you want to make use of
 // a non-default pin for your Z Probe.
 // Note: It's expected that your Z Probe triggers in the direction towards your bed. If your Z Probe does not trigger when traveling towards you bed, it will trigger when it's moving

From 44b88b41a29ffd436ae79539a35749a5f98f9650 Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Sat, 28 Mar 2015 05:01:04 -0500
Subject: [PATCH 03/37] Added credit for code.

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

diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index 21141938ba..b8ef0755a2 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -484,6 +484,8 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 
   #endif
 
+// Z Probe and Z endstop support
+// Added by Chris Roadfeldt 3-28-2015
 // If you have a Z Probe in addition to endstop(s) for Z Homing, uncomment the #define Z_PROBE_AND_ENDSTOP line below and configure Z Probe settings.
 // Only use this if you have both a Z PROBE and Z HOMING ENDSTOP(S). If you are using Z_SAFE_HOMING above, then you probably don't need this unless you want to make use of
 // a non-default pin for your Z Probe.

From 92eb8109ab73871da4b651c6f45cb908f0155a2f Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Sat, 28 Mar 2015 05:09:48 -0500
Subject: [PATCH 04/37] Fix declaration of old_z_probe_endstop.

---
 Marlin/stepper.cpp | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index bf83c927e5..4241111d78 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -97,9 +97,10 @@ static bool old_x_min_endstop = false,
               old_z2_min_endstop = false,
               old_z2_max_endstop = false;
             #endif
-            #if defined Z_PROBE_AND_ENDSTOP
-              old_z_probe_endstop = false;
-            #endif
+
+#ifdef Z_PROBE_AND_ENDSTOP
+static bool old_z_probe_endstop = false;
+#endif
 
 static bool check_endstops = true;
 

From fd823449adc46369ff4e3efdb51dfc03f041615e Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Sat, 28 Mar 2015 05:42:38 -0500
Subject: [PATCH 05/37] Added serial message for Z Probe trigger.

---
 Marlin/Configuration.h | 6 +++---
 Marlin/Marlin_main.cpp | 5 ++++-
 Marlin/language.h      | 1 +
 3 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index b8ef0755a2..262b4e15b6 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -497,9 +497,9 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
   #ifdef Z_PROBE_AND_ENDSTOP
 
 // As of 3-28-2015, there are NO Z Probe pins defined in any board config files.
-// Z_PROBE_PIN is for the signal pin only. RAMPS 1.3/1.4 boards may be able to use the 5V, Ground and the D1 pin in the Aux 1 section of the RAMPS board for the signal.
-// The D1 pin in Aux 1 on RAMPS maps to the Arduino D1 pin. The Z_PROBE_PIN is setting the pin to use on the Arduino. Since the D1 pin on the RAMPS maps to D1 on Arduino, this works.
-// If you have RAMPS 1.3/1.4 and want to use the RAMPS D1 pin, set Z_PROBE_PIN to 1 and use ground and 5v next to it as needed. Check the RAMPS 1.3/1.4 pinout diagram for details.
+// Z_PROBE_PIN is for the signal pin only. RAMPS 1.3/1.4 boards may be able to use the 5V, Ground and the D32 pin in the Aux 4 section of the RAMPS board for the signal.
+// The D32 pin in Aux 4 on RAMPS maps to the Arduino D32 pin. The Z_PROBE_PIN is setting the pin to use on the Arduino. Since the D32 pin on the RAMPS maps to D32 on Arduino, this works.
+// If you have RAMPS 1.3/1.4 and want to use the RAMPS D32 pin, set Z_PROBE_PIN to 32 and use ground and 5v next to it as needed. Check the RAMPS 1.3/1.4 pinout diagram for details.
 // WARNING: Setting the wrong pin may have unexpected and disastrous outcomes. Use with caution and do your homework.
   #define Z_PROBE_PIN -1
 
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 0808a727dc..ca06000b6a 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -3509,7 +3509,10 @@ inline void gcode_M119() {
     SERIAL_PROTOCOLPGM(MSG_Z2_MAX);
     SERIAL_PROTOCOLLN(((READ(Z2_MAX_PIN)^Z2_MAX_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
   #endif
-  
+  #if defined(Z_PROBE_PIN) && Z_PROBE_PIN >-1
+    SERIAL_PROTOCOLPGM(MSG_Z_PROBE);
+    SERIALPROTOCOLLN(((READ(Z_PROBE_PIN)^72Z_PROBE_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
+  #endif
 }
 
 /**
diff --git a/Marlin/language.h b/Marlin/language.h
index 3f2291a947..0fbaa39b14 100644
--- a/Marlin/language.h
+++ b/Marlin/language.h
@@ -132,6 +132,7 @@
 #define MSG_Z_MIN                           "z_min: "
 #define MSG_Z_MAX                           "z_max: "
 #define MSG_Z2_MAX                          "z2_max: "
+#define MSG_Z_PROBE							"z_probe: "
 #define MSG_M119_REPORT                     "Reporting endstop status"
 #define MSG_ENDSTOP_HIT                     "TRIGGERED"
 #define MSG_ENDSTOP_OPEN                    "open"

From 8b81f20c6127f4fd579512f004a85facf1a2501a Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Sat, 28 Mar 2015 05:55:42 -0500
Subject: [PATCH 06/37] Filling in more places where Z_PROBE_PIN and
 Z_PROBE_AND_ENDSTOP need to be. Added Sanity Check for it. Added hook so it's
 enabled.

---
 Marlin/Marlin_main.cpp |  5 +++++
 Marlin/SanityCheck.h   | 11 ++++++-----
 Marlin/pins.h          |  2 +-
 3 files changed, 12 insertions(+), 6 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index ca06000b6a..7f8592b2c0 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -1290,8 +1290,13 @@ static void engage_z_probe() {
     
     st_synchronize();
     
+    #if defined(Z_PROBE_AND_ENDSTOP)
+    bool z_probe_endstop = (READ(Z_PROBE_PIN) != Z_PROBE_ENDSTOP_INVERTING);
+    if (z_probe_endstop)
+    #else
     bool z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
     if (z_min_endstop)
+    #endif
     {
         if (!Stopped)
         {
diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h
index d5183abbaa..1024b6ab47 100644
--- a/Marlin/SanityCheck.h
+++ b/Marlin/SanityCheck.h
@@ -90,13 +90,14 @@
      * Require a Z Min pin
      */
     #if Z_MIN_PIN == -1
-      #ifdef Z_PROBE_REPEATABILITY_TEST
-        #error You must have a Z_MIN endstop to enable Z_PROBE_REPEATABILITY_TEST.
-      #else
-        #error ENABLE_AUTO_BED_LEVELING requires a Z_MIN endstop. Z_MIN_PIN must point to a valid hardware pin.
+      #if Z_PROBE_PIN == -1
+        #ifdef Z_PROBE_REPEATABILITY_TEST
+          #error You must have a Z_MIN endstop to enable Z_PROBE_REPEATABILITY_TEST.
+        #else
+          #error ENABLE_AUTO_BED_LEVELING requires a Z_MIN or Z_PROBE endstop. Z_MIN_PIN or Z_PROBE_PIN must point to a valid hardware pin.
+        #endif
       #endif
     #endif
-
     /**
      * Check if Probe_Offset * Grid Points is greater than Probing Range
      */
diff --git a/Marlin/pins.h b/Marlin/pins.h
index 8f013d5d01..e9d06e998c 100644
--- a/Marlin/pins.h
+++ b/Marlin/pins.h
@@ -211,7 +211,7 @@
   #define Z_MIN_PIN          -1
 #endif
 
-#define SENSITIVE_PINS { 0, 1, X_STEP_PIN, X_DIR_PIN, X_ENABLE_PIN, X_MIN_PIN, X_MAX_PIN, Y_STEP_PIN, Y_DIR_PIN, Y_ENABLE_PIN, Y_MIN_PIN, Y_MAX_PIN, Z_STEP_PIN, Z_DIR_PIN, Z_ENABLE_PIN, Z_MIN_PIN, Z_MAX_PIN, PS_ON_PIN, \
+#define SENSITIVE_PINS { 0, 1, X_STEP_PIN, X_DIR_PIN, X_ENABLE_PIN, X_MIN_PIN, X_MAX_PIN, Y_STEP_PIN, Y_DIR_PIN, Y_ENABLE_PIN, Y_MIN_PIN, Y_MAX_PIN, Z_STEP_PIN, Z_DIR_PIN, Z_ENABLE_PIN, Z_MIN_PIN, Z_MAX_PIN, Z_PROBE_PIN, PS_ON_PIN, \
                         HEATER_BED_PIN, FAN_PIN, \
                         _E0_PINS _E1_PINS _E2_PINS _E3_PINS \
                         analogInputToDigitalPin(TEMP_BED_PIN) \

From 059052889f76dea37bfe9bd8e6b402231b9c0995 Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Sat, 28 Mar 2015 06:08:04 -0500
Subject: [PATCH 07/37] Further Sanity Checks for Z_PROBE_AND_ENDSTOP.

---
 Marlin/SanityCheck.h | 12 +++++++++++-
 1 file changed, 11 insertions(+), 1 deletion(-)

diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h
index 1024b6ab47..5a24cbc1ce 100644
--- a/Marlin/SanityCheck.h
+++ b/Marlin/SanityCheck.h
@@ -92,12 +92,22 @@
     #if Z_MIN_PIN == -1
       #if Z_PROBE_PIN == -1
         #ifdef Z_PROBE_REPEATABILITY_TEST
-          #error You must have a Z_MIN endstop to enable Z_PROBE_REPEATABILITY_TEST.
+          #error You must have a Z_MIN or Z_PROBE endstop to enable Z_PROBE_REPEATABILITY_TEST.
         #else
           #error ENABLE_AUTO_BED_LEVELING requires a Z_MIN or Z_PROBE endstop. Z_MIN_PIN or Z_PROBE_PIN must point to a valid hardware pin.
         #endif
       #endif
     #endif
+
+    /**
+     * Require a Z Probe Pin if Z_PROBE_AND_ENDSTOP is enabled.
+     */
+    #if defined(Z_PROBE_AND_ENDSTOP)
+      #if Z_PROBE_PIN == -1
+        #error You must have a Z_PROBE_PIN defined if you enable Z_PROBE_AND_ENDSTOP
+      #endif
+    #endif
+
     /**
      * Check if Probe_Offset * Grid Points is greater than Probing Range
      */

From 324c14943bdb58a26cf95574dc9b0d329bbe4027 Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Sun, 29 Mar 2015 02:16:09 -0500
Subject: [PATCH 08/37] Bug fixes for Z_PROBE_AND_ENDSTOP. Code cleanup for
 Z_PROBE_AND_ENDSTOP. Added Z_PROBE_PIN to pins_RAMPS_13.h

---
 Marlin/Configuration.h |  3 +--
 Marlin/Marlin_main.cpp |  9 +++++++--
 Marlin/pins_RAMPS_13.h |  5 +++++
 Marlin/stepper.cpp     | 19 ++-----------------
 4 files changed, 15 insertions(+), 21 deletions(-)

diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index 262b4e15b6..b23864065c 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -499,9 +499,8 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 // As of 3-28-2015, there are NO Z Probe pins defined in any board config files.
 // Z_PROBE_PIN is for the signal pin only. RAMPS 1.3/1.4 boards may be able to use the 5V, Ground and the D32 pin in the Aux 4 section of the RAMPS board for the signal.
 // The D32 pin in Aux 4 on RAMPS maps to the Arduino D32 pin. The Z_PROBE_PIN is setting the pin to use on the Arduino. Since the D32 pin on the RAMPS maps to D32 on Arduino, this works.
-// If you have RAMPS 1.3/1.4 and want to use the RAMPS D32 pin, set Z_PROBE_PIN to 32 and use ground and 5v next to it as needed. Check the RAMPS 1.3/1.4 pinout diagram for details.
+// D32 is currently selected in the RAMPS 1.3/1.4 pin file. Update the pins.h file for your control board to make use of this. Not doing so nullifies Z_PROBE_AND_ENDSTOP
 // WARNING: Setting the wrong pin may have unexpected and disastrous outcomes. Use with caution and do your homework.
-  #define Z_PROBE_PIN -1
 
 // Mechanical endstop with COM to ground and NC to Signal uses "false" here (most common setup).
     const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic of the endstop.
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 7f8592b2c0..87ec4792cd 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -1359,8 +1359,13 @@ static void retract_z_probe() {
     
     st_synchronize();
     
+    #if defined(Z_PROBE_AND_ENDSTOP)
+    bool z_probe_endstop = (READ(Z_PROBE_PIN) != Z_PROBE_ENDSTOP_INVERTING);
+    if (z_probe_endstop)
+    #else
     bool z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
-    if (!z_min_endstop)
+    if (z_min_endstop)
+    #endif
     {
         if (!Stopped)
         {
@@ -3516,7 +3521,7 @@ inline void gcode_M119() {
   #endif
   #if defined(Z_PROBE_PIN) && Z_PROBE_PIN >-1
     SERIAL_PROTOCOLPGM(MSG_Z_PROBE);
-    SERIALPROTOCOLLN(((READ(Z_PROBE_PIN)^72Z_PROBE_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
+    SERIAL_PROTOCOLLN(((READ(Z_PROBE_PIN)^Z_PROBE_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
   #endif
 }
 
diff --git a/Marlin/pins_RAMPS_13.h b/Marlin/pins_RAMPS_13.h
index ece70005b3..4bd2bce468 100644
--- a/Marlin/pins_RAMPS_13.h
+++ b/Marlin/pins_RAMPS_13.h
@@ -62,6 +62,11 @@
   #define FILWIDTH_PIN        5
 #endif
 
+#if defined(Z_PROBE_ANDENDSTOP)
+  // Define a pin to use as the signal pin on Arduino for the Z_PROBE endstop.
+ #define 32
+#endif
+
 #if defined(FILAMENT_RUNOUT_SENSOR)
   // define digital pin 4 for the filament runout sensor. Use the RAMPS 1.4 digital input 4 on the servos connector
   #define FILRUNOUT_PIN        4
diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index 4241111d78..c246e5e52e 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -527,22 +527,15 @@ ISR(TIMER1_COMPA_vect) {
 
         #if defined(Z_PROBE_PIN) && Z_PROBE_PIN > -1
           UPDATE_ENDSTOP(z, Z, probe, PROBE);
-          bool z_probe_endstop(READ(Z_PROBE_PIN) != Z_MIN_ENDSTOP_INVERTING);
+          z_probe_endstop=(READ(Z_PROBE_PIN) != Z_MIN_ENDSTOP_INVERTING);
           if(z_probe_endstop && old_z_probe_endstop)
           {
         	  endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
         	  endstop_z_hit=true;
 
 //        	  if (z_probe_endstop && old_z_probe_endstop) SERIAL_ECHOLN("z_probe_endstop = true");
-
-
-        	  if (!(performing_homing)) //if not performing home
-        	  {
-        		  step_events_completed = current_block->step_event_count;
-        	  }
           }
           old_z_probe_endstop = z_probe_endstop;
-          old_z2_probe_endstop = z2_probe_endstop;
         #endif
       }
     }
@@ -581,22 +574,14 @@ ISR(TIMER1_COMPA_vect) {
 
         #if defined(Z_PROBE_PIN) && Z_PROBE_PIN > -1
           UPDATE_ENDSTOP(z, Z, probe, PROBE);
-          bool z_probe_endstop(READ(Z_PROBE_PIN) != Z_MAX_ENDSTOP_INVERTING);
+          z_probe_endstop=(READ(Z_PROBE_PIN) != Z_MAX_ENDSTOP_INVERTING);
           if(z_probe_endstop && old_z_probe_endstop)
           {
         	  endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
         	  endstop_z_hit=true;
-
 //        	  if (z_probe_endstop && old_z_probe_endstop) SERIAL_ECHOLN("z_probe_endstop = true");
-
-
-        	  if (!(performing_homing)) //if not performing home
-        	  {
-        		  step_events_completed = current_block->step_event_count;
-        	  }
           }
           old_z_probe_endstop = z_probe_endstop;
-          old_z2_probe_endstop = z2_probe_endstop;
         #endif
       }
     }

From 6125124d6c3e76d7826f81355779c9d0e958dce2 Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Sun, 29 Mar 2015 02:29:16 -0500
Subject: [PATCH 09/37] Config file cleanup for Z_PROBE_AND_ENDSTOP support.

---
 Marlin/Configuration.h | 37 ++++++++++++++-----------------------
 1 file changed, 14 insertions(+), 23 deletions(-)

diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index b23864065c..e2b0bfc37f 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -309,6 +309,7 @@ your extruder heater takes 2 minutes to hit the target on heating.
 #define ENDSTOPPULLUP_XMIN
 #define ENDSTOPPULLUP_YMIN
 #define ENDSTOPPULLUP_ZMIN
+#define ENDSTOPPULLUP_ZPROBE
 
 // Mechanical endstop with COM to ground and NC to Signal uses "false" here (most common setup).
 const bool X_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic of the endstop.
@@ -317,9 +318,15 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 const bool X_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic of the endstop.
 const bool Y_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic of the endstop.
 const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic of the endstop.
+const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic of the endstop.
 
 //#define DISABLE_MAX_ENDSTOPS
 //#define DISABLE_MIN_ENDSTOPS
+// If you want to enable the Z Probe pin, but disable its use, uncomment the line below.
+// This only affects a Z Probe Endstop if you have separate Z min endstop as well and have
+// activated Z_PROBE_AND_ENDSTOP below. If you are using the Z Min endstop on your Z Probe,
+// this has no effect.
+//#define DISABLE_Z_PROBE_ENDSTOP
 
 // For Inverting Stepper Enable Pins (Active Low) use 0, Non Inverting (Active High) use 1
 #define X_ENABLE_ON 0
@@ -484,33 +491,17 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 
   #endif
 
-// Z Probe and Z endstop support
+// Seaparte concurrent Z Probe and Z min endstop support
 // Added by Chris Roadfeldt 3-28-2015
-// If you have a Z Probe in addition to endstop(s) for Z Homing, uncomment the #define Z_PROBE_AND_ENDSTOP line below and configure Z Probe settings.
-// Only use this if you have both a Z PROBE and Z HOMING ENDSTOP(S). If you are using Z_SAFE_HOMING above, then you probably don't need this unless you want to make use of
-// a non-default pin for your Z Probe.
-// Note: It's expected that your Z Probe triggers in the direction towards your bed. If your Z Probe does not trigger when traveling towards you bed, it will trigger when it's moving
-// away from the bed.
-
-//  #define Z_PROBE_AND_ENDSTOP
-
-  #ifdef Z_PROBE_AND_ENDSTOP
-
-// As of 3-28-2015, there are NO Z Probe pins defined in any board config files.
-// Z_PROBE_PIN is for the signal pin only. RAMPS 1.3/1.4 boards may be able to use the 5V, Ground and the D32 pin in the Aux 4 section of the RAMPS board for the signal.
+// If you have a Z Probe in addition to a Z min endstop, uncomment the #define Z_PROBE_AND_ENDSTOP line below and configure Z Probe settings.
+// Only use this if you have both a Z PROBE and Z Min ENDSTOP.
+// Note: It's expected that your Z Probe triggers in the direction towards your bed.
+// In order to use this, you must have a Z_PROBE_PIN defined in the pins.h file for your control board.
+// RAMPS 1.3/1.4 boards may be able to use the 5V, Ground and the D32 pin in the Aux 4 section of the RAMPS board for the signal.
 // The D32 pin in Aux 4 on RAMPS maps to the Arduino D32 pin. The Z_PROBE_PIN is setting the pin to use on the Arduino. Since the D32 pin on the RAMPS maps to D32 on Arduino, this works.
 // D32 is currently selected in the RAMPS 1.3/1.4 pin file. Update the pins.h file for your control board to make use of this. Not doing so nullifies Z_PROBE_AND_ENDSTOP
 // WARNING: Setting the wrong pin may have unexpected and disastrous outcomes. Use with caution and do your homework.
-
-// Mechanical endstop with COM to ground and NC to Signal uses "false" here (most common setup).
-    const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic of the endstop.
-
-// The pullups are needed if you directly connect a mechanical endstop between the signal and ground pins.
-    #define ENDSTOPPULLUP_ZPROBE
-
-// If you want to enable the Z Probe pin, but disable its use, uncomment the line below.
-//    #define DISABLE_Z_PROBE_ENDSTOP
-  #endif
+//  #define Z_PROBE_AND_ENDSTOP
 
 #endif // ENABLE_AUTO_BED_LEVELING
 

From a98ac4033b605e622be3f0e35039b1bdd95c0466 Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Sun, 29 Mar 2015 02:36:26 -0500
Subject: [PATCH 10/37] Added Z_PROBE_PIN to #define for Z_PROBE_AND_ENDSTOP
 support.

---
 Marlin/pins_RAMPS_13.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Marlin/pins_RAMPS_13.h b/Marlin/pins_RAMPS_13.h
index 4bd2bce468..173310031c 100644
--- a/Marlin/pins_RAMPS_13.h
+++ b/Marlin/pins_RAMPS_13.h
@@ -64,7 +64,7 @@
 
 #if defined(Z_PROBE_ANDENDSTOP)
   // Define a pin to use as the signal pin on Arduino for the Z_PROBE endstop.
- #define 32
+ #define Z_PROBE_PIN 32
 #endif
 
 #if defined(FILAMENT_RUNOUT_SENSOR)

From c75a5e8c551985cbb5aabc4e4475be6f54782c12 Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Sun, 29 Mar 2015 02:38:08 -0500
Subject: [PATCH 11/37] Fixed typo for Z_PROBE_AND_ENDSTOP support in
 pins_RAMPS.h. Confirmed Sanity checks are working.....

---
 Marlin/pins_RAMPS_13.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Marlin/pins_RAMPS_13.h b/Marlin/pins_RAMPS_13.h
index 173310031c..3ca12dd156 100644
--- a/Marlin/pins_RAMPS_13.h
+++ b/Marlin/pins_RAMPS_13.h
@@ -62,7 +62,7 @@
   #define FILWIDTH_PIN        5
 #endif
 
-#if defined(Z_PROBE_ANDENDSTOP)
+#if defined(Z_PROBE_AND_ENDSTOP)
   // Define a pin to use as the signal pin on Arduino for the Z_PROBE endstop.
  #define Z_PROBE_PIN 32
 #endif

From 05134f0807a1a3be6306947e5cf5a1c567522c5e Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Sun, 29 Mar 2015 02:51:51 -0500
Subject: [PATCH 12/37] Additional Sanity Checks for Z_PROBE_AND_ENDSTOP

---
 Marlin/SanityCheck.h | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h
index 5a24cbc1ce..cd8ef97c9a 100644
--- a/Marlin/SanityCheck.h
+++ b/Marlin/SanityCheck.h
@@ -106,8 +106,16 @@
       #if Z_PROBE_PIN == -1
         #error You must have a Z_PROBE_PIN defined if you enable Z_PROBE_AND_ENDSTOP
       #endif
+      #ifndef (NUM_SERVOS)
+        #error You must have NUM_SERVOS defined and there must be at least 1 configured to use Z_PROBE_AND_ENDSTOP
+      #endif
+      #if defined(NUM_SERVOS) && NUM_SERVOS < 1
+        #error You must have at least 1 servo defined for NUM_SERVOS to use Z_PROBE_AND_ENDSTOP
+      #endif
+      #ifndef SERVO_ENDSTOPS
+        #error You must have SERVO_ENDSTOPS defined and have the Z index set to at least 1 to use Z_PROBE_AND_ENDSTOP
+      #endif
     #endif
-
     /**
      * Check if Probe_Offset * Grid Points is greater than Probing Range
      */

From 82d2f111299f7c48be64b6f5830fc17cd2a0873d Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Sun, 29 Mar 2015 02:53:55 -0500
Subject: [PATCH 13/37] More additional sanity checks for Z_PROBE_AND_ENDSTOP

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

diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h
index cd8ef97c9a..8c60415fa8 100644
--- a/Marlin/SanityCheck.h
+++ b/Marlin/SanityCheck.h
@@ -115,6 +115,9 @@
       #ifndef SERVO_ENDSTOPS
         #error You must have SERVO_ENDSTOPS defined and have the Z index set to at least 1 to use Z_PROBE_AND_ENDSTOP
       #endif
+      #ifndef SERVO_ENDSTOP_ANGLES
+        #error You must have SERVO_ENDSTOP_ANGLES defined for Z Extend and Retract to use Z_PROBE_AND_ENSTOP
+      #endif
     #endif
     /**
      * Check if Probe_Offset * Grid Points is greater than Probing Range

From fe4549cd834e1b6b4901b74e163c2f8c12bf09fb Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Sun, 29 Mar 2015 03:04:59 -0500
Subject: [PATCH 14/37] Config file Z_PROBE_AND_ENDSTOP description clean up.

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

diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index e2b0bfc37f..6d76a8ee41 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -491,14 +491,13 @@ const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic
 
   #endif
 
-// Seaparte concurrent Z Probe and Z min endstop support
+// Support for concurrent and seperate Z Probe and Z min endstop use.
 // Added by Chris Roadfeldt 3-28-2015
-// If you have a Z Probe in addition to a Z min endstop, uncomment the #define Z_PROBE_AND_ENDSTOP line below and configure Z Probe settings.
-// Only use this if you have both a Z PROBE and Z Min ENDSTOP.
-// Note: It's expected that your Z Probe triggers in the direction towards your bed.
+// If you would like to use both a Z Probe and a Z min endstop at the same time, uncomment #define Z_PROBE_AND_ENDSTOP below
+// You will want to disable Z_SAFE_HOMING above as you will still use the Z min endstop for homing.
 // In order to use this, you must have a Z_PROBE_PIN defined in the pins.h file for your control board.
 // RAMPS 1.3/1.4 boards may be able to use the 5V, Ground and the D32 pin in the Aux 4 section of the RAMPS board for the signal.
-// The D32 pin in Aux 4 on RAMPS maps to the Arduino D32 pin. The Z_PROBE_PIN is setting the pin to use on the Arduino. Since the D32 pin on the RAMPS maps to D32 on Arduino, this works.
+// The D32 pin in Aux 4 on RAMPS maps to the Arduino D32 pin. Z_PROBE_PIN is setting the pin to use on the Arduino. Since the D32 pin on the RAMPS maps to D32 on Arduino, this works.
 // D32 is currently selected in the RAMPS 1.3/1.4 pin file. Update the pins.h file for your control board to make use of this. Not doing so nullifies Z_PROBE_AND_ENDSTOP
 // WARNING: Setting the wrong pin may have unexpected and disastrous outcomes. Use with caution and do your homework.
 //  #define Z_PROBE_AND_ENDSTOP

From 80285251cccf1464b39469122e26804e23a96efb Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Sun, 29 Mar 2015 03:06:20 -0500
Subject: [PATCH 15/37] More config cleanup work for Z_PROBE_AND_ENDSTOP.

---
 Marlin/Configuration.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index 6d76a8ee41..29ea48c592 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -500,6 +500,7 @@ const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic
 // The D32 pin in Aux 4 on RAMPS maps to the Arduino D32 pin. Z_PROBE_PIN is setting the pin to use on the Arduino. Since the D32 pin on the RAMPS maps to D32 on Arduino, this works.
 // D32 is currently selected in the RAMPS 1.3/1.4 pin file. Update the pins.h file for your control board to make use of this. Not doing so nullifies Z_PROBE_AND_ENDSTOP
 // WARNING: Setting the wrong pin may have unexpected and disastrous outcomes. Use with caution and do your homework.
+
 //  #define Z_PROBE_AND_ENDSTOP
 
 #endif // ENABLE_AUTO_BED_LEVELING

From 08a7aa16c548951c0be7f7b5a4956f62768447f9 Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Sun, 29 Mar 2015 03:21:48 -0500
Subject: [PATCH 16/37] Don't deploy Z Probe on Z homing if Z_PROBE_AND_ENDSTOP
 is defined.

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

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 87ec4792cd..a020c6e0d5 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -1493,18 +1493,20 @@ static void homeaxis(int axis) {
 
 
 #ifndef Z_PROBE_SLED
-    // Engage Servo endstop if enabled
-    #ifdef SERVO_ENDSTOPS
-      #if SERVO_LEVELING
+    // Engage Servo endstop if enabled and we are not using Z_PROBE_AND_ENDSTOP
+    #ifndef Z_PROBE_AND_ENDSTOP
+      #ifdef SERVO_ENDSTOPS
+        #if SERVO_LEVELING
         if (axis==Z_AXIS) {
           engage_z_probe();
         }
       else
-      #endif
+        #endif
       if (servo_endstops[axis] > -1) {
         servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2]);
       }
-    #endif
+      #endif
+    #endif // Z_PROBE_AND_ENDSTOP
 #endif // Z_PROBE_SLED
     #ifdef Z_DUAL_ENDSTOPS
       if (axis==Z_AXIS) In_Homing_Process(true);
@@ -1922,10 +1924,12 @@ inline void gcode_G28() {
 
         if (home_all_axis || code_seen(axis_codes[Z_AXIS])) {
           #if defined(Z_RAISE_BEFORE_HOMING) && Z_RAISE_BEFORE_HOMING > 0
+            #ifndef Z_PROBE_AND_ENDSTOP
             destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS);    // Set destination away from bed
             feedrate = max_feedrate[Z_AXIS];
             plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
             st_synchronize();
+            #endif
           #endif
           HOMEAXIS(Z);
         }

From 666fad349471c7d1d3b5556d30497966df5f7104 Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Sun, 29 Mar 2015 03:25:03 -0500
Subject: [PATCH 17/37] Typo fixed in sanity check.

---
 Marlin/SanityCheck.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h
index 8c60415fa8..11d4d84645 100644
--- a/Marlin/SanityCheck.h
+++ b/Marlin/SanityCheck.h
@@ -106,7 +106,7 @@
       #if Z_PROBE_PIN == -1
         #error You must have a Z_PROBE_PIN defined if you enable Z_PROBE_AND_ENDSTOP
       #endif
-      #ifndef (NUM_SERVOS)
+      #ifndef NUM_SERVOS
         #error You must have NUM_SERVOS defined and there must be at least 1 configured to use Z_PROBE_AND_ENDSTOP
       #endif
       #if defined(NUM_SERVOS) && NUM_SERVOS < 1

From 62834a1c43ad20fe815d52fef1d3c1944b81a609 Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Sun, 29 Mar 2015 03:55:41 -0500
Subject: [PATCH 18/37] Don't deploy probe on Z Axis homing if
 Z_PROBE_AND_ENDSTOP is enabled, unless Z_SAFE_HOMING is.

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

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index a020c6e0d5..520d4a6a50 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -1493,20 +1493,18 @@ static void homeaxis(int axis) {
 
 
 #ifndef Z_PROBE_SLED
-    // Engage Servo endstop if enabled and we are not using Z_PROBE_AND_ENDSTOP
-    #ifndef Z_PROBE_AND_ENDSTOP
-      #ifdef SERVO_ENDSTOPS
-        #if SERVO_LEVELING
+    // Engage Servo endstop if enabled and we are not using Z_PROBE_AND_ENDSTOP unless we are using Z_SAFE_HOMING
+    #ifdef SERVO_ENDSTOPS && (defined (Z_SAFE_HOMING) || ! defined (Z_PROBE_AND_ENDSTOP))
+      #if SERVO_LEVELING
         if (axis==Z_AXIS) {
           engage_z_probe();
         }
       else
-        #endif
+      #endif
       if (servo_endstops[axis] > -1) {
         servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2]);
       }
-      #endif
-    #endif // Z_PROBE_AND_ENDSTOP
+    #endif
 #endif // Z_PROBE_SLED
     #ifdef Z_DUAL_ENDSTOPS
       if (axis==Z_AXIS) In_Homing_Process(true);

From a9802c95b32eac41b35b520db299fabf7a88e205 Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Sun, 29 Mar 2015 04:18:37 -0500
Subject: [PATCH 19/37] Ensure Z_PROBE_PIN is defined if Z_PROBE_AND_ENDSTOP
 is.

---
 Marlin/SanityCheck.h | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h
index 11d4d84645..1427da2127 100644
--- a/Marlin/SanityCheck.h
+++ b/Marlin/SanityCheck.h
@@ -103,8 +103,11 @@
      * Require a Z Probe Pin if Z_PROBE_AND_ENDSTOP is enabled.
      */
     #if defined(Z_PROBE_AND_ENDSTOP)
+      #ifndef Z_PROBE_PIN
+        #error You must have a Z_PROBE_PIN defined in your pins_XXXX.h file if you enable Z_PROBE_AND_ENDSTOP
+      #endif
       #if Z_PROBE_PIN == -1
-        #error You must have a Z_PROBE_PIN defined if you enable Z_PROBE_AND_ENDSTOP
+        #error You must set Z_PROBE_PIN to a valid pin if you enable Z_PROBE_AND_ENDSTOP
       #endif
       #ifndef NUM_SERVOS
         #error You must have NUM_SERVOS defined and there must be at least 1 configured to use Z_PROBE_AND_ENDSTOP

From 992b07ca5741fe179670d521f9e3912e720debf8 Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Mon, 30 Mar 2015 22:58:10 -0500
Subject: [PATCH 20/37] Don't disable Z_RAISE_BEFORE_HOMING when
 Z_PROBE_AND_ENDSTOP is enabled.

---
 Marlin/Marlin_main.cpp | 2 --
 1 file changed, 2 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 0f04453523..d4c454ba3b 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -2032,12 +2032,10 @@ inline void gcode_G28() {
         if (home_all_axis || homeZ) {
           // Raise Z before homing Z? Shouldn't this happen before homing X or Y?
           #if defined(Z_RAISE_BEFORE_HOMING) && Z_RAISE_BEFORE_HOMING > 0
-            #ifndef Z_PROBE_AND_ENDSTOP
             destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS);    // Set destination away from bed
             feedrate = max_feedrate[Z_AXIS];
             line_to_destination();
             st_synchronize();
-            #endif
           #endif
           HOMEAXIS(Z);
         }

From e08f8eed0581bb91c886ffbf7e1234ef58935501 Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Mon, 30 Mar 2015 23:51:36 -0500
Subject: [PATCH 21/37] Revert 06f767d..cba5692

This rolls back to commit 06f767d608120f09bcd0fd0aee582220cd8657d9.
---
 Marlin/Marlin.h                               |   1 +
 Marlin/Marlin_main.cpp                        | 869 +++++++-----------
 Marlin/dogm_lcd_implementation.h              |   2 +-
 Marlin/planner.cpp                            |   4 +-
 Marlin/stepper.cpp                            | 115 +--
 Marlin/ultralcd.cpp                           |   2 +-
 .../ultralcd_implementation_hitachi_HD44780.h |  16 +-
 7 files changed, 409 insertions(+), 600 deletions(-)

diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h
index 45a94e82e3..46720d9a34 100644
--- a/Marlin/Marlin.h
+++ b/Marlin/Marlin.h
@@ -229,6 +229,7 @@ void refresh_cmd_timeout(void);
 extern float homing_feedrate[];
 extern bool axis_relative_modes[];
 extern int feedmultiply;
+extern int extrudemultiply; // Sets extrude multiply factor (in percent) for all extruders
 extern bool volumetric_enabled;
 extern int extruder_multiply[EXTRUDERS]; // sets extrude multiply factor (in percent) for each extruder individually
 extern float filament_size[EXTRUDERS]; // cross-sectional area of filament (in millimeters), typically around 1.75 or 2.85, 0 disables the volumetric calculations for the extruder.
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 0f04453523..0096206246 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -79,7 +79,7 @@
 // G4  - Dwell S<seconds> or P<milliseconds>
 // G10 - retract filament according to settings of M207
 // G11 - retract recover filament according to settings of M208
-// G28 - Home one or more axes
+// G28 - Home all Axis
 // G29 - Detailed Z-Probe, probes the bed at 3 or more points.  Will fail if you haven't homed yet.
 // G30 - Single Z Probe, probes bed at current XY location.
 // G31 - Dock sled (Z_PROBE_SLED only)
@@ -210,6 +210,7 @@ int homing_bump_divisor[] = HOMING_BUMP_DIVISOR;
 bool axis_relative_modes[] = AXIS_RELATIVE_MODES;
 int feedmultiply = 100; //100->1 200->2
 int saved_feedmultiply;
+int extrudemultiply = 100; //100->1 200->2
 int extruder_multiply[EXTRUDERS] = ARRAY_BY_EXTRUDERS(100, 100, 100, 100);
 bool volumetric_enabled = false;
 float filament_size[EXTRUDERS] = ARRAY_BY_EXTRUDERS(DEFAULT_NOMINAL_FILAMENT_DIA, DEFAULT_NOMINAL_FILAMENT_DIA, DEFAULT_NOMINAL_FILAMENT_DIA, DEFAULT_NOMINAL_FILAMENT_DIA);
@@ -305,7 +306,7 @@ int fanSpeed = 0;
 #ifdef SCARA
   float axis_scaling[3] = { 1, 1, 1 };    // Build size scaling, default to 1
   static float delta[3] = { 0, 0, 0 };		
-#endif
+#endif        
 
 bool cancel_heatup = false;
 
@@ -476,6 +477,8 @@ bool enquecommand(const char *cmd)
   return true;
 }
 
+
+
 void setup_killpin()
 {
   #if defined(KILL_PIN) && KILL_PIN > -1
@@ -898,7 +901,7 @@ bool code_seen(char code) {
   strchr_pointer = strchr(cmdbuffer[bufindr], code);
   return (strchr_pointer != NULL);  //Return True if a character was found
 }
-  
+
 #define DEFINE_PGM_READ_ANY(type, reader)       \
     static inline type pgm_read_any(const type *p)  \
     { return pgm_read_##reader##_near(p); }
@@ -929,7 +932,7 @@ XYZ_CONSTS_FROM_CONFIG(signed char, home_dir,  HOME_DIR);
 
   static float x_home_pos(int extruder) {
     if (extruder == 0)
-      return base_home_pos(X_AXIS) + home_offset[X_AXIS];
+    return base_home_pos(X_AXIS) + home_offset[X_AXIS];
     else
       // In dual carriage mode the extruder offset provides an override of the
       // second X-carriage offset when homed - otherwise X2_HOME_POS is used.
@@ -958,15 +961,15 @@ static void axis_is_at_home(int axis) {
     if (axis == X_AXIS) {
       if (active_extruder != 0) {
         current_position[X_AXIS] = x_home_pos(active_extruder);
-                 min_pos[X_AXIS] = X2_MIN_POS;
-                 max_pos[X_AXIS] = max(extruder_offset[1][X_AXIS], X2_MAX_POS);
+        min_pos[X_AXIS] = X2_MIN_POS;
+        max_pos[X_AXIS] = max(extruder_offset[1][X_AXIS], X2_MAX_POS);
         return;
       }
       else if (dual_x_carriage_mode == DXC_DUPLICATION_MODE) {
-        float xoff = home_offset[X_AXIS];
-        current_position[X_AXIS] = base_home_pos(X_AXIS) + xoff;
-                 min_pos[X_AXIS] = base_min_pos(X_AXIS) + xoff;
-                 max_pos[X_AXIS] = min(base_max_pos(X_AXIS) + xoff, max(extruder_offset[1][X_AXIS], X2_MAX_POS) - duplicate_extruder_x_offset);
+        current_position[X_AXIS] = base_home_pos(X_AXIS) + home_offset[X_AXIS];
+        min_pos[X_AXIS] = base_min_pos(X_AXIS) + home_offset[X_AXIS];
+        max_pos[X_AXIS] = min(base_max_pos(X_AXIS) + home_offset[X_AXIS],
+                                max(extruder_offset[1][X_AXIS], X2_MAX_POS) - duplicate_extruder_x_offset);
         return;
       }
     }
@@ -1020,189 +1023,178 @@ static void axis_is_at_home(int axis) {
 }
 
 /**
- * Some planner shorthand inline functions
+ * Shorthand to tell the planner our current position (in mm).
  */
-inline void line_to_current_position() {
-  plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate/60, active_extruder);
-}
-inline void line_to_z(float zPosition) {
-  plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
-}
-inline void line_to_destination() {
-  plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
-}
 inline void sync_plan_position() {
   plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
 }
 
 #ifdef ENABLE_AUTO_BED_LEVELING
+#ifdef AUTO_BED_LEVELING_GRID
 
-  #ifdef AUTO_BED_LEVELING_GRID
+#ifndef DELTA
+  static void set_bed_level_equation_lsq(double *plane_equation_coefficients) {
+    vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1);
+    planeNormal.debug("planeNormal");
+    plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
+    //bedLevel.debug("bedLevel");
 
-    #ifndef DELTA
+    //plan_bed_level_matrix.debug("bed level before");
+    //vector_3 uncorrected_position = plan_get_position_mm();
+    //uncorrected_position.debug("position before");
 
-      static void set_bed_level_equation_lsq(double *plane_equation_coefficients) {
-        vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1);
-        planeNormal.debug("planeNormal");
-        plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
-        //bedLevel.debug("bedLevel");
+    vector_3 corrected_position = plan_get_position();
+    //corrected_position.debug("position after");
+    current_position[X_AXIS] = corrected_position.x;
+    current_position[Y_AXIS] = corrected_position.y;
+    current_position[Z_AXIS] = zprobe_zoffset; // was: corrected_position.z
 
-        //plan_bed_level_matrix.debug("bed level before");
-        //vector_3 uncorrected_position = plan_get_position_mm();
-        //uncorrected_position.debug("position before");
+    sync_plan_position();
+  }
+#endif
 
-        vector_3 corrected_position = plan_get_position();
-        //corrected_position.debug("position after");
-        current_position[X_AXIS] = corrected_position.x;
-        current_position[Y_AXIS] = corrected_position.y;
-        current_position[Z_AXIS] = zprobe_zoffset; // was: corrected_position.z
+#else // not AUTO_BED_LEVELING_GRID
 
-        sync_plan_position();
-      }
+static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float z_at_pt_3) {
 
-    #endif // !DELTA
+    plan_bed_level_matrix.set_to_identity();
 
-  #else // !AUTO_BED_LEVELING_GRID
+    vector_3 pt1 = vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, z_at_pt_1);
+    vector_3 pt2 = vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, z_at_pt_2);
+    vector_3 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();
 
-    static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float z_at_pt_3) {
-
-      plan_bed_level_matrix.set_to_identity();
-
-      vector_3 pt1 = vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, z_at_pt_1);
-      vector_3 pt2 = vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, z_at_pt_2);
-      vector_3 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();
-
-      if (planeNormal.z < 0) {
-        planeNormal.x = -planeNormal.x;
-        planeNormal.y = -planeNormal.y;
-        planeNormal.z = -planeNormal.z;
-      }
-
-      plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
-
-      vector_3 corrected_position = plan_get_position();
-      current_position[X_AXIS] = corrected_position.x;
-      current_position[Y_AXIS] = corrected_position.y;
-      current_position[Z_AXIS] = zprobe_zoffset; // was: corrected_position.z
-
-      sync_plan_position();
+    if (planeNormal.z < 0) {
+      planeNormal.x = -planeNormal.x;
+      planeNormal.y = -planeNormal.y;
+      planeNormal.z = -planeNormal.z;
     }
 
-  #endif // !AUTO_BED_LEVELING_GRID
+    plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
 
-  static void run_z_probe() {
+    vector_3 corrected_position = plan_get_position();
+    current_position[X_AXIS] = corrected_position.x;
+    current_position[Y_AXIS] = corrected_position.y;
+    current_position[Z_AXIS] = zprobe_zoffset; // was: corrected_position.z
 
-    #ifdef DELTA
+    sync_plan_position();
+}
+
+#endif // AUTO_BED_LEVELING_GRID
+
+static void run_z_probe() {
+  #ifdef DELTA
     
-      float start_z = current_position[Z_AXIS];
-      long start_steps = st_get_position(Z_AXIS);
+    float start_z = current_position[Z_AXIS];
+    long start_steps = st_get_position(Z_AXIS);
+  
+    // move down slowly until you find the bed
+    feedrate = homing_feedrate[Z_AXIS] / 4;
+    destination[Z_AXIS] = -10;
+    prepare_move_raw();
+    st_synchronize();
+    endstops_hit_on_purpose();
     
-      // move down slowly until you find the bed
-      feedrate = homing_feedrate[Z_AXIS] / 4;
-      destination[Z_AXIS] = -10;
-      prepare_move_raw();
-      st_synchronize();
-      endstops_hit_on_purpose();
-      
-      // we have to let the planner know where we are right now as it is not where we said to go.
-      long stop_steps = st_get_position(Z_AXIS);
-      float mm = start_z - float(start_steps - stop_steps) / axis_steps_per_unit[Z_AXIS];
-      current_position[Z_AXIS] = mm;
-      calculate_delta(current_position);
-      plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
-      
-    #else // !DELTA
+    // we have to let the planner know where we are right now as it is not where we said to go.
+    long stop_steps = st_get_position(Z_AXIS);
+    float mm = start_z - float(start_steps - stop_steps) / axis_steps_per_unit[Z_AXIS];
+    current_position[Z_AXIS] = mm;
+    calculate_delta(current_position);
+    plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
+    
+  #else
 
-      plan_bed_level_matrix.set_to_identity();
-      feedrate = homing_feedrate[Z_AXIS];
+    plan_bed_level_matrix.set_to_identity();
+    feedrate = homing_feedrate[Z_AXIS];
 
-      // move down until you find the bed
-      float zPosition = -10;
-      line_to_z(zPosition);
-      st_synchronize();
+    // move down until you find the bed
+    float zPosition = -10;
+    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
+    st_synchronize();
 
-      // we have to let the planner know where we are right now as it is not where we said to go.
-      zPosition = st_get_position_mm(Z_AXIS);
-      plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS]);
+        // we have to let the planner know where we are right now as it is not where we said to go.
+    zPosition = st_get_position_mm(Z_AXIS);
+    plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS]);
 
-      // move up the retract distance
-      zPosition += home_retract_mm(Z_AXIS);
-      line_to_z(zPosition);
-      st_synchronize();
-      endstops_hit_on_purpose();
+    // move up the retract distance
+    zPosition += home_retract_mm(Z_AXIS);
+    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
+    st_synchronize();
+    endstops_hit_on_purpose();
 
-      // move back down slowly to find bed
-      if (homing_bump_divisor[Z_AXIS] >= 1)
-        feedrate = homing_feedrate[Z_AXIS] / homing_bump_divisor[Z_AXIS];
-      else {
-        feedrate = homing_feedrate[Z_AXIS] / 10;
-        SERIAL_ECHOLN("Warning: The Homing Bump Feedrate Divisor cannot be less than 1");
-      }
+    // move back down slowly to find bed
+    if (homing_bump_divisor[Z_AXIS] >= 1) {
+      feedrate = homing_feedrate[Z_AXIS]/homing_bump_divisor[Z_AXIS];
+    } 
+    else {
+      feedrate = homing_feedrate[Z_AXIS]/10;
+      SERIAL_ECHOLN("Warning: The Homing Bump Feedrate Divisor cannot be less then 1");
+    }
 
-      zPosition -= home_retract_mm(Z_AXIS) * 2;
-      line_to_z(zPosition);
-      st_synchronize();
-      endstops_hit_on_purpose();
+    zPosition -= home_retract_mm(Z_AXIS) * 2;
+    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
+    st_synchronize();
+    endstops_hit_on_purpose();
 
-      current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
-      // make sure the planner knows where we are as it may be a bit different than we last said to move to
-      sync_plan_position();
-      
-    #endif // !DELTA
-  }
+    current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
+    // make sure the planner knows where we are as it may be a bit different than we last said to move to
+    sync_plan_position();
+    
+  #endif
+}
 
-  static void do_blocking_move_to(float x, float y, float z) {
+static void do_blocking_move_to(float x, float y, float z) {
     float oldFeedRate = feedrate;
 
-    #ifdef DELTA
+#ifdef DELTA
 
-      feedrate = XY_TRAVEL_SPEED;
-      
-      destination[X_AXIS] = x;
-      destination[Y_AXIS] = y;
-      destination[Z_AXIS] = z;
-      prepare_move_raw();
-      st_synchronize();
+    feedrate = XY_TRAVEL_SPEED;
+    
+    destination[X_AXIS] = x;
+    destination[Y_AXIS] = y;
+    destination[Z_AXIS] = z;
+    prepare_move_raw();
+    st_synchronize();
 
-    #else
+#else
 
-      feedrate = homing_feedrate[Z_AXIS];
+    feedrate = homing_feedrate[Z_AXIS];
 
-      current_position[Z_AXIS] = z;
-      line_to_current_position();
-      st_synchronize();
+    current_position[Z_AXIS] = z;
+    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate/60, active_extruder);
+    st_synchronize();
 
-      feedrate = xy_travel_speed;
+    feedrate = xy_travel_speed;
 
-      current_position[X_AXIS] = x;
-      current_position[Y_AXIS] = y;
-      line_to_current_position();
-      st_synchronize();
+    current_position[X_AXIS] = x;
+    current_position[Y_AXIS] = y;
+    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate/60, active_extruder);
+    st_synchronize();
 
-    #endif
+#endif
 
     feedrate = oldFeedRate;
-  }
+}
 
-  static void setup_for_endstop_move() {
+static void setup_for_endstop_move() {
     saved_feedrate = feedrate;
     saved_feedmultiply = feedmultiply;
     feedmultiply = 100;
     previous_millis_cmd = millis();
-    enable_endstops(true);
-  }
 
-  static void clean_up_after_endstop_move() {
-    #ifdef ENDSTOPS_ONLY_FOR_HOMING
-      enable_endstops(false);
-    #endif
+    enable_endstops(true);
+}
+
+static void clean_up_after_endstop_move() {
+#ifdef ENDSTOPS_ONLY_FOR_HOMING
+    enable_endstops(false);
+#endif
+
     feedrate = saved_feedrate;
     feedmultiply = saved_feedmultiply;
     previous_millis_cmd = millis();
-  }
+}
 
-<<<<<<< HEAD
 static void engage_z_probe() {
   // Engage Z Servo endstop if enabled
   #ifdef SERVO_ENDSTOPS
@@ -1250,59 +1242,13 @@ static void engage_z_probe() {
             SERIAL_ERROR_START;
             SERIAL_ERRORLNPGM("Z-Probe failed to engage!");
             LCD_ALERTMESSAGEPGM("Err: ZPROBE");
-=======
-  static void engage_z_probe() {
-
-    #ifdef SERVO_ENDSTOPS
-
-      // Engage Z Servo endstop if enabled
-      if (servo_endstops[Z_AXIS] >= 0) {
-        #if SERVO_LEVELING
-          servos[servo_endstops[Z_AXIS]].attach(0);
-        #endif
-        servos[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2]);
-        #if SERVO_LEVELING
-          delay(PROBE_SERVO_DEACTIVATION_DELAY);
-          servos[servo_endstops[Z_AXIS]].detach();
-        #endif
-      }
-
-    #elif defined(Z_PROBE_ALLEN_KEY)
-
-      feedrate = homing_feedrate[X_AXIS];
-
-      // Move to the start position to initiate deployment
-      destination[X_AXIS] = Z_PROBE_ALLEN_KEY_DEPLOY_X;
-      destination[Y_AXIS] = Z_PROBE_ALLEN_KEY_DEPLOY_Y;
-      destination[Z_AXIS] = Z_PROBE_ALLEN_KEY_DEPLOY_Z;
-      prepare_move_raw();
-
-      // Home X to touch the belt
-      feedrate = homing_feedrate[X_AXIS]/10;
-      destination[X_AXIS] = 0;
-      prepare_move_raw();
-      
-      // Home Y for safety
-      feedrate = homing_feedrate[X_AXIS]/2;
-      destination[Y_AXIS] = 0;
-      prepare_move_raw();
-      
-      st_synchronize();
-      
-      bool z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
-      if (z_min_endstop) {
-        if (!Stopped) {
-          SERIAL_ERROR_START;
-          SERIAL_ERRORLNPGM("Z-Probe failed to engage!");
-          LCD_ALERTMESSAGEPGM("Err: ZPROBE");
->>>>>>> MarlinFirmware/Development
         }
         Stop();
-      }
+    }
+  #endif
 
-    #endif // Z_PROBE_ALLEN_KEY
+}
 
-<<<<<<< HEAD
 static void retract_z_probe() {
   // Retract Z Servo endstop if enabled
   #ifdef SERVO_ENDSTOPS
@@ -1365,216 +1311,126 @@ static void retract_z_probe() {
             SERIAL_ERROR_START;
             SERIAL_ERRORLNPGM("Z-Probe failed to retract!");
             LCD_ALERTMESSAGEPGM("Err: ZPROBE");
-=======
-  }
-
-  static void retract_z_probe(const float z_after=Z_RAISE_AFTER_PROBING) {
-
-    #ifdef SERVO_ENDSTOPS
-
-      // Retract Z Servo endstop if enabled
-      if (servo_endstops[Z_AXIS] >= 0) {
-
-        if (z_after > 0) {
-          do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_after);
-          st_synchronize();
->>>>>>> MarlinFirmware/Development
-        }
-      
-        #if SERVO_LEVELING
-          servos[servo_endstops[Z_AXIS]].attach(0);
-        #endif
-
-        servos[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2 + 1]);
-
-        #if SERVO_LEVELING
-          delay(PROBE_SERVO_DEACTIVATION_DELAY);
-          servos[servo_endstops[Z_AXIS]].detach();
-        #endif
-      }
-
-    #elif defined(Z_PROBE_ALLEN_KEY)
-
-      // Move up for safety
-      feedrate = homing_feedrate[X_AXIS];
-      destination[Z_AXIS] = current_position[Z_AXIS] + Z_RAISE_AFTER_PROBING;
-      prepare_move_raw();
-
-      // Move to the start position to initiate retraction
-      destination[X_AXIS] = Z_PROBE_ALLEN_KEY_RETRACT_X;
-      destination[Y_AXIS] = Z_PROBE_ALLEN_KEY_RETRACT_Y;
-      destination[Z_AXIS] = Z_PROBE_ALLEN_KEY_RETRACT_Z;
-      prepare_move_raw();
-
-      // Move the nozzle down to push the probe into retracted position
-      feedrate = homing_feedrate[Z_AXIS]/10;
-      destination[Z_AXIS] = current_position[Z_AXIS] - Z_PROBE_ALLEN_KEY_RETRACT_DEPTH;
-      prepare_move_raw();
-      
-      // Move up for safety
-      feedrate = homing_feedrate[Z_AXIS]/2;
-      destination[Z_AXIS] = current_position[Z_AXIS] + Z_PROBE_ALLEN_KEY_RETRACT_DEPTH * 2;
-      prepare_move_raw();
-      
-      // Home XY for safety
-      feedrate = homing_feedrate[X_AXIS]/2;
-      destination[X_AXIS] = 0;
-      destination[Y_AXIS] = 0;
-      prepare_move_raw();
-      
-      st_synchronize();
-      
-      bool z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
-      if (!z_min_endstop) {
-        if (!Stopped) {
-          SERIAL_ERROR_START;
-          SERIAL_ERRORLNPGM("Z-Probe failed to retract!");
-          LCD_ALERTMESSAGEPGM("Err: ZPROBE");
         }
         Stop();
-      }
+    }
+  #endif
 
-    #endif
+}
 
+enum ProbeAction {
+  ProbeStay             = 0,
+  ProbeEngage           = BIT(0),
+  ProbeRetract          = BIT(1),
+  ProbeEngageAndRetract = (ProbeEngage | ProbeRetract)
+};
+
+/// Probe bed height at position (x,y), returns the measured z value
+static float probe_pt(float x, float y, float z_before, ProbeAction retract_action=ProbeEngageAndRetract, int verbose_level=1) {
+  // move to right place
+  do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_before);
+  do_blocking_move_to(x - X_PROBE_OFFSET_FROM_EXTRUDER, y - Y_PROBE_OFFSET_FROM_EXTRUDER, current_position[Z_AXIS]);
+
+  #if !defined(Z_PROBE_SLED) && !defined(Z_PROBE_ALLEN_KEY)
+    if (retract_action & ProbeEngage) engage_z_probe();
+  #endif
+
+  run_z_probe();
+  float measured_z = current_position[Z_AXIS];
+
+  #if !defined(Z_PROBE_SLED) && !defined(Z_PROBE_ALLEN_KEY)
+    if (retract_action & ProbeRetract) retract_z_probe();
+  #endif
+
+  if (verbose_level > 2) {
+    SERIAL_PROTOCOLPGM(MSG_BED);
+    SERIAL_PROTOCOLPGM(" X: ");
+    SERIAL_PROTOCOL_F(x, 3);
+    SERIAL_PROTOCOLPGM(" Y: ");
+    SERIAL_PROTOCOL_F(y, 3);
+    SERIAL_PROTOCOLPGM(" Z: ");
+    SERIAL_PROTOCOL_F(measured_z, 3);
+    SERIAL_EOL;
   }
+  return measured_z;
+}
 
-  enum ProbeAction {
-    ProbeStay             = 0,
-    ProbeEngage           = BIT(0),
-    ProbeRetract          = BIT(1),
-    ProbeEngageAndRetract = (ProbeEngage | ProbeRetract)
-  };
-
-  // Probe bed height at position (x,y), returns the measured z value
-  static float probe_pt(float x, float y, float z_before, ProbeAction retract_action=ProbeEngageAndRetract, int verbose_level=1) {
-    // move to right place
-    do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_before);
-    do_blocking_move_to(x - X_PROBE_OFFSET_FROM_EXTRUDER, y - Y_PROBE_OFFSET_FROM_EXTRUDER, current_position[Z_AXIS]);
-
-    #if !defined(Z_PROBE_SLED) && !defined(Z_PROBE_ALLEN_KEY)
-      if (retract_action & ProbeEngage) engage_z_probe();
-    #endif
-
-    run_z_probe();
-    float measured_z = current_position[Z_AXIS];
-
-    #if !defined(Z_PROBE_SLED) && !defined(Z_PROBE_ALLEN_KEY)
-      if (retract_action & ProbeRetract) retract_z_probe(z_before);
-    #endif
-
-    if (verbose_level > 2) {
-      SERIAL_PROTOCOLPGM(MSG_BED);
-      SERIAL_PROTOCOLPGM(" X: ");
-      SERIAL_PROTOCOL_F(x, 3);
-      SERIAL_PROTOCOLPGM(" Y: ");
-      SERIAL_PROTOCOL_F(y, 3);
-      SERIAL_PROTOCOLPGM(" Z: ");
-      SERIAL_PROTOCOL_F(measured_z, 3);
-      SERIAL_EOL;
-    }
-    return measured_z;
+#ifdef DELTA
+static void extrapolate_one_point(int x, int y, int xdir, int ydir) {
+  if (bed_level[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 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[x][y] = median;
+}
 
-  #ifdef DELTA
-
-    /**
-     * 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) {
-        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 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[x][y] = median;
+// Fill in the unprobed points (corners of circular print surface)
+// 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++) {
+      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);
     }
+  }
+}
 
-    // Fill in the unprobed points (corners of circular print surface)
-    // 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++) {
-          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 (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_PROTOCOLPGM(" ");
     }
+    SERIAL_ECHOLN("");
+  }
+}
 
-    // 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++) {
-          SERIAL_PROTOCOL_F(bed_level[x][y], 2);
-          SERIAL_PROTOCOLPGM(" ");
-        }
-        SERIAL_ECHOLN("");
-      }
+// Reset calibration results to zero.
+void reset_bed_level() {
+  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;
     }
+  }
+}
 
-    // Reset calibration results to zero.
-    void reset_bed_level() {
-      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;
-        }
-      }
-    }
-
-  #endif // DELTA
+#endif // DELTA
 
 #endif // ENABLE_AUTO_BED_LEVELING
 
 static void homeaxis(int axis) {
-  #define HOMEAXIS_DO(LETTER) \
-    ((LETTER##_MIN_PIN > -1 && LETTER##_HOME_DIR==-1) || (LETTER##_MAX_PIN > -1 && LETTER##_HOME_DIR==1))
+#define HOMEAXIS_DO(LETTER) \
+  ((LETTER##_MIN_PIN > -1 && LETTER##_HOME_DIR==-1) || (LETTER##_MAX_PIN > -1 && LETTER##_HOME_DIR==1))
 
-  if (axis == X_AXIS ? HOMEAXIS_DO(X) :
-      axis == Y_AXIS ? HOMEAXIS_DO(Y) :
-      axis == Z_AXIS ? HOMEAXIS_DO(Z) : 0) {
-
-    int axis_home_dir;
-
-    #ifdef DUAL_X_CARRIAGE
-      if (axis == X_AXIS) axis_home_dir = x_home_dir(active_extruder);
-    #else
-      axis_home_dir = home_dir(axis);
-    #endif
+  if (axis==X_AXIS ? HOMEAXIS_DO(X) :
+      axis==Y_AXIS ? HOMEAXIS_DO(Y) :
+      axis==Z_AXIS ? HOMEAXIS_DO(Z) :
+      0) {
+    int axis_home_dir = home_dir(axis);
+#ifdef DUAL_X_CARRIAGE
+    if (axis == X_AXIS)
+      axis_home_dir = x_home_dir(active_extruder);
+#endif
 
     current_position[axis] = 0;
     sync_plan_position();
 
-    #ifndef Z_PROBE_SLED
-      // Engage Servo endstop if enabled
-      #ifdef SERVO_ENDSTOPS
-        #if SERVO_LEVELING
-          if (axis == Z_AXIS) {
-            engage_z_probe();
-          }
-          else
-        #endif // SERVO_LEVELING
 
-        if (servo_endstops[axis] > -1)
-          servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2]);
-
-      #endif // SERVO_ENDSTOPS
-
-    #endif // Z_PROBE_SLED
-
-<<<<<<< HEAD
 #ifndef Z_PROBE_SLED
     // Engage Servo endstop if enabled and we are not using Z_PROBE_AND_ENDSTOP unless we are using Z_SAFE_HOMING
     #ifdef SERVO_ENDSTOPS && (defined (Z_SAFE_HOMING) || ! defined (Z_PROBE_AND_ENDSTOP))
@@ -1589,33 +1445,33 @@ static void homeaxis(int axis) {
       }
     #endif
 #endif // Z_PROBE_SLED
-=======
->>>>>>> MarlinFirmware/Development
     #ifdef Z_DUAL_ENDSTOPS
-      if (axis == Z_AXIS) In_Homing_Process(true);
+      if (axis==Z_AXIS) In_Homing_Process(true);
     #endif
-
     destination[axis] = 1.5 * max_length(axis) * axis_home_dir;
     feedrate = homing_feedrate[axis];
-    line_to_destination();
+    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
     st_synchronize();
 
     current_position[axis] = 0;
     sync_plan_position();
     destination[axis] = -home_retract_mm(axis) * axis_home_dir;
-    line_to_destination();
+    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
     st_synchronize();
 
-    destination[axis] = 2 * home_retract_mm(axis) * axis_home_dir;
+    destination[axis] = 2*home_retract_mm(axis) * axis_home_dir;
 
     if (homing_bump_divisor[axis] >= 1)
-      feedrate = homing_feedrate[axis] / homing_bump_divisor[axis];
-    else {
-      feedrate = homing_feedrate[axis] / 10;
-      SERIAL_ECHOLN("Warning: The Homing Bump Feedrate Divisor cannot be less than 1");
+    {
+        feedrate = homing_feedrate[axis]/homing_bump_divisor[axis];
+    } 
+    else
+    {
+        feedrate = homing_feedrate[axis]/10;
+        SERIAL_ECHOLN("Warning: The Homing Bump Feedrate Divisor cannot be less then 1");
     }
 
-    line_to_destination();
+    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
     st_synchronize();
     #ifdef Z_DUAL_ENDSTOPS
       if (axis==Z_AXIS)
@@ -1630,7 +1486,7 @@ static void homeaxis(int axis) {
           destination[axis] = fabs(z_endstop_adj);
           if (z_endstop_adj < 0) Lock_z_motor(true); else Lock_z2_motor(true);        
         }
-        line_to_destination();
+        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
         st_synchronize();
         Lock_z_motor(false);
         Lock_z2_motor(false);
@@ -1643,7 +1499,7 @@ static void homeaxis(int axis) {
     if (endstop_adj[axis] * axis_home_dir < 0) {
       sync_plan_position();
       destination[axis] = endstop_adj[axis];
-      line_to_destination();
+      plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
       st_synchronize();
     }
 #endif
@@ -1688,7 +1544,7 @@ void refresh_cmd_timeout(void)
       }
       plan_set_e_position(current_position[E_AXIS]);
       float oldFeedrate = feedrate;
-      feedrate = retract_feedrate * 60;
+      feedrate=retract_feedrate*60;
       retracted[active_extruder]=true;
       prepare_move();
       if(retract_zlift > 0.01) {
@@ -1724,8 +1580,8 @@ void refresh_cmd_timeout(void)
       }
       plan_set_e_position(current_position[E_AXIS]);
       float oldFeedrate = feedrate;
-      feedrate = retract_recover_feedrate * 60;
-      retracted[active_extruder] = false;
+      feedrate=retract_recover_feedrate*60;
+      retracted[active_extruder]=false;
       prepare_move();
       feedrate = oldFeedrate;
     }
@@ -1879,16 +1735,17 @@ inline void gcode_G4() {
  */
 inline void gcode_G28() {
   #ifdef ENABLE_AUTO_BED_LEVELING
-    plan_bed_level_matrix.set_to_identity();  //Reset the plane ("erase" all leveling data)
     #ifdef DELTA
       reset_bed_level();
+    #else
+      plan_bed_level_matrix.set_to_identity();  //Reset the plane ("erase" all leveling data)
     #endif
   #endif
 
   #if defined(MESH_BED_LEVELING)
     uint8_t mbl_was_active = mbl.active;
     mbl.active = 0;
-  #endif
+  #endif  // MESH_BED_LEVELING
 
   saved_feedrate = feedrate;
   saved_feedmultiply = feedmultiply;
@@ -1911,7 +1768,7 @@ inline void gcode_G28() {
 
     for (int i = X_AXIS; i <= Z_AXIS; i++) destination[i] = 3 * Z_MAX_LENGTH;
     feedrate = 1.732 * homing_feedrate[X_AXIS];
-    line_to_destination();
+    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
     st_synchronize();
     endstops_hit_on_purpose();
 
@@ -1959,7 +1816,7 @@ inline void gcode_G28() {
         } else {
           feedrate *= sqrt(pow(max_length(X_AXIS) / max_length(Y_AXIS), 2) + 1);
         }
-        line_to_destination();
+        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
         st_synchronize();
 
         axis_is_at_home(X_AXIS);
@@ -1967,7 +1824,7 @@ inline void gcode_G28() {
         sync_plan_position();
         destination[X_AXIS] = current_position[X_AXIS];
         destination[Y_AXIS] = current_position[Y_AXIS];
-        line_to_destination();
+        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
         feedrate = 0.0;
         st_synchronize();
         endstops_hit_on_purpose();
@@ -2035,7 +1892,7 @@ inline void gcode_G28() {
             #ifndef Z_PROBE_AND_ENDSTOP
             destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS);    // Set destination away from bed
             feedrate = max_feedrate[Z_AXIS];
-            line_to_destination();
+            plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
             st_synchronize();
             #endif
           #endif
@@ -2048,11 +1905,11 @@ inline void gcode_G28() {
           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] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS);    // Set destination away from bed
-          feedrate = XY_TRAVEL_SPEED;
+          feedrate = XY_TRAVEL_SPEED / 60;
           current_position[Z_AXIS] = 0;
 
           sync_plan_position();
-          line_to_destination();
+          plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
           st_synchronize();
           current_position[X_AXIS] = destination[X_AXIS];
           current_position[Y_AXIS] = destination[Y_AXIS];
@@ -2074,7 +1931,7 @@ inline void gcode_G28() {
               plan_set_position(cpx, cpy, current_position[Z_AXIS], current_position[E_AXIS]);
               destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS);    // Set destination away from bed
               feedrate = max_feedrate[Z_AXIS];
-              line_to_destination();
+              plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
               st_synchronize();
               HOMEAXIS(Z);
             }
@@ -2127,7 +1984,7 @@ inline void gcode_G28() {
       destination[Z_AXIS] = current_position[Z_AXIS];
       destination[E_AXIS] = current_position[E_AXIS];
       feedrate = homing_feedrate[X_AXIS];
-      line_to_destination();
+      plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
       st_synchronize();
       current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
       sync_plan_position();
@@ -2141,19 +1998,6 @@ inline void gcode_G28() {
   endstops_hit_on_purpose();
 }
 
-#if defined(MESH_BED_LEVELING) || defined(ENABLE_AUTO_BED_LEVELING)
-
-  // Check for known positions in X and Y
-  inline bool can_run_bed_leveling() {
-  	if (axis_known_position[X_AXIS] && axis_known_position[Y_AXIS]) return true;
-    LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
-    SERIAL_ECHO_START;
-    SERIAL_ECHOLNPGM(MSG_POSITION_UNKNOWN);
-    return false;
-  }
-
-#endif // MESH_BED_LEVELING || ENABLE_AUTO_BED_LEVELING
-
 #ifdef MESH_BED_LEVELING
 
   /**
@@ -2168,10 +2012,6 @@ inline void gcode_G28() {
    *
    */
   inline void gcode_G29() {
-
-    // Prevent leveling without first homing in X and Y
-    if (!can_run_bed_leveling()) return;
-
     static int probe_point = -1;
     int state = 0;
     if (code_seen('S') || code_seen('s')) {
@@ -2288,8 +2128,13 @@ inline void gcode_G28() {
    */
   inline void gcode_G29() {
 
-    // Prevent leveling without first homing in X and Y
-    if (!can_run_bed_leveling()) return;
+    // Prevent user from running a G29 without first homing in X and Y
+    if (!axis_known_position[X_AXIS] || !axis_known_position[Y_AXIS]) {
+      LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
+      SERIAL_ECHO_START;
+      SERIAL_ECHOLNPGM(MSG_POSITION_UNKNOWN);
+      return;
+    }
 
     int verbose_level = 1;
 
@@ -2371,15 +2216,16 @@ inline void gcode_G28() {
 
     st_synchronize();
 
-    if (!dryrun) {
-      // make sure the bed_level_rotation_matrix is identity or the planner will get it wrong
-      plan_bed_level_matrix.set_to_identity();
-
+    if (!dryrun)
+    {
       #ifdef DELTA
         reset_bed_level();
       #else //!DELTA
+
+        // make sure the bed_level_rotation_matrix is identity or the planner will get it incorectly
         //vector_3 corrected_position = plan_get_position_mm();
         //corrected_position.debug("position before G29");
+        plan_bed_level_matrix.set_to_identity();
         vector_3 uncorrected_position = plan_get_position();
         //uncorrected_position.debug("position during G29");
         current_position[X_AXIS] = uncorrected_position.x;
@@ -2387,7 +2233,7 @@ inline void gcode_G28() {
         current_position[Z_AXIS] = uncorrected_position.z;
         sync_plan_position();
 
-      #endif // !DELTA
+      #endif
     }
     
     setup_for_endstop_move();
@@ -2448,12 +2294,13 @@ inline void gcode_G28() {
 
           // raise extruder
           float measured_z,
-                z_before = Z_RAISE_BETWEEN_PROBINGS + (probePointCounter ? current_position[Z_AXIS] : 0);
+                z_before = probePointCounter == 0 ? Z_RAISE_BEFORE_PROBING : current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS;
 
           #ifdef DELTA
             // Avoid probing the corners (outside the round or hexagon print surface) on a delta printer.
             float distance_from_center = sqrt(xProbe*xProbe + yProbe*yProbe);
-            if (distance_from_center > DELTA_PROBABLE_RADIUS) continue;
+            if (distance_from_center > DELTA_PROBABLE_RADIUS)
+              continue;
           #endif //DELTA
 
           // Enhanced G29 - Do not retract servo between probes
@@ -2481,11 +2328,6 @@ inline void gcode_G28() {
           #endif
 
           probePointCounter++;
-
-          manage_heater();
-          manage_inactivity();
-          lcd_update();
-
         } //xProbe
       } //yProbe
 
@@ -2572,14 +2414,16 @@ inline void gcode_G28() {
       if (verbose_level > 0)
         plan_bed_level_matrix.debug(" \n\nBed Level Correction Matrix:");
 
-      if (!dryrun) {
-        // Correct the Z height difference from z-probe position and hotend tip position.
-        // The Z height on homing is measured by Z-Probe, but the probe is quite far from the hotend.
-        // When the bed is uneven, this height must be corrected.
-        float x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER,
-              y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER,
-              z_tmp = current_position[Z_AXIS],
-              real_z = (float)st_get_position(Z_AXIS) / axis_steps_per_unit[Z_AXIS];  //get the real Z (since the auto bed leveling is already correcting the plane)
+      // Correct the Z height difference from z-probe position and hotend tip position.
+      // The Z height on homing is measured by Z-Probe, but the probe is quite far from the hotend.
+      // When the bed is uneven, this height must be corrected.
+      if (!dryrun)
+      {
+        float x_tmp, y_tmp, z_tmp, real_z;
+        real_z = float(st_get_position(Z_AXIS)) / axis_steps_per_unit[Z_AXIS];  //get the real Z (since the auto bed leveling is already correcting the plane)
+        x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER;
+        y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER;
+        z_tmp = current_position[Z_AXIS];
 
         apply_rotation_xyz(plan_bed_level_matrix, x_tmp, y_tmp, z_tmp);         //Apply the correction sending the probe offset
         current_position[Z_AXIS] = z_tmp - real_z + current_position[Z_AXIS];   //The difference is added to current position and sent to planner.
@@ -3947,7 +3791,7 @@ inline void gcode_M221() {
       extruder_multiply[tmp_extruder] = sval;
     }
     else {
-      extruder_multiply[active_extruder] = sval;
+      extrudemultiply = sval;
     }
   }
 }
@@ -4384,7 +4228,7 @@ inline void gcode_M400() { st_synchronize(); }
     //SERIAL_PROTOCOLPGM("Filament dia (measured mm):");
     //SERIAL_PROTOCOL(filament_width_meas);
     //SERIAL_PROTOCOLPGM("Extrusion ratio(%):");
-    //SERIAL_PROTOCOL(extruder_multiply[active_extruder]);
+    //SERIAL_PROTOCOL(extrudemultiply);
   }
 
   /**
@@ -4857,14 +4701,18 @@ void process_commands() {
       gcode_G28();
       break;
 
-    #if defined(ENABLE_AUTO_BED_LEVELING) || defined(MESH_BED_LEVELING)
-      case 29: // G29 Detailed Z-Probe, probes the bed at 3 or more points.
+    #if defined(MESH_BED_LEVELING)
+      case 29: // G29 Handle mesh based leveling
         gcode_G29();
         break;
     #endif
 
     #ifdef ENABLE_AUTO_BED_LEVELING
 
+      case 29: // G29 Detailed Z-Probe, probes the bed at 3 or more points.
+        gcode_G29();
+        break;
+
       #ifndef Z_PROBE_SLED
 
         case 30: // G30 Single Z Probe
@@ -5559,72 +5407,69 @@ void prepare_move()
   
   #ifdef SCARA //for now same as delta-code
 
-    float difference[NUM_AXIS];
-    for (int8_t i = 0; i < NUM_AXIS; i++) difference[i] = destination[i] - current_position[i];
+float difference[NUM_AXIS];
+for (int8_t i=0; i < NUM_AXIS; i++) {
+  difference[i] = destination[i] - current_position[i];
+}
 
-    float cartesian_mm = sqrt(  sq(difference[X_AXIS]) +
-                                sq(difference[Y_AXIS]) +
-                                sq(difference[Z_AXIS]));
-    if (cartesian_mm < 0.000001) { cartesian_mm = abs(difference[E_AXIS]); }
-    if (cartesian_mm < 0.000001) { return; }
-    float seconds = 6000 * cartesian_mm / feedrate / feedmultiply;
-    int steps = max(1, int(scara_segments_per_second * seconds));
+float cartesian_mm = sqrt(  sq(difference[X_AXIS]) +
+              sq(difference[Y_AXIS]) +
+              sq(difference[Z_AXIS]));
+if (cartesian_mm < 0.000001) { cartesian_mm = abs(difference[E_AXIS]); }
+if (cartesian_mm < 0.000001) { return; }
+float seconds = 6000 * cartesian_mm / feedrate / feedmultiply;
+int steps = max(1, int(scara_segments_per_second * seconds));
+ //SERIAL_ECHOPGM("mm="); SERIAL_ECHO(cartesian_mm);
+ //SERIAL_ECHOPGM(" seconds="); SERIAL_ECHO(seconds);
+ //SERIAL_ECHOPGM(" steps="); SERIAL_ECHOLN(steps);
+for (int s = 1; s <= steps; s++) {
+  float fraction = float(s) / float(steps);
+  for(int8_t i=0; i < NUM_AXIS; i++) {
+    destination[i] = current_position[i] + difference[i] * fraction;
+  }
 
-    //SERIAL_ECHOPGM("mm="); SERIAL_ECHO(cartesian_mm);
-    //SERIAL_ECHOPGM(" seconds="); SERIAL_ECHO(seconds);
-    //SERIAL_ECHOPGM(" steps="); SERIAL_ECHOLN(steps);
-
-    for (int s = 1; s <= steps; s++) {
-      float fraction = float(s) / float(steps);
-      for(int8_t i = 0; i < NUM_AXIS; i++) {
-        destination[i] = current_position[i] + difference[i] * fraction;
-      }
   
-      calculate_delta(destination);
-      //SERIAL_ECHOPGM("destination[X_AXIS]="); SERIAL_ECHOLN(destination[X_AXIS]);
-      //SERIAL_ECHOPGM("destination[Y_AXIS]="); SERIAL_ECHOLN(destination[Y_AXIS]);
-      //SERIAL_ECHOPGM("destination[Z_AXIS]="); SERIAL_ECHOLN(destination[Z_AXIS]);
-      //SERIAL_ECHOPGM("delta[X_AXIS]="); SERIAL_ECHOLN(delta[X_AXIS]);
-      //SERIAL_ECHOPGM("delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]);
-      //SERIAL_ECHOPGM("delta[Z_AXIS]="); SERIAL_ECHOLN(delta[Z_AXIS]);
-
-      plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],
-        destination[E_AXIS], feedrate*feedmultiply/60/100.0,
-        active_extruder);
-    }
-
-  #endif // SCARA
+  calculate_delta(destination);
+         //SERIAL_ECHOPGM("destination[X_AXIS]="); SERIAL_ECHOLN(destination[X_AXIS]);
+         //SERIAL_ECHOPGM("destination[Y_AXIS]="); SERIAL_ECHOLN(destination[Y_AXIS]);
+         //SERIAL_ECHOPGM("destination[Z_AXIS]="); SERIAL_ECHOLN(destination[Z_AXIS]);
+         //SERIAL_ECHOPGM("delta[X_AXIS]="); SERIAL_ECHOLN(delta[X_AXIS]);
+         //SERIAL_ECHOPGM("delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]);
+         //SERIAL_ECHOPGM("delta[Z_AXIS]="); SERIAL_ECHOLN(delta[Z_AXIS]);
+         
+  plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],
+  destination[E_AXIS], feedrate*feedmultiply/60/100.0,
+  active_extruder);
+}
+#endif // SCARA
   
-  #ifdef DELTA
-
-    float difference[NUM_AXIS];
-    for (int8_t i=0; i < NUM_AXIS; i++) difference[i] = destination[i] - current_position[i];
-
-    float cartesian_mm = sqrt(sq(difference[X_AXIS]) +
-                              sq(difference[Y_AXIS]) +
-                              sq(difference[Z_AXIS]));
-    if (cartesian_mm < 0.000001) cartesian_mm = abs(difference[E_AXIS]);
-    if (cartesian_mm < 0.000001) return;
-    float seconds = 6000 * cartesian_mm / feedrate / feedmultiply;
-    int steps = max(1, int(delta_segments_per_second * seconds));
-
-    // SERIAL_ECHOPGM("mm="); SERIAL_ECHO(cartesian_mm);
-    // SERIAL_ECHOPGM(" seconds="); SERIAL_ECHO(seconds);
-    // SERIAL_ECHOPGM(" steps="); SERIAL_ECHOLN(steps);
-
-    for (int s = 1; s <= steps; s++) {
-      float fraction = float(s) / float(steps);
-      for (int8_t i = 0; i < NUM_AXIS; i++) destination[i] = current_position[i] + difference[i] * fraction;
-      calculate_delta(destination);
-      #ifdef ENABLE_AUTO_BED_LEVELING
-        adjust_delta(destination);
-      #endif
-      plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],
-                       destination[E_AXIS], feedrate*feedmultiply/60/100.0,
-                       active_extruder);
+#ifdef DELTA
+  float difference[NUM_AXIS];
+  for (int8_t i=0; i < NUM_AXIS; i++) {
+    difference[i] = destination[i] - current_position[i];
+  }
+  float cartesian_mm = sqrt(sq(difference[X_AXIS]) +
+                            sq(difference[Y_AXIS]) +
+                            sq(difference[Z_AXIS]));
+  if (cartesian_mm < 0.000001) { cartesian_mm = abs(difference[E_AXIS]); }
+  if (cartesian_mm < 0.000001) { return; }
+  float seconds = 6000 * cartesian_mm / feedrate / feedmultiply;
+  int steps = max(1, int(delta_segments_per_second * seconds));
+  // SERIAL_ECHOPGM("mm="); SERIAL_ECHO(cartesian_mm);
+  // SERIAL_ECHOPGM(" seconds="); SERIAL_ECHO(seconds);
+  // SERIAL_ECHOPGM(" steps="); SERIAL_ECHOLN(steps);
+  for (int s = 1; s <= steps; s++) {
+    float fraction = float(s) / float(steps);
+    for(int8_t i=0; i < NUM_AXIS; i++) {
+      destination[i] = current_position[i] + difference[i] * fraction;
     }
-
-  #endif // DELTA
+    calculate_delta(destination);
+    plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],
+                     destination[E_AXIS], feedrate*feedmultiply/60/100.0,
+                     active_extruder);
+  }
+  
+#endif // DELTA
 
 #ifdef DUAL_X_CARRIAGE
   if (active_extruder_parked)
@@ -5670,13 +5515,13 @@ void prepare_move()
 #if ! (defined DELTA || defined SCARA)
   // Do not use feedmultiply for E or Z only moves
   if( (current_position[X_AXIS] == destination [X_AXIS]) && (current_position[Y_AXIS] == destination [Y_AXIS])) {
-    line_to_destination();
+    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
   } else {
 #if defined(MESH_BED_LEVELING)
-    mesh_plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], (feedrate/60)*(feedmultiply/100.0), active_extruder);
+    mesh_plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder);
     return;
 #else
-    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], (feedrate/60)*(feedmultiply/100.0), active_extruder);
+    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder);
 #endif  // MESH_BED_LEVELING
   }
 #endif // !(DELTA || SCARA)
diff --git a/Marlin/dogm_lcd_implementation.h b/Marlin/dogm_lcd_implementation.h
index 63e99bd3aa..89cd5e835c 100644
--- a/Marlin/dogm_lcd_implementation.h
+++ b/Marlin/dogm_lcd_implementation.h
@@ -369,7 +369,7 @@ static void lcd_implementation_status_screen() {
       lcd_printPGM(PSTR("dia:"));
       lcd_print(ftostr12ns(filament_width_meas));
       lcd_printPGM(PSTR(" factor:"));
-      lcd_print(itostr3(volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM]));
+      lcd_print(itostr3(extrudemultiply));
       lcd_print('%');
     }
   #endif
diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index d98ef63d4d..786527d0d7 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -545,7 +545,7 @@ float junction_deviation = 0.1;
   block->steps[Z_AXIS] = labs(dz);
   block->steps[E_AXIS] = labs(de);
   block->steps[E_AXIS] *= volumetric_multiplier[active_extruder];
-  block->steps[E_AXIS] *= extruder_multiply[active_extruder];
+  block->steps[E_AXIS] *= extrudemultiply;
   block->steps[E_AXIS] /= 100;
   block->step_event_count = max(block->steps[X_AXIS], max(block->steps[Y_AXIS], max(block->steps[Z_AXIS], block->steps[E_AXIS])));
 
@@ -679,7 +679,7 @@ float junction_deviation = 0.1;
     delta_mm[Y_AXIS] = dy / axis_steps_per_unit[Y_AXIS];
   #endif
   delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS];
-  delta_mm[E_AXIS] = (de / axis_steps_per_unit[E_AXIS]) * volumetric_multiplier[active_extruder] * extruder_multiply[active_extruder] / 100.0;
+  delta_mm[E_AXIS] = (de / axis_steps_per_unit[E_AXIS]) * volumetric_multiplier[active_extruder] * extrudemultiply / 100.0;
 
   if (block->steps[X_AXIS] <= dropsegments && block->steps[Y_AXIS] <= dropsegments && block->steps[Z_AXIS] <= dropsegments) {
     block->millimeters = fabs(delta_mm[E_AXIS]);
diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index d38474bafd..73c23ae9de 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -515,36 +515,31 @@ ISR(TIMER1_COMPA_vect) {
     }
 
     if (TEST(out_bits, Z_AXIS)) {   // -direction
-
       Z_APPLY_DIR(INVERT_Z_DIR,0);
       count_direction[Z_AXIS] = -1;
-
-      if (check_endstops) {
-
-        #if defined(Z_MIN_PIN) && Z_MIN_PIN >= 0
-
-          #ifdef Z_DUAL_ENDSTOPS
-
-            bool z_min_endstop = READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING,
-                z2_min_endstop =
-                  #if defined(Z2_MIN_PIN) && Z2_MIN_PIN >= 0
-                    READ(Z2_MIN_PIN) != Z2_MIN_ENDSTOP_INVERTING
-                  #else
-                    z_min_endstop
-                  #endif
-                ;
-
-            bool z_min_both = z_min_endstop && old_z_min_endstop,
-                z2_min_both = z2_min_endstop && old_z2_min_endstop;
-            if ((z_min_both || z2_min_both) && current_block->steps[Z_AXIS] > 0) {
+      if (check_endstops) 
+      {
+        #if defined(Z_MIN_PIN) && Z_MIN_PIN > -1
+          #ifndef Z_DUAL_ENDSTOPS
+            UPDATE_ENDSTOP(z, Z, min, MIN);
+          #else
+            bool z_min_endstop=(READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
+            #if defined(Z2_MIN_PIN) && Z2_MIN_PIN > -1
+              bool z2_min_endstop=(READ(Z2_MIN_PIN) != Z2_MIN_ENDSTOP_INVERTING);
+            #else
+              bool z2_min_endstop=z_min_endstop;
+            #endif
+            if(((z_min_endstop && old_z_min_endstop) || (z2_min_endstop && old_z2_min_endstop)) && (current_block->steps[Z_AXIS] > 0))
+            {
               endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
-              endstop_z_hit = true;
-              if (!performing_homing || (performing_homing && z_min_both && z2_min_both)) //if not performing home or if both endstops were trigged during homing...
+              endstop_z_hit=true;
+              if (!(performing_homing) || ((performing_homing)&&(z_min_endstop && old_z_min_endstop)&&(z2_min_endstop && old_z2_min_endstop))) //if not performing home or if both endstops were trigged during homing...
+              {
                 step_events_completed = current_block->step_event_count;
+              } 
             }
             old_z_min_endstop = z_min_endstop;
             old_z2_min_endstop = z2_min_endstop;
-<<<<<<< HEAD
           #endif
         #endif
 
@@ -561,55 +556,37 @@ ISR(TIMER1_COMPA_vect) {
           old_z_probe_endstop = z_probe_endstop;
         #endif
       }
-=======
-
-          #else // !Z_DUAL_ENDSTOPS
-
-            UPDATE_ENDSTOP(z, Z, min, MIN);
-
-          #endif // !Z_DUAL_ENDSTOPS
-
-        #endif // Z_MIN_PIN
-
-      } // check_endstops
-
->>>>>>> MarlinFirmware/Development
     }
     else { // +direction
-
       Z_APPLY_DIR(!INVERT_Z_DIR,0);
       count_direction[Z_AXIS] = 1;
-
       if (check_endstops) {
-
         #if defined(Z_MAX_PIN) && Z_MAX_PIN >= 0
-
-          #ifdef Z_DUAL_ENDSTOPS
-
-            bool z_max_endstop = READ(Z_MAX_PIN) != Z_MAX_ENDSTOP_INVERTING,
-                z2_max_endstop =
-                  #if defined(Z2_MAX_PIN) && Z2_MAX_PIN >= 0
-                    READ(Z2_MAX_PIN) != Z2_MAX_ENDSTOP_INVERTING
-                  #else
-                    z_max_endstop
-                  #endif
-                ;
-
-            bool z_max_both = z_max_endstop && old_z_max_endstop,
-                z2_max_both = z2_max_endstop && old_z2_max_endstop;
-            if ((z_max_both || z2_max_both) && current_block->steps[Z_AXIS] > 0) {
+          #ifndef Z_DUAL_ENDSTOPS
+            UPDATE_ENDSTOP(z, Z, max, MAX);
+          #else
+            bool z_max_endstop=(READ(Z_MAX_PIN) != Z_MAX_ENDSTOP_INVERTING);
+            #if defined(Z2_MAX_PIN) && Z2_MAX_PIN > -1
+              bool z2_max_endstop=(READ(Z2_MAX_PIN) != Z2_MAX_ENDSTOP_INVERTING);
+            #else
+              bool z2_max_endstop=z_max_endstop;
+            #endif
+            if(((z_max_endstop && old_z_max_endstop) || (z2_max_endstop && old_z2_max_endstop)) && (current_block->steps[Z_AXIS] > 0))
+            {
               endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
-              endstop_z_hit = true;
+              endstop_z_hit=true;
 
-             // if (z_max_both) SERIAL_ECHOLN("z_max_endstop = true");
-             // if (z2_max_both) SERIAL_ECHOLN("z2_max_endstop = true");
+//              if (z_max_endstop && old_z_max_endstop) SERIAL_ECHOLN("z_max_endstop = true");
+//              if (z2_max_endstop && old_z2_max_endstop) SERIAL_ECHOLN("z2_max_endstop = true");
 
-              if (!performing_homing || (performing_homing && z_max_both && z2_max_both)) //if not performing home or if both endstops were trigged during homing...
+            
+              if (!(performing_homing) || ((performing_homing)&&(z_max_endstop && old_z_max_endstop)&&(z2_max_endstop && old_z2_max_endstop))) //if not performing home or if both endstops were trigged during homing...
+              {
                 step_events_completed = current_block->step_event_count;
+              } 
             }
             old_z_max_endstop = z_max_endstop;
             old_z2_max_endstop = z2_max_endstop;
-<<<<<<< HEAD
           #endif
         #endif
 
@@ -626,34 +603,20 @@ ISR(TIMER1_COMPA_vect) {
         #endif
       }
     }
-=======
-
-          #else // !Z_DUAL_ENDSTOPS
-
-            UPDATE_ENDSTOP(z, Z, max, MAX);
-
-          #endif // !Z_DUAL_ENDSTOPS
-
-        #endif // Z_MAX_PIN
-
-      } // check_endstops
-
-    } // +direction
->>>>>>> MarlinFirmware/Development
 
     #ifndef ADVANCE
       if (TEST(out_bits, E_AXIS)) {  // -direction
         REV_E_DIR();
-        count_direction[E_AXIS] = -1;
+        count_direction[E_AXIS]=-1;
       }
       else { // +direction
         NORM_E_DIR();
-        count_direction[E_AXIS] = 1;
+        count_direction[E_AXIS]=1;
       }
     #endif //!ADVANCE
 
     // Take multiple steps per interrupt (For high speed moves)
-    for (int8_t i = 0; i < step_loops; i++) {
+    for (int8_t i=0; i < step_loops; i++) {
       #ifndef AT90USB
         MSerial.checkRx(); // Check for serial chars.
       #endif
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index 58a66973f4..c85f8e14df 100644
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -485,7 +485,7 @@ static void lcd_tune_menu() {
     MENU_MULTIPLIER_ITEM_EDIT(int3, MSG_BED, &target_temperature_bed, 0, BED_MAXTEMP - 15);
   #endif
   MENU_MULTIPLIER_ITEM_EDIT(int3, MSG_FAN_SPEED, &fanSpeed, 0, 255);
-  MENU_ITEM_EDIT(int3, MSG_FLOW, &extruder_multiply[active_extruder], 10, 999);
+  MENU_ITEM_EDIT(int3, MSG_FLOW, &extrudemultiply, 10, 999);
   MENU_ITEM_EDIT(int3, MSG_FLOW MSG_F0, &extruder_multiply[0], 10, 999);
   #if TEMP_SENSOR_1 != 0
     MENU_ITEM_EDIT(int3, MSG_FLOW MSG_F1, &extruder_multiply[1], 10, 999);
diff --git a/Marlin/ultralcd_implementation_hitachi_HD44780.h b/Marlin/ultralcd_implementation_hitachi_HD44780.h
index c21785ed25..aaa55800ac 100644
--- a/Marlin/ultralcd_implementation_hitachi_HD44780.h
+++ b/Marlin/ultralcd_implementation_hitachi_HD44780.h
@@ -624,7 +624,7 @@ static void lcd_implementation_status_screen()
 
 static void lcd_implementation_drawmenu_generic(bool sel, uint8_t row, const char* pstr, char pre_char, char post_char) {
   char c;
-  uint8_t n = LCD_WIDTH - 2;
+  uint8_t n = LCD_WIDTH - 1 - (LCD_WIDTH < 20 ? 1 : 2);
   lcd.setCursor(0, row);
   lcd.print(sel ? pre_char : ' ');
   while ((c = pgm_read_byte(pstr)) && n > 0) {
@@ -633,11 +633,12 @@ static void lcd_implementation_drawmenu_generic(bool sel, uint8_t row, const cha
   }
   while(n--) lcd.print(' ');
   lcd.print(post_char);
+  lcd.print(' ');
 }
 
 static void lcd_implementation_drawmenu_setting_edit_generic(bool sel, uint8_t row, const char* pstr, char pre_char, char* data) {
   char c;
-  uint8_t n = LCD_WIDTH - 2 - lcd_strlen(data);
+  uint8_t n = LCD_WIDTH - 1 - (LCD_WIDTH < 20 ? 1 : 2) - lcd_strlen(data);
   lcd.setCursor(0, row);
   lcd.print(sel ? pre_char : ' ');
   while ((c = pgm_read_byte(pstr)) && n > 0) {
@@ -650,7 +651,7 @@ static void lcd_implementation_drawmenu_setting_edit_generic(bool sel, uint8_t r
 }
 static void lcd_implementation_drawmenu_setting_edit_generic_P(bool sel, uint8_t row, const char* pstr, char pre_char, const char* data) {
   char c;
-  uint8_t n = LCD_WIDTH - 2 - lcd_strlen_P(data);
+  uint8_t n = LCD_WIDTH - 1 - (LCD_WIDTH < 20 ? 1 : 2) - lcd_strlen_P(data);
   lcd.setCursor(0, row);
   lcd.print(sel ? pre_char : ' ');
   while ((c = pgm_read_byte(pstr)) && n > 0) {
@@ -687,11 +688,11 @@ void lcd_implementation_drawedit(const char* pstr, char* value) {
   lcd.setCursor(1, 1);
   lcd_printPGM(pstr);
   lcd.print(':');
-  lcd.setCursor(LCD_WIDTH - lcd_strlen(value), 1);
+  lcd.setCursor(LCD_WIDTH - (LCD_WIDTH < 20 ? 0 : 1) - lcd_strlen(value), 1);
   lcd_print(value);
 }
 
-static void lcd_implementation_drawmenu_sd(bool sel, uint8_t row, const char* pstr, const char* filename, char* longFilename, uint8_t concat, char post_char) {
+static void lcd_implementation_drawmenu_sd(bool sel, uint8_t row, const char* pstr, const char* filename, char* longFilename, uint8_t concat) {
   char c;
   uint8_t n = LCD_WIDTH - concat;
   lcd.setCursor(0, row);
@@ -705,15 +706,14 @@ static void lcd_implementation_drawmenu_sd(bool sel, uint8_t row, const char* ps
     filename++;
   }
   while (n--) lcd.print(' ');
-  lcd.print(post_char);
 }
 
 static void lcd_implementation_drawmenu_sdfile(bool sel, uint8_t row, const char* pstr, const char* filename, char* longFilename) {
-  lcd_implementation_drawmenu_sd(sel, row, pstr, filename, longFilename, 2, ' ');
+  lcd_implementation_drawmenu_sd(sel, row, pstr, filename, longFilename, 1);
 }
 
 static void lcd_implementation_drawmenu_sddirectory(bool sel, uint8_t row, const char* pstr, const char* filename, char* longFilename) {
-  lcd_implementation_drawmenu_sd(sel, row, pstr, filename, longFilename, 2, LCD_STR_FOLDER[0]);
+  lcd_implementation_drawmenu_sd(sel, row, pstr, filename, longFilename, 2);
 }
 
 #define lcd_implementation_drawmenu_back(sel, row, pstr, data) lcd_implementation_drawmenu_generic(sel, row, pstr, LCD_STR_UPLEVEL[0], LCD_STR_UPLEVEL[0])

From 3175c70c796281e3aa3bf5df2300f405345fdf63 Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Tue, 31 Mar 2015 00:11:11 -0500
Subject: [PATCH 22/37] Manually synching back up with
 MarlinFirmware/Development.

---
 Marlin/Conditionals.h  | 3 +++
 Marlin/Configuration.h | 2 +-
 2 files changed, 4 insertions(+), 1 deletion(-)

diff --git a/Marlin/Conditionals.h b/Marlin/Conditionals.h
index 62ea7ba54a..5f626b9fbd 100644
--- a/Marlin/Conditionals.h
+++ b/Marlin/Conditionals.h
@@ -185,6 +185,9 @@
       #define ENDSTOPPULLUP_YMIN
       #define ENDSTOPPULLUP_ZMIN
     #endif
+    #ifndef DISABLE_Z_PROBE_ENDSTOP
+      #define ENDSTOPPULL_ZPROBE
+    #endif
   #endif
 
   /**
diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index 481004f1d3..82d7107456 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -313,8 +313,8 @@ your extruder heater takes 2 minutes to hit the target on heating.
   // #define ENDSTOPPULLUP_XMIN
   // #define ENDSTOPPULLUP_YMIN
   // #define ENDSTOPPULLUP_ZMIN
+  // #define ENDSTOPPULLUP_ZPROBE
 #endif
->>>>>>> MarlinFirmware/Development
 
 // Mechanical endstop with COM to ground and NC to Signal uses "false" here (most common setup).
 const bool X_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic of the endstop.

From ec1d9c0b8f87a0d76c1f54496b0e8584081dd099 Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Tue, 31 Mar 2015 01:06:01 -0500
Subject: [PATCH 23/37] Use Z_PROBE_ENDSTOP_INVERTING when checking pin status.

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

diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index 73c23ae9de..0dbc2a297b 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -545,7 +545,7 @@ ISR(TIMER1_COMPA_vect) {
 
         #if defined(Z_PROBE_PIN) && Z_PROBE_PIN > -1
           UPDATE_ENDSTOP(z, Z, probe, PROBE);
-          z_probe_endstop=(READ(Z_PROBE_PIN) != Z_MIN_ENDSTOP_INVERTING);
+          z_probe_endstop=(READ(Z_PROBE_PIN) != Z_PROBE_ENDSTOP_INVERTING);
           if(z_probe_endstop && old_z_probe_endstop)
           {
         	  endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
@@ -592,7 +592,7 @@ ISR(TIMER1_COMPA_vect) {
 
         #if defined(Z_PROBE_PIN) && Z_PROBE_PIN > -1
           UPDATE_ENDSTOP(z, Z, probe, PROBE);
-          z_probe_endstop=(READ(Z_PROBE_PIN) != Z_MAX_ENDSTOP_INVERTING);
+          z_probe_endstop=(READ(Z_PROBE_PIN) != Z_PROBE_ENDSTOP_INVERTING);
           if(z_probe_endstop && old_z_probe_endstop)
           {
         	  endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];

From 17707e7479e943fa06965336b9fe380427d3060c Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Tue, 31 Mar 2015 02:56:41 -0500
Subject: [PATCH 24/37] Fixed Z_PROBE_PIN pullup bug. Documented some
 additional areas that should be addressed if Z_PROBE is fully separated from
 Z_MIN or Z_MAX. Fixed a documentation error in sanity checks. Servos start at
 0 not 1.

---
 Marlin/Conditionals.h  |   2 +-
 Marlin/Marlin_main.cpp | 977 ++++++++++++++++++++++++-----------------
 Marlin/SanityCheck.h   |   2 +-
 Marlin/stepper.cpp     |  13 +-
 4 files changed, 581 insertions(+), 413 deletions(-)

diff --git a/Marlin/Conditionals.h b/Marlin/Conditionals.h
index 5f626b9fbd..413e225629 100644
--- a/Marlin/Conditionals.h
+++ b/Marlin/Conditionals.h
@@ -186,7 +186,7 @@
       #define ENDSTOPPULLUP_ZMIN
     #endif
     #ifndef DISABLE_Z_PROBE_ENDSTOP
-      #define ENDSTOPPULL_ZPROBE
+      #define ENDSTOPPULLUP_ZPROBE
     #endif
   #endif
 
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 13905683a5..0683c67e71 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -79,7 +79,7 @@
 // G4  - Dwell S<seconds> or P<milliseconds>
 // G10 - retract filament according to settings of M207
 // G11 - retract recover filament according to settings of M208
-// G28 - Home all Axis
+// G28 - Home one or more axes
 // G29 - Detailed Z-Probe, probes the bed at 3 or more points.  Will fail if you haven't homed yet.
 // G30 - Single Z Probe, probes bed at current XY location.
 // G31 - Dock sled (Z_PROBE_SLED only)
@@ -170,10 +170,10 @@
 // M404 - N<dia in mm> Enter the nominal filament width (3mm, 1.75mm ) or will display nominal filament width without parameters
 // M405 - Turn on Filament Sensor extrusion control.  Optional D<delay in cm> to set delay in centimeters between sensor and extruder
 // M406 - Turn off Filament Sensor extrusion control
-// M407 - Display measured filament diameter
+// M407 - Displays measured filament diameter
 // M500 - Store parameters in EEPROM
 // M501 - Read parameters from EEPROM (if you need reset them after you changed them temporarily).
-// M502 - Revert to the default "factory settings". You still need to store them in EEPROM afterwards if you want to.
+// M502 - Revert to the default "factory settings".  You still need to store them in EEPROM afterwards if you want to.
 // M503 - Print the current settings (from memory not from EEPROM). Use S0 to leave off headings.
 // M540 - Use S[0|1] to enable or disable the stop SD card print on endstop hit (requires ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
 // M600 - Pause for filament change X[pos] Y[pos] Z[relative lift] E[initial retract] L[later retract distance for removal]
@@ -210,7 +210,6 @@ int homing_bump_divisor[] = HOMING_BUMP_DIVISOR;
 bool axis_relative_modes[] = AXIS_RELATIVE_MODES;
 int feedmultiply = 100; //100->1 200->2
 int saved_feedmultiply;
-int extrudemultiply = 100; //100->1 200->2
 int extruder_multiply[EXTRUDERS] = ARRAY_BY_EXTRUDERS(100, 100, 100, 100);
 bool volumetric_enabled = false;
 float filament_size[EXTRUDERS] = ARRAY_BY_EXTRUDERS(DEFAULT_NOMINAL_FILAMENT_DIA, DEFAULT_NOMINAL_FILAMENT_DIA, DEFAULT_NOMINAL_FILAMENT_DIA, DEFAULT_NOMINAL_FILAMENT_DIA);
@@ -273,7 +272,7 @@ int fanSpeed = 0;
 
 #endif // FWRETRACT
 
-#if defined(ULTIPANEL) && HAS_POWER_SWITCH
+#ifdef ULTIPANEL
   bool powersupply = 
     #ifdef PS_DEFAULT_OFF
       false
@@ -306,19 +305,19 @@ int fanSpeed = 0;
 #ifdef SCARA
   float axis_scaling[3] = { 1, 1, 1 };    // Build size scaling, default to 1
   static float delta[3] = { 0, 0, 0 };		
-#endif        
+#endif
 
 bool cancel_heatup = false;
 
 #ifdef FILAMENT_SENSOR
   //Variables for Filament Sensor input
-  float filament_width_nominal = DEFAULT_NOMINAL_FILAMENT_DIA;  //Set nominal filament width, can be changed with M404
-  bool filament_sensor = false;  //M405 turns on filament_sensor control, M406 turns it off
-  float filament_width_meas = DEFAULT_MEASURED_FILAMENT_DIA; //Stores the measured filament diameter
+  float filament_width_nominal=DEFAULT_NOMINAL_FILAMENT_DIA;  //Set nominal filament width, can be changed with M404
+  bool filament_sensor=false;  //M405 turns on filament_sensor control, M406 turns it off
+  float filament_width_meas=DEFAULT_MEASURED_FILAMENT_DIA; //Stores the measured filament diameter
   signed char measurement_delay[MAX_MEASUREMENT_DELAY+1];  //ring buffer to delay measurement  store extruder factor after subtracting 100
-  int delay_index1 = 0;  //index into ring buffer
-  int delay_index2 = -1;  //index into ring buffer - set to -1 on startup to indicate ring buffer needs to be initialized
-  float delay_dist = 0; //delay distance counter
+  int delay_index1=0;  //index into ring buffer
+  int delay_index2=-1;  //index into ring buffer - set to -1 on startup to indicate ring buffer needs to be initialized
+  float delay_dist=0; //delay distance counter
   int meas_delay_cm = MEASUREMENT_DELAY_CM;  //distance delay setting
 #endif
 
@@ -477,8 +476,6 @@ bool enquecommand(const char *cmd)
   return true;
 }
 
-
-
 void setup_killpin()
 {
   #if defined(KILL_PIN) && KILL_PIN > -1
@@ -519,8 +516,8 @@ void setup_powerhold()
   #if defined(SUICIDE_PIN) && SUICIDE_PIN > -1
     OUT_WRITE(SUICIDE_PIN, HIGH);
   #endif
-  #if HAS_POWER_SWITCH
-    #ifdef PS_DEFAULT_OFF
+  #if defined(PS_ON_PIN) && PS_ON_PIN > -1
+    #if defined(PS_DEFAULT_OFF)
       OUT_WRITE(PS_ON_PIN, PS_ON_ASLEEP);
     #else
       OUT_WRITE(PS_ON_PIN, PS_ON_AWAKE);
@@ -932,7 +929,7 @@ XYZ_CONSTS_FROM_CONFIG(signed char, home_dir,  HOME_DIR);
 
   static float x_home_pos(int extruder) {
     if (extruder == 0)
-    return base_home_pos(X_AXIS) + home_offset[X_AXIS];
+      return base_home_pos(X_AXIS) + home_offset[X_AXIS];
     else
       // In dual carriage mode the extruder offset provides an override of the
       // second X-carriage offset when homed - otherwise X2_HOME_POS is used.
@@ -961,15 +958,15 @@ static void axis_is_at_home(int axis) {
     if (axis == X_AXIS) {
       if (active_extruder != 0) {
         current_position[X_AXIS] = x_home_pos(active_extruder);
-        min_pos[X_AXIS] = X2_MIN_POS;
-        max_pos[X_AXIS] = max(extruder_offset[1][X_AXIS], X2_MAX_POS);
+                 min_pos[X_AXIS] = X2_MIN_POS;
+                 max_pos[X_AXIS] = max(extruder_offset[1][X_AXIS], X2_MAX_POS);
         return;
       }
       else if (dual_x_carriage_mode == DXC_DUPLICATION_MODE) {
-        current_position[X_AXIS] = base_home_pos(X_AXIS) + home_offset[X_AXIS];
-        min_pos[X_AXIS] = base_min_pos(X_AXIS) + home_offset[X_AXIS];
-        max_pos[X_AXIS] = min(base_max_pos(X_AXIS) + home_offset[X_AXIS],
-                                max(extruder_offset[1][X_AXIS], X2_MAX_POS) - duplicate_extruder_x_offset);
+        float xoff = home_offset[X_AXIS];
+        current_position[X_AXIS] = base_home_pos(X_AXIS) + xoff;
+                 min_pos[X_AXIS] = base_min_pos(X_AXIS) + xoff;
+                 max_pos[X_AXIS] = min(base_max_pos(X_AXIS) + xoff, max(extruder_offset[1][X_AXIS], X2_MAX_POS) - duplicate_extruder_x_offset);
         return;
       }
     }
@@ -1023,178 +1020,189 @@ static void axis_is_at_home(int axis) {
 }
 
 /**
- * Shorthand to tell the planner our current position (in mm).
+ * Some planner shorthand inline functions
  */
+inline void line_to_current_position() {
+  plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate/60, active_extruder);
+}
+inline void line_to_z(float zPosition) {
+  plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
+}
+inline void line_to_destination() {
+  plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+}
 inline void sync_plan_position() {
   plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
 }
 
 #ifdef ENABLE_AUTO_BED_LEVELING
-#ifdef AUTO_BED_LEVELING_GRID
 
-#ifndef DELTA
-  static void set_bed_level_equation_lsq(double *plane_equation_coefficients) {
-    vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1);
-    planeNormal.debug("planeNormal");
-    plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
-    //bedLevel.debug("bedLevel");
+  #ifdef AUTO_BED_LEVELING_GRID
 
-    //plan_bed_level_matrix.debug("bed level before");
-    //vector_3 uncorrected_position = plan_get_position_mm();
-    //uncorrected_position.debug("position before");
+    #ifndef DELTA
 
-    vector_3 corrected_position = plan_get_position();
-    //corrected_position.debug("position after");
-    current_position[X_AXIS] = corrected_position.x;
-    current_position[Y_AXIS] = corrected_position.y;
-    current_position[Z_AXIS] = zprobe_zoffset; // was: corrected_position.z
+      static void set_bed_level_equation_lsq(double *plane_equation_coefficients) {
+        vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1);
+        planeNormal.debug("planeNormal");
+        plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
+        //bedLevel.debug("bedLevel");
 
-    sync_plan_position();
+        //plan_bed_level_matrix.debug("bed level before");
+        //vector_3 uncorrected_position = plan_get_position_mm();
+        //uncorrected_position.debug("position before");
+
+        vector_3 corrected_position = plan_get_position();
+        //corrected_position.debug("position after");
+        current_position[X_AXIS] = corrected_position.x;
+        current_position[Y_AXIS] = corrected_position.y;
+        current_position[Z_AXIS] = zprobe_zoffset; // was: corrected_position.z
+
+        sync_plan_position();
+      }
+
+    #endif // !DELTA
+
+  #else // !AUTO_BED_LEVELING_GRID
+
+    static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float z_at_pt_3) {
+
+      plan_bed_level_matrix.set_to_identity();
+
+      vector_3 pt1 = vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, z_at_pt_1);
+      vector_3 pt2 = vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, z_at_pt_2);
+      vector_3 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();
+
+      if (planeNormal.z < 0) {
+        planeNormal.x = -planeNormal.x;
+        planeNormal.y = -planeNormal.y;
+        planeNormal.z = -planeNormal.z;
+      }
+
+      plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
+
+      vector_3 corrected_position = plan_get_position();
+      current_position[X_AXIS] = corrected_position.x;
+      current_position[Y_AXIS] = corrected_position.y;
+      current_position[Z_AXIS] = zprobe_zoffset; // was: corrected_position.z
+
+      sync_plan_position();
+    }
+
+  #endif // !AUTO_BED_LEVELING_GRID
+
+  static void run_z_probe() {
+
+    #ifdef DELTA
+
+      float start_z = current_position[Z_AXIS];
+      long start_steps = st_get_position(Z_AXIS);
+    
+      // move down slowly until you find the bed
+      feedrate = homing_feedrate[Z_AXIS] / 4;
+      destination[Z_AXIS] = -10;
+      prepare_move_raw();
+      st_synchronize();
+      endstops_hit_on_purpose();
+
+      // we have to let the planner know where we are right now as it is not where we said to go.
+      long stop_steps = st_get_position(Z_AXIS);
+      float mm = start_z - float(start_steps - stop_steps) / axis_steps_per_unit[Z_AXIS];
+      current_position[Z_AXIS] = mm;
+      calculate_delta(current_position);
+      plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
+
+    #else // !DELTA
+
+      plan_bed_level_matrix.set_to_identity();
+      feedrate = homing_feedrate[Z_AXIS];
+
+      // move down until you find the bed
+      float zPosition = -10;
+      line_to_z(zPosition);
+      st_synchronize();
+
+      // we have to let the planner know where we are right now as it is not where we said to go.
+      zPosition = st_get_position_mm(Z_AXIS);
+      plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS]);
+
+      // move up the retract distance
+      zPosition += home_retract_mm(Z_AXIS);
+      line_to_z(zPosition);
+      st_synchronize();
+      endstops_hit_on_purpose();
+
+      // move back down slowly to find bed
+      if (homing_bump_divisor[Z_AXIS] >= 1)
+        feedrate = homing_feedrate[Z_AXIS] / homing_bump_divisor[Z_AXIS];
+      else {
+        feedrate = homing_feedrate[Z_AXIS] / 10;
+        SERIAL_ECHOLN("Warning: The Homing Bump Feedrate Divisor cannot be less than 1");
+      }
+
+      zPosition -= home_retract_mm(Z_AXIS) * 2;
+      line_to_z(zPosition);
+      st_synchronize();
+      endstops_hit_on_purpose();
+
+      current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
+      // make sure the planner knows where we are as it may be a bit different than we last said to move to
+      sync_plan_position();
+
+    #endif // !DELTA
   }
-#endif
 
-#else // not AUTO_BED_LEVELING_GRID
-
-static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float z_at_pt_3) {
-
-    plan_bed_level_matrix.set_to_identity();
-
-    vector_3 pt1 = vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, z_at_pt_1);
-    vector_3 pt2 = vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, z_at_pt_2);
-    vector_3 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();
-
-    if (planeNormal.z < 0) {
-      planeNormal.x = -planeNormal.x;
-      planeNormal.y = -planeNormal.y;
-      planeNormal.z = -planeNormal.z;
-    }
-
-    plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
-
-    vector_3 corrected_position = plan_get_position();
-    current_position[X_AXIS] = corrected_position.x;
-    current_position[Y_AXIS] = corrected_position.y;
-    current_position[Z_AXIS] = zprobe_zoffset; // was: corrected_position.z
-
-    sync_plan_position();
-}
-
-#endif // AUTO_BED_LEVELING_GRID
-
-static void run_z_probe() {
-  #ifdef DELTA
-    
-    float start_z = current_position[Z_AXIS];
-    long start_steps = st_get_position(Z_AXIS);
-  
-    // move down slowly until you find the bed
-    feedrate = homing_feedrate[Z_AXIS] / 4;
-    destination[Z_AXIS] = -10;
-    prepare_move_raw();
-    st_synchronize();
-    endstops_hit_on_purpose();
-    
-    // we have to let the planner know where we are right now as it is not where we said to go.
-    long stop_steps = st_get_position(Z_AXIS);
-    float mm = start_z - float(start_steps - stop_steps) / axis_steps_per_unit[Z_AXIS];
-    current_position[Z_AXIS] = mm;
-    calculate_delta(current_position);
-    plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
-    
-  #else
-
-    plan_bed_level_matrix.set_to_identity();
-    feedrate = homing_feedrate[Z_AXIS];
-
-    // move down until you find the bed
-    float zPosition = -10;
-    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
-    st_synchronize();
-
-        // we have to let the planner know where we are right now as it is not where we said to go.
-    zPosition = st_get_position_mm(Z_AXIS);
-    plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS]);
-
-    // move up the retract distance
-    zPosition += home_retract_mm(Z_AXIS);
-    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
-    st_synchronize();
-    endstops_hit_on_purpose();
-
-    // move back down slowly to find bed
-    if (homing_bump_divisor[Z_AXIS] >= 1) {
-      feedrate = homing_feedrate[Z_AXIS]/homing_bump_divisor[Z_AXIS];
-    } 
-    else {
-      feedrate = homing_feedrate[Z_AXIS]/10;
-      SERIAL_ECHOLN("Warning: The Homing Bump Feedrate Divisor cannot be less then 1");
-    }
-
-    zPosition -= home_retract_mm(Z_AXIS) * 2;
-    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
-    st_synchronize();
-    endstops_hit_on_purpose();
-
-    current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
-    // make sure the planner knows where we are as it may be a bit different than we last said to move to
-    sync_plan_position();
-    
-  #endif
-}
-
-static void do_blocking_move_to(float x, float y, float z) {
+  static void do_blocking_move_to(float x, float y, float z) {
     float oldFeedRate = feedrate;
 
-#ifdef DELTA
+    #ifdef DELTA
 
-    feedrate = XY_TRAVEL_SPEED;
-    
-    destination[X_AXIS] = x;
-    destination[Y_AXIS] = y;
-    destination[Z_AXIS] = z;
-    prepare_move_raw();
-    st_synchronize();
+      feedrate = XY_TRAVEL_SPEED;
 
-#else
+      destination[X_AXIS] = x;
+      destination[Y_AXIS] = y;
+      destination[Z_AXIS] = z;
+      prepare_move_raw();
+      st_synchronize();
 
-    feedrate = homing_feedrate[Z_AXIS];
+    #else
 
-    current_position[Z_AXIS] = z;
-    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate/60, active_extruder);
-    st_synchronize();
+      feedrate = homing_feedrate[Z_AXIS];
 
-    feedrate = xy_travel_speed;
+      current_position[Z_AXIS] = z;
+      line_to_current_position();
+      st_synchronize();
 
-    current_position[X_AXIS] = x;
-    current_position[Y_AXIS] = y;
-    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate/60, active_extruder);
-    st_synchronize();
+      feedrate = xy_travel_speed;
 
-#endif
+      current_position[X_AXIS] = x;
+      current_position[Y_AXIS] = y;
+      line_to_current_position();
+      st_synchronize();
+
+    #endif
 
     feedrate = oldFeedRate;
-}
+  }
 
-static void setup_for_endstop_move() {
+  static void setup_for_endstop_move() {
     saved_feedrate = feedrate;
     saved_feedmultiply = feedmultiply;
     feedmultiply = 100;
     previous_millis_cmd = millis();
-
     enable_endstops(true);
-}
-
-static void clean_up_after_endstop_move() {
-#ifdef ENDSTOPS_ONLY_FOR_HOMING
-    enable_endstops(false);
-#endif
+  }
 
+  static void clean_up_after_endstop_move() {
+    #ifdef ENDSTOPS_ONLY_FOR_HOMING
+      enable_endstops(false);
+    #endif
     feedrate = saved_feedrate;
     feedmultiply = saved_feedmultiply;
     previous_millis_cmd = millis();
-}
+  }
 
+<<<<<<< HEAD
 static void engage_z_probe() {
   // Engage Z Servo endstop if enabled
   #ifdef SERVO_ENDSTOPS
@@ -1229,6 +1237,9 @@ static void engage_z_probe() {
     
     st_synchronize();
     
+    // If Z_PROBE_AND_ENDSTOP is changed to completely break it's bonds from Z_MIN_ENDSTOP and become
+    // it's own unique entity, then the following logic will need to be modified
+    // so it only uses the Z_PROBE
     #if defined(Z_PROBE_AND_ENDSTOP)
     bool z_probe_endstop = (READ(Z_PROBE_PIN) != Z_PROBE_ENDSTOP_INVERTING);
     if (z_probe_endstop)
@@ -1242,13 +1253,59 @@ static void engage_z_probe() {
             SERIAL_ERROR_START;
             SERIAL_ERRORLNPGM("Z-Probe failed to engage!");
             LCD_ALERTMESSAGEPGM("Err: ZPROBE");
+=======
+  static void engage_z_probe() {
+
+    #ifdef SERVO_ENDSTOPS
+
+      // Engage Z Servo endstop if enabled
+      if (servo_endstops[Z_AXIS] >= 0) {
+        #if SERVO_LEVELING
+          servos[servo_endstops[Z_AXIS]].attach(0);
+        #endif
+        servos[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2]);
+        #if SERVO_LEVELING
+          delay(PROBE_SERVO_DEACTIVATION_DELAY);
+          servos[servo_endstops[Z_AXIS]].detach();
+        #endif
+      }
+
+    #elif defined(Z_PROBE_ALLEN_KEY)
+
+      feedrate = homing_feedrate[X_AXIS];
+
+      // Move to the start position to initiate deployment
+      destination[X_AXIS] = Z_PROBE_ALLEN_KEY_DEPLOY_X;
+      destination[Y_AXIS] = Z_PROBE_ALLEN_KEY_DEPLOY_Y;
+      destination[Z_AXIS] = Z_PROBE_ALLEN_KEY_DEPLOY_Z;
+      prepare_move_raw();
+
+      // Home X to touch the belt
+      feedrate = homing_feedrate[X_AXIS]/10;
+      destination[X_AXIS] = 0;
+      prepare_move_raw();
+
+      // Home Y for safety
+      feedrate = homing_feedrate[X_AXIS]/2;
+      destination[Y_AXIS] = 0;
+      prepare_move_raw();
+
+      st_synchronize();
+
+      bool z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
+      if (z_min_endstop) {
+        if (!Stopped) {
+          SERIAL_ERROR_START;
+          SERIAL_ERRORLNPGM("Z-Probe failed to engage!");
+          LCD_ALERTMESSAGEPGM("Err: ZPROBE");
+>>>>>>> MarlinFirmware/Development
         }
         Stop();
-    }
-  #endif
+      }
 
-}
+    #endif // Z_PROBE_ALLEN_KEY
 
+<<<<<<< HEAD
 static void retract_z_probe() {
   // Retract Z Servo endstop if enabled
   #ifdef SERVO_ENDSTOPS
@@ -1298,6 +1355,9 @@ static void retract_z_probe() {
     
     st_synchronize();
     
+    // If Z_PROBE_AND_ENDSTOP is changed to completely break it's bonds from Z_MIN_ENDSTOP and become
+    // it's own unique entity, then the following logic will need to be modified
+    // so it only uses the Z_PROBE
     #if defined(Z_PROBE_AND_ENDSTOP)
     bool z_probe_endstop = (READ(Z_PROBE_PIN) != Z_PROBE_ENDSTOP_INVERTING);
     if (z_probe_endstop)
@@ -1311,126 +1371,219 @@ static void retract_z_probe() {
             SERIAL_ERROR_START;
             SERIAL_ERRORLNPGM("Z-Probe failed to retract!");
             LCD_ALERTMESSAGEPGM("Err: ZPROBE");
+=======
+  }
+
+  static void retract_z_probe(const float z_after=Z_RAISE_AFTER_PROBING) {
+
+    #ifdef SERVO_ENDSTOPS
+
+      // Retract Z Servo endstop if enabled
+      if (servo_endstops[Z_AXIS] >= 0) {
+
+        if (z_after > 0) {
+          do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_after);
+          st_synchronize();
+>>>>>>> MarlinFirmware/Development
+        }
+
+        #if SERVO_LEVELING
+          servos[servo_endstops[Z_AXIS]].attach(0);
+        #endif
+
+        servos[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2 + 1]);
+
+        #if SERVO_LEVELING
+          delay(PROBE_SERVO_DEACTIVATION_DELAY);
+          servos[servo_endstops[Z_AXIS]].detach();
+        #endif
+      }
+
+    #elif defined(Z_PROBE_ALLEN_KEY)
+
+      // Move up for safety
+      feedrate = homing_feedrate[X_AXIS];
+      destination[Z_AXIS] = current_position[Z_AXIS] + Z_RAISE_AFTER_PROBING;
+      prepare_move_raw();
+
+      // Move to the start position to initiate retraction
+      destination[X_AXIS] = Z_PROBE_ALLEN_KEY_RETRACT_X;
+      destination[Y_AXIS] = Z_PROBE_ALLEN_KEY_RETRACT_Y;
+      destination[Z_AXIS] = Z_PROBE_ALLEN_KEY_RETRACT_Z;
+      prepare_move_raw();
+
+      // Move the nozzle down to push the probe into retracted position
+      feedrate = homing_feedrate[Z_AXIS]/10;
+      destination[Z_AXIS] = current_position[Z_AXIS] - Z_PROBE_ALLEN_KEY_RETRACT_DEPTH;
+      prepare_move_raw();
+
+      // Move up for safety
+      feedrate = homing_feedrate[Z_AXIS]/2;
+      destination[Z_AXIS] = current_position[Z_AXIS] + Z_PROBE_ALLEN_KEY_RETRACT_DEPTH * 2;
+      prepare_move_raw();
+
+      // Home XY for safety
+      feedrate = homing_feedrate[X_AXIS]/2;
+      destination[X_AXIS] = 0;
+      destination[Y_AXIS] = 0;
+      prepare_move_raw();
+
+      st_synchronize();
+
+      // If Z_PROBE_AND_ENDSTOP is changed to completely break it's bonds from Z_MIN_ENDSTOP and become
+      // it's own unique entity, then the following logic will need to be modified
+      // so it only uses the Z_PROBE
+      bool z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
+      if (!z_min_endstop) {
+        if (!Stopped) {
+          SERIAL_ERROR_START;
+          SERIAL_ERRORLNPGM("Z-Probe failed to retract!");
+          LCD_ALERTMESSAGEPGM("Err: ZPROBE");
         }
         Stop();
+      }
+
+    #endif
+
+  }
+
+  enum ProbeAction {
+    ProbeStay             = 0,
+    ProbeEngage           = BIT(0),
+    ProbeRetract          = BIT(1),
+    ProbeEngageAndRetract = (ProbeEngage | ProbeRetract)
+  };
+
+  // Probe bed height at position (x,y), returns the measured z value
+  static float probe_pt(float x, float y, float z_before, ProbeAction retract_action=ProbeEngageAndRetract, int verbose_level=1) {
+    // move to right place
+    do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_before);
+    do_blocking_move_to(x - X_PROBE_OFFSET_FROM_EXTRUDER, y - Y_PROBE_OFFSET_FROM_EXTRUDER, current_position[Z_AXIS]);
+
+    #if !defined(Z_PROBE_SLED) && !defined(Z_PROBE_ALLEN_KEY)
+      if (retract_action & ProbeEngage) engage_z_probe();
+    #endif
+
+    run_z_probe();
+    float measured_z = current_position[Z_AXIS];
+
+    #if !defined(Z_PROBE_SLED) && !defined(Z_PROBE_ALLEN_KEY)
+      if (retract_action & ProbeRetract) retract_z_probe(z_before);
+    #endif
+
+    if (verbose_level > 2) {
+      SERIAL_PROTOCOLPGM(MSG_BED);
+      SERIAL_PROTOCOLPGM(" X: ");
+      SERIAL_PROTOCOL_F(x, 3);
+      SERIAL_PROTOCOLPGM(" Y: ");
+      SERIAL_PROTOCOL_F(y, 3);
+      SERIAL_PROTOCOLPGM(" Z: ");
+      SERIAL_PROTOCOL_F(measured_z, 3);
+      SERIAL_EOL;
     }
-  #endif
-
-}
-
-enum ProbeAction {
-  ProbeStay             = 0,
-  ProbeEngage           = BIT(0),
-  ProbeRetract          = BIT(1),
-  ProbeEngageAndRetract = (ProbeEngage | ProbeRetract)
-};
-
-/// Probe bed height at position (x,y), returns the measured z value
-static float probe_pt(float x, float y, float z_before, ProbeAction retract_action=ProbeEngageAndRetract, int verbose_level=1) {
-  // move to right place
-  do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_before);
-  do_blocking_move_to(x - X_PROBE_OFFSET_FROM_EXTRUDER, y - Y_PROBE_OFFSET_FROM_EXTRUDER, current_position[Z_AXIS]);
-
-  #if !defined(Z_PROBE_SLED) && !defined(Z_PROBE_ALLEN_KEY)
-    if (retract_action & ProbeEngage) engage_z_probe();
-  #endif
-
-  run_z_probe();
-  float measured_z = current_position[Z_AXIS];
-
-  #if !defined(Z_PROBE_SLED) && !defined(Z_PROBE_ALLEN_KEY)
-    if (retract_action & ProbeRetract) retract_z_probe();
-  #endif
-
-  if (verbose_level > 2) {
-    SERIAL_PROTOCOLPGM(MSG_BED);
-    SERIAL_PROTOCOLPGM(" X: ");
-    SERIAL_PROTOCOL_F(x, 3);
-    SERIAL_PROTOCOLPGM(" Y: ");
-    SERIAL_PROTOCOL_F(y, 3);
-    SERIAL_PROTOCOLPGM(" Z: ");
-    SERIAL_PROTOCOL_F(measured_z, 3);
-    SERIAL_EOL;
+    return measured_z;
   }
-  return measured_z;
-}
 
-#ifdef DELTA
-static void extrapolate_one_point(int x, int y, int xdir, int ydir) {
-  if (bed_level[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 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[x][y] = median;
-}
+  #ifdef DELTA
 
-// Fill in the unprobed points (corners of circular print surface)
-// 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++) {
-      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);
+    /**
+     * 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) {
+        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 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[x][y] = median;
     }
-  }
-}
 
-// 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++) {
-      SERIAL_PROTOCOL_F(bed_level[x][y], 2);
-      SERIAL_PROTOCOLPGM(" ");
+    // Fill in the unprobed points (corners of circular print surface)
+    // 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++) {
+          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);
+        }
+      }
     }
-    SERIAL_ECHOLN("");
-  }
-}
 
-// Reset calibration results to zero.
-void reset_bed_level() {
-  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;
+    // 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++) {
+          SERIAL_PROTOCOL_F(bed_level[x][y], 2);
+          SERIAL_PROTOCOLPGM(" ");
+        }
+        SERIAL_ECHOLN("");
+      }
     }
-  }
-}
 
-#endif // DELTA
+    // Reset calibration results to zero.
+    void reset_bed_level() {
+      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;
+        }
+      }
+    }
+
+  #endif // DELTA
 
 #endif // ENABLE_AUTO_BED_LEVELING
 
 static void homeaxis(int axis) {
-#define HOMEAXIS_DO(LETTER) \
-  ((LETTER##_MIN_PIN > -1 && LETTER##_HOME_DIR==-1) || (LETTER##_MAX_PIN > -1 && LETTER##_HOME_DIR==1))
+  #define HOMEAXIS_DO(LETTER) \
+    ((LETTER##_MIN_PIN > -1 && LETTER##_HOME_DIR==-1) || (LETTER##_MAX_PIN > -1 && LETTER##_HOME_DIR==1))
 
-  if (axis==X_AXIS ? HOMEAXIS_DO(X) :
-      axis==Y_AXIS ? HOMEAXIS_DO(Y) :
-      axis==Z_AXIS ? HOMEAXIS_DO(Z) :
-      0) {
-    int axis_home_dir = home_dir(axis);
-#ifdef DUAL_X_CARRIAGE
-    if (axis == X_AXIS)
-      axis_home_dir = x_home_dir(active_extruder);
-#endif
+  if (axis == X_AXIS ? HOMEAXIS_DO(X) :
+      axis == Y_AXIS ? HOMEAXIS_DO(Y) :
+      axis == Z_AXIS ? HOMEAXIS_DO(Z) : 0) {
+
+    int axis_home_dir;
+
+    #ifdef DUAL_X_CARRIAGE
+      if (axis == X_AXIS) axis_home_dir = x_home_dir(active_extruder);
+    #else
+      axis_home_dir = home_dir(axis);
+    #endif
 
     current_position[axis] = 0;
     sync_plan_position();
 
+    #ifndef Z_PROBE_SLED
+      // Engage Servo endstop if enabled
+      #ifdef SERVO_ENDSTOPS
+        #if SERVO_LEVELING
+          if (axis == Z_AXIS) {
+            engage_z_probe();
+          }
+          else
+        #endif // SERVO_LEVELING
 
+        if (servo_endstops[axis] > -1)
+          servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2]);
+
+      #endif // SERVO_ENDSTOPS
+
+    #endif // Z_PROBE_SLED
+
+<<<<<<< HEAD
 #ifndef Z_PROBE_SLED
     // Engage Servo endstop if enabled and we are not using Z_PROBE_AND_ENDSTOP unless we are using Z_SAFE_HOMING
     #ifdef SERVO_ENDSTOPS && (defined (Z_SAFE_HOMING) || ! defined (Z_PROBE_AND_ENDSTOP))
@@ -1445,33 +1598,33 @@ static void homeaxis(int axis) {
       }
     #endif
 #endif // Z_PROBE_SLED
+=======
+>>>>>>> MarlinFirmware/Development
     #ifdef Z_DUAL_ENDSTOPS
-      if (axis==Z_AXIS) In_Homing_Process(true);
+      if (axis == Z_AXIS) In_Homing_Process(true);
     #endif
+
     destination[axis] = 1.5 * max_length(axis) * axis_home_dir;
     feedrate = homing_feedrate[axis];
-    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+    line_to_destination();
     st_synchronize();
 
     current_position[axis] = 0;
     sync_plan_position();
     destination[axis] = -home_retract_mm(axis) * axis_home_dir;
-    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+    line_to_destination();
     st_synchronize();
 
-    destination[axis] = 2*home_retract_mm(axis) * axis_home_dir;
+    destination[axis] = 2 * home_retract_mm(axis) * axis_home_dir;
 
     if (homing_bump_divisor[axis] >= 1)
-    {
-        feedrate = homing_feedrate[axis]/homing_bump_divisor[axis];
-    } 
-    else
-    {
-        feedrate = homing_feedrate[axis]/10;
-        SERIAL_ECHOLN("Warning: The Homing Bump Feedrate Divisor cannot be less then 1");
+      feedrate = homing_feedrate[axis] / homing_bump_divisor[axis];
+    else {
+      feedrate = homing_feedrate[axis] / 10;
+      SERIAL_ECHOLN("Warning: The Homing Bump Feedrate Divisor cannot be less than 1");
     }
 
-    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+    line_to_destination();
     st_synchronize();
     #ifdef Z_DUAL_ENDSTOPS
       if (axis==Z_AXIS)
@@ -1486,7 +1639,7 @@ static void homeaxis(int axis) {
           destination[axis] = fabs(z_endstop_adj);
           if (z_endstop_adj < 0) Lock_z_motor(true); else Lock_z2_motor(true);        
         }
-        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+        line_to_destination();
         st_synchronize();
         Lock_z_motor(false);
         Lock_z2_motor(false);
@@ -1499,7 +1652,7 @@ static void homeaxis(int axis) {
     if (endstop_adj[axis] * axis_home_dir < 0) {
       sync_plan_position();
       destination[axis] = endstop_adj[axis];
-      plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+      line_to_destination();
       st_synchronize();
     }
 #endif
@@ -1544,7 +1697,7 @@ void refresh_cmd_timeout(void)
       }
       plan_set_e_position(current_position[E_AXIS]);
       float oldFeedrate = feedrate;
-      feedrate=retract_feedrate*60;
+      feedrate = retract_feedrate * 60;
       retracted[active_extruder]=true;
       prepare_move();
       if(retract_zlift > 0.01) {
@@ -1580,8 +1733,8 @@ void refresh_cmd_timeout(void)
       }
       plan_set_e_position(current_position[E_AXIS]);
       float oldFeedrate = feedrate;
-      feedrate=retract_recover_feedrate*60;
-      retracted[active_extruder]=false;
+      feedrate = retract_recover_feedrate * 60;
+      retracted[active_extruder] = false;
       prepare_move();
       feedrate = oldFeedrate;
     }
@@ -1735,17 +1888,16 @@ inline void gcode_G4() {
  */
 inline void gcode_G28() {
   #ifdef ENABLE_AUTO_BED_LEVELING
+    plan_bed_level_matrix.set_to_identity();  //Reset the plane ("erase" all leveling data)
     #ifdef DELTA
       reset_bed_level();
-    #else
-      plan_bed_level_matrix.set_to_identity();  //Reset the plane ("erase" all leveling data)
     #endif
   #endif
 
   #if defined(MESH_BED_LEVELING)
     uint8_t mbl_was_active = mbl.active;
     mbl.active = 0;
-  #endif  // MESH_BED_LEVELING
+  #endif
 
   saved_feedrate = feedrate;
   saved_feedmultiply = feedmultiply;
@@ -1768,7 +1920,7 @@ inline void gcode_G28() {
 
     for (int i = X_AXIS; i <= Z_AXIS; i++) destination[i] = 3 * Z_MAX_LENGTH;
     feedrate = 1.732 * homing_feedrate[X_AXIS];
-    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+    line_to_destination();
     st_synchronize();
     endstops_hit_on_purpose();
 
@@ -1816,7 +1968,7 @@ inline void gcode_G28() {
         } else {
           feedrate *= sqrt(pow(max_length(X_AXIS) / max_length(Y_AXIS), 2) + 1);
         }
-        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+        line_to_destination();
         st_synchronize();
 
         axis_is_at_home(X_AXIS);
@@ -1824,7 +1976,7 @@ inline void gcode_G28() {
         sync_plan_position();
         destination[X_AXIS] = current_position[X_AXIS];
         destination[Y_AXIS] = current_position[Y_AXIS];
-        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+        line_to_destination();
         feedrate = 0.0;
         st_synchronize();
         endstops_hit_on_purpose();
@@ -1891,7 +2043,7 @@ inline void gcode_G28() {
           #if defined(Z_RAISE_BEFORE_HOMING) && Z_RAISE_BEFORE_HOMING > 0
             destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS);    // Set destination away from bed
             feedrate = max_feedrate[Z_AXIS];
-            plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
+            line_to_destination();
             st_synchronize();
           #endif
           HOMEAXIS(Z);
@@ -1903,11 +2055,11 @@ inline void gcode_G28() {
           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] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS);    // Set destination away from bed
-          feedrate = XY_TRAVEL_SPEED / 60;
+          feedrate = XY_TRAVEL_SPEED;
           current_position[Z_AXIS] = 0;
 
           sync_plan_position();
-          plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
+          line_to_destination();
           st_synchronize();
           current_position[X_AXIS] = destination[X_AXIS];
           current_position[Y_AXIS] = destination[Y_AXIS];
@@ -1929,7 +2081,7 @@ inline void gcode_G28() {
               plan_set_position(cpx, cpy, current_position[Z_AXIS], current_position[E_AXIS]);
               destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS);    // Set destination away from bed
               feedrate = max_feedrate[Z_AXIS];
-              plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
+              line_to_destination();
               st_synchronize();
               HOMEAXIS(Z);
             }
@@ -1982,7 +2134,7 @@ inline void gcode_G28() {
       destination[Z_AXIS] = current_position[Z_AXIS];
       destination[E_AXIS] = current_position[E_AXIS];
       feedrate = homing_feedrate[X_AXIS];
-      plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
+      line_to_destination();
       st_synchronize();
       current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
       sync_plan_position();
@@ -1996,6 +2148,19 @@ inline void gcode_G28() {
   endstops_hit_on_purpose();
 }
 
+#if defined(MESH_BED_LEVELING) || defined(ENABLE_AUTO_BED_LEVELING)
+
+  // Check for known positions in X and Y
+  inline bool can_run_bed_leveling() {
+  	if (axis_known_position[X_AXIS] && axis_known_position[Y_AXIS]) return true;
+    LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
+    SERIAL_ECHO_START;
+    SERIAL_ECHOLNPGM(MSG_POSITION_UNKNOWN);
+    return false;
+  }
+
+#endif // MESH_BED_LEVELING || ENABLE_AUTO_BED_LEVELING
+
 #ifdef MESH_BED_LEVELING
 
   /**
@@ -2010,6 +2175,10 @@ inline void gcode_G28() {
    *
    */
   inline void gcode_G29() {
+
+    // Prevent leveling without first homing in X and Y
+    if (!can_run_bed_leveling()) return;
+
     static int probe_point = -1;
     int state = 0;
     if (code_seen('S') || code_seen('s')) {
@@ -2126,13 +2295,8 @@ inline void gcode_G28() {
    */
   inline void gcode_G29() {
 
-    // Prevent user from running a G29 without first homing in X and Y
-    if (!axis_known_position[X_AXIS] || !axis_known_position[Y_AXIS]) {
-      LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
-      SERIAL_ECHO_START;
-      SERIAL_ECHOLNPGM(MSG_POSITION_UNKNOWN);
-      return;
-    }
+    // Prevent leveling without first homing in X and Y
+    if (!can_run_bed_leveling()) return;
 
     int verbose_level = 1;
 
@@ -2214,16 +2378,15 @@ inline void gcode_G28() {
 
     st_synchronize();
 
-    if (!dryrun)
-    {
+    if (!dryrun) {
+      // make sure the bed_level_rotation_matrix is identity or the planner will get it wrong
+      plan_bed_level_matrix.set_to_identity();
+
       #ifdef DELTA
         reset_bed_level();
       #else //!DELTA
-
-        // make sure the bed_level_rotation_matrix is identity or the planner will get it incorectly
         //vector_3 corrected_position = plan_get_position_mm();
         //corrected_position.debug("position before G29");
-        plan_bed_level_matrix.set_to_identity();
         vector_3 uncorrected_position = plan_get_position();
         //uncorrected_position.debug("position during G29");
         current_position[X_AXIS] = uncorrected_position.x;
@@ -2231,7 +2394,7 @@ inline void gcode_G28() {
         current_position[Z_AXIS] = uncorrected_position.z;
         sync_plan_position();
 
-      #endif
+      #endif // !DELTA
     }
     
     setup_for_endstop_move();
@@ -2292,13 +2455,12 @@ inline void gcode_G28() {
 
           // raise extruder
           float measured_z,
-                z_before = probePointCounter == 0 ? Z_RAISE_BEFORE_PROBING : current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS;
+                z_before = Z_RAISE_BETWEEN_PROBINGS + (probePointCounter ? current_position[Z_AXIS] : 0);
 
           #ifdef DELTA
             // Avoid probing the corners (outside the round or hexagon print surface) on a delta printer.
             float distance_from_center = sqrt(xProbe*xProbe + yProbe*yProbe);
-            if (distance_from_center > DELTA_PROBABLE_RADIUS)
-              continue;
+            if (distance_from_center > DELTA_PROBABLE_RADIUS) continue;
           #endif //DELTA
 
           // Enhanced G29 - Do not retract servo between probes
@@ -2326,6 +2488,11 @@ inline void gcode_G28() {
           #endif
 
           probePointCounter++;
+
+          manage_heater();
+          manage_inactivity();
+          lcd_update();
+
         } //xProbe
       } //yProbe
 
@@ -2412,16 +2579,14 @@ inline void gcode_G28() {
       if (verbose_level > 0)
         plan_bed_level_matrix.debug(" \n\nBed Level Correction Matrix:");
 
-      // Correct the Z height difference from z-probe position and hotend tip position.
-      // The Z height on homing is measured by Z-Probe, but the probe is quite far from the hotend.
-      // When the bed is uneven, this height must be corrected.
-      if (!dryrun)
-      {
-        float x_tmp, y_tmp, z_tmp, real_z;
-        real_z = float(st_get_position(Z_AXIS)) / axis_steps_per_unit[Z_AXIS];  //get the real Z (since the auto bed leveling is already correcting the plane)
-        x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER;
-        y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER;
-        z_tmp = current_position[Z_AXIS];
+      if (!dryrun) {
+        // Correct the Z height difference from z-probe position and hotend tip position.
+        // The Z height on homing is measured by Z-Probe, but the probe is quite far from the hotend.
+        // When the bed is uneven, this height must be corrected.
+        float x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER,
+              y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER,
+              z_tmp = current_position[Z_AXIS],
+              real_z = (float)st_get_position(Z_AXIS) / axis_steps_per_unit[Z_AXIS];  //get the real Z (since the auto bed leveling is already correcting the plane)
 
         apply_rotation_xyz(plan_bed_level_matrix, x_tmp, y_tmp, z_tmp);         //Apply the correction sending the probe offset
         current_position[Z_AXIS] = z_tmp - real_z + current_position[Z_AXIS];   //The difference is added to current position and sent to planner.
@@ -2757,11 +2922,13 @@ inline void gcode_M42() {
   } // code_seen('S')
 }
 
-
+// If Z_PROBE_AND_ENDSTOP is changed to completely break it's bonds from Z_MIN_ENDSTOP and become
+// it's own unique entity, then the following logic will need to be modified
+// so it only uses the Z_PROBE
 #if defined(ENABLE_AUTO_BED_LEVELING) && defined(Z_PROBE_REPEATABILITY_TEST)
 
-  #if Z_MIN_PIN == -1
-    #error "You must have a Z_MIN endstop in order to enable calculation of Z-Probe repeatability."
+  #if (Z_MIN_PIN == -1) && (! defined (Z_PROBE_PIN) || Z_PROBE_PIN == -1)
+    #error "You must have a Z_MIN or Z_PROBE endstop in order to enable calculation of Z-Probe repeatability."
   #endif
 
   /**
@@ -3267,7 +3434,7 @@ inline void gcode_M140() {
   if (code_seen('S')) setTargetBed(code_value());
 }
 
-#if HAS_POWER_SWITCH
+#if defined(PS_ON_PIN) && PS_ON_PIN > -1
 
   /**
    * M80: Turn on Power Supply
@@ -3289,12 +3456,10 @@ inline void gcode_M140() {
     #endif
   }
 
-#endif // HAS_POWER_SWITCH
+#endif // PS_ON_PIN
 
 /**
- * M81: Turn off Power, including Power Supply, if there is one.
- *
- *      This code should ALWAYS be available for EMERGENCY SHUTDOWN!
+ * M81: Turn off Power Supply
  */
 inline void gcode_M81() {
   disable_heater();
@@ -3309,19 +3474,16 @@ inline void gcode_M81() {
   #if defined(SUICIDE_PIN) && SUICIDE_PIN > -1
     st_synchronize();
     suicide();
-  #elif HAS_POWER_SWITCH
+  #elif defined(PS_ON_PIN) && PS_ON_PIN > -1
     OUT_WRITE(PS_ON_PIN, PS_ON_ASLEEP);
   #endif
   #ifdef ULTIPANEL
-    #if HAS_POWER_SWITCH
-      powersupply = false;
-    #endif
+    powersupply = false;
     LCD_MESSAGEPGM(MACHINE_NAME " " MSG_OFF ".");
     lcd_update();
   #endif
 }
 
-
 /**
  * M82: Set E codes absolute (default)
  */
@@ -3490,7 +3652,7 @@ inline void gcode_M119() {
     SERIAL_PROTOCOLPGM(MSG_Z2_MAX);
     SERIAL_PROTOCOLLN(((READ(Z2_MAX_PIN)^Z2_MAX_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
   #endif
-  #if defined(Z_PROBE_PIN) && Z_PROBE_PIN >-1
+  #if defined(Z_PROBE_PIN) && Z_PROBE_PIN > -1
     SERIAL_PROTOCOLPGM(MSG_Z_PROBE);
     SERIAL_PROTOCOLLN(((READ(Z_PROBE_PIN)^Z_PROBE_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
   #endif
@@ -3794,7 +3956,7 @@ inline void gcode_M221() {
       extruder_multiply[tmp_extruder] = sval;
     }
     else {
-      extrudemultiply = sval;
+      extruder_multiply[active_extruder] = sval;
     }
   }
 }
@@ -4231,7 +4393,7 @@ inline void gcode_M400() { st_synchronize(); }
     //SERIAL_PROTOCOLPGM("Filament dia (measured mm):");
     //SERIAL_PROTOCOL(filament_width_meas);
     //SERIAL_PROTOCOLPGM("Extrusion ratio(%):");
-    //SERIAL_PROTOCOL(extrudemultiply);
+    //SERIAL_PROTOCOL(extruder_multiply[active_extruder]);
   }
 
   /**
@@ -4704,18 +4866,14 @@ void process_commands() {
       gcode_G28();
       break;
 
-    #if defined(MESH_BED_LEVELING)
-      case 29: // G29 Handle mesh based leveling
+    #if defined(ENABLE_AUTO_BED_LEVELING) || defined(MESH_BED_LEVELING)
+      case 29: // G29 Detailed Z-Probe, probes the bed at 3 or more points.
         gcode_G29();
         break;
     #endif
 
     #ifdef ENABLE_AUTO_BED_LEVELING
 
-      case 29: // G29 Detailed Z-Probe, probes the bed at 3 or more points.
-        gcode_G29();
-        break;
-
       #ifndef Z_PROBE_SLED
 
         case 30: // G30 Single Z Probe
@@ -4862,15 +5020,15 @@ void process_commands() {
         #endif //HEATER_2_PIN
       #endif //BARICUDA
 
-      #if HAS_POWER_SWITCH
+      #if defined(PS_ON_PIN) && PS_ON_PIN > -1
 
         case 80: // M80 - Turn on Power Supply
           gcode_M80();
           break;
 
-      #endif // HAS_POWER_SWITCH
+      #endif // PS_ON_PIN
 
-      case 81: // M81 - Turn off Power, including Power Supply, if possible
+      case 81: // M81 - Turn off Power Supply
         gcode_M81();
         break;
 
@@ -5410,69 +5568,72 @@ void prepare_move()
   
   #ifdef SCARA //for now same as delta-code
 
-float difference[NUM_AXIS];
-for (int8_t i=0; i < NUM_AXIS; i++) {
-  difference[i] = destination[i] - current_position[i];
-}
+    float difference[NUM_AXIS];
+    for (int8_t i = 0; i < NUM_AXIS; i++) difference[i] = destination[i] - current_position[i];
 
-float cartesian_mm = sqrt(  sq(difference[X_AXIS]) +
-              sq(difference[Y_AXIS]) +
-              sq(difference[Z_AXIS]));
-if (cartesian_mm < 0.000001) { cartesian_mm = abs(difference[E_AXIS]); }
-if (cartesian_mm < 0.000001) { return; }
-float seconds = 6000 * cartesian_mm / feedrate / feedmultiply;
-int steps = max(1, int(scara_segments_per_second * seconds));
- //SERIAL_ECHOPGM("mm="); SERIAL_ECHO(cartesian_mm);
- //SERIAL_ECHOPGM(" seconds="); SERIAL_ECHO(seconds);
- //SERIAL_ECHOPGM(" steps="); SERIAL_ECHOLN(steps);
-for (int s = 1; s <= steps; s++) {
-  float fraction = float(s) / float(steps);
-  for(int8_t i=0; i < NUM_AXIS; i++) {
-    destination[i] = current_position[i] + difference[i] * fraction;
-  }
+    float cartesian_mm = sqrt(  sq(difference[X_AXIS]) +
+                                sq(difference[Y_AXIS]) +
+                                sq(difference[Z_AXIS]));
+    if (cartesian_mm < 0.000001) { cartesian_mm = abs(difference[E_AXIS]); }
+    if (cartesian_mm < 0.000001) { return; }
+    float seconds = 6000 * cartesian_mm / feedrate / feedmultiply;
+    int steps = max(1, int(scara_segments_per_second * seconds));
 
+    //SERIAL_ECHOPGM("mm="); SERIAL_ECHO(cartesian_mm);
+    //SERIAL_ECHOPGM(" seconds="); SERIAL_ECHO(seconds);
+    //SERIAL_ECHOPGM(" steps="); SERIAL_ECHOLN(steps);
+
+    for (int s = 1; s <= steps; s++) {
+      float fraction = float(s) / float(steps);
+      for(int8_t i = 0; i < NUM_AXIS; i++) {
+        destination[i] = current_position[i] + difference[i] * fraction;
+      }
   
-  calculate_delta(destination);
-         //SERIAL_ECHOPGM("destination[X_AXIS]="); SERIAL_ECHOLN(destination[X_AXIS]);
-         //SERIAL_ECHOPGM("destination[Y_AXIS]="); SERIAL_ECHOLN(destination[Y_AXIS]);
-         //SERIAL_ECHOPGM("destination[Z_AXIS]="); SERIAL_ECHOLN(destination[Z_AXIS]);
-         //SERIAL_ECHOPGM("delta[X_AXIS]="); SERIAL_ECHOLN(delta[X_AXIS]);
-         //SERIAL_ECHOPGM("delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]);
-         //SERIAL_ECHOPGM("delta[Z_AXIS]="); SERIAL_ECHOLN(delta[Z_AXIS]);
-         
-  plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],
-  destination[E_AXIS], feedrate*feedmultiply/60/100.0,
-  active_extruder);
-}
-#endif // SCARA
-  
-#ifdef DELTA
-  float difference[NUM_AXIS];
-  for (int8_t i=0; i < NUM_AXIS; i++) {
-    difference[i] = destination[i] - current_position[i];
-  }
-  float cartesian_mm = sqrt(sq(difference[X_AXIS]) +
-                            sq(difference[Y_AXIS]) +
-                            sq(difference[Z_AXIS]));
-  if (cartesian_mm < 0.000001) { cartesian_mm = abs(difference[E_AXIS]); }
-  if (cartesian_mm < 0.000001) { return; }
-  float seconds = 6000 * cartesian_mm / feedrate / feedmultiply;
-  int steps = max(1, int(delta_segments_per_second * seconds));
-  // SERIAL_ECHOPGM("mm="); SERIAL_ECHO(cartesian_mm);
-  // SERIAL_ECHOPGM(" seconds="); SERIAL_ECHO(seconds);
-  // SERIAL_ECHOPGM(" steps="); SERIAL_ECHOLN(steps);
-  for (int s = 1; s <= steps; s++) {
-    float fraction = float(s) / float(steps);
-    for(int8_t i=0; i < NUM_AXIS; i++) {
-      destination[i] = current_position[i] + difference[i] * fraction;
+      calculate_delta(destination);
+      //SERIAL_ECHOPGM("destination[X_AXIS]="); SERIAL_ECHOLN(destination[X_AXIS]);
+      //SERIAL_ECHOPGM("destination[Y_AXIS]="); SERIAL_ECHOLN(destination[Y_AXIS]);
+      //SERIAL_ECHOPGM("destination[Z_AXIS]="); SERIAL_ECHOLN(destination[Z_AXIS]);
+      //SERIAL_ECHOPGM("delta[X_AXIS]="); SERIAL_ECHOLN(delta[X_AXIS]);
+      //SERIAL_ECHOPGM("delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]);
+      //SERIAL_ECHOPGM("delta[Z_AXIS]="); SERIAL_ECHOLN(delta[Z_AXIS]);
+
+      plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],
+        destination[E_AXIS], feedrate*feedmultiply/60/100.0,
+        active_extruder);
     }
-    calculate_delta(destination);
-    plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],
-                     destination[E_AXIS], feedrate*feedmultiply/60/100.0,
-                     active_extruder);
-  }
+
+  #endif // SCARA
   
-#endif // DELTA
+  #ifdef DELTA
+
+    float difference[NUM_AXIS];
+    for (int8_t i=0; i < NUM_AXIS; i++) difference[i] = destination[i] - current_position[i];
+
+    float cartesian_mm = sqrt(sq(difference[X_AXIS]) +
+                              sq(difference[Y_AXIS]) +
+                              sq(difference[Z_AXIS]));
+    if (cartesian_mm < 0.000001) cartesian_mm = abs(difference[E_AXIS]);
+    if (cartesian_mm < 0.000001) return;
+    float seconds = 6000 * cartesian_mm / feedrate / feedmultiply;
+    int steps = max(1, int(delta_segments_per_second * seconds));
+
+    // SERIAL_ECHOPGM("mm="); SERIAL_ECHO(cartesian_mm);
+    // SERIAL_ECHOPGM(" seconds="); SERIAL_ECHO(seconds);
+    // SERIAL_ECHOPGM(" steps="); SERIAL_ECHOLN(steps);
+
+    for (int s = 1; s <= steps; s++) {
+      float fraction = float(s) / float(steps);
+      for (int8_t i = 0; i < NUM_AXIS; i++) destination[i] = current_position[i] + difference[i] * fraction;
+      calculate_delta(destination);
+      #ifdef ENABLE_AUTO_BED_LEVELING
+        adjust_delta(destination);
+      #endif
+      plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],
+                       destination[E_AXIS], feedrate*feedmultiply/60/100.0,
+                       active_extruder);
+    }
+
+  #endif // DELTA
 
 #ifdef DUAL_X_CARRIAGE
   if (active_extruder_parked)
@@ -5518,13 +5679,13 @@ for (int s = 1; s <= steps; s++) {
 #if ! (defined DELTA || defined SCARA)
   // Do not use feedmultiply for E or Z only moves
   if( (current_position[X_AXIS] == destination [X_AXIS]) && (current_position[Y_AXIS] == destination [Y_AXIS])) {
-    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+    line_to_destination();
   } else {
 #if defined(MESH_BED_LEVELING)
-    mesh_plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder);
+    mesh_plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], (feedrate/60)*(feedmultiply/100.0), active_extruder);
     return;
 #else
-    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder);
+    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], (feedrate/60)*(feedmultiply/100.0), active_extruder);
 #endif  // MESH_BED_LEVELING
   }
 #endif // !(DELTA || SCARA)
@@ -5844,17 +6005,19 @@ void kill()
   disable_e2();
   disable_e3();
 
-  #if HAS_POWER_SWITCH
-    pinMode(PS_ON_PIN, INPUT);
-  #endif
-
+#if defined(PS_ON_PIN) && PS_ON_PIN > -1
+  pinMode(PS_ON_PIN,INPUT);
+#endif
   SERIAL_ERROR_START;
   SERIAL_ERRORLNPGM(MSG_ERR_KILLED);
   LCD_ALERTMESSAGEPGM(MSG_KILLED);
   
   // FMC small patch to update the LCD before ending
   sei();   // enable interrupts
-  for (int i = 5; i--; lcd_update()) delay(200); // Wait a short time
+  for ( int i=5; i--; lcd_update())
+  {
+     delay(200);
+  }
   cli();   // disable interrupts
   suicide();
   while(1) { /* Intentionally left empty */ } // Wait for reset
diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h
index 1427da2127..10b1d30cce 100644
--- a/Marlin/SanityCheck.h
+++ b/Marlin/SanityCheck.h
@@ -116,7 +116,7 @@
         #error You must have at least 1 servo defined for NUM_SERVOS to use Z_PROBE_AND_ENDSTOP
       #endif
       #ifndef SERVO_ENDSTOPS
-        #error You must have SERVO_ENDSTOPS defined and have the Z index set to at least 1 to use Z_PROBE_AND_ENDSTOP
+        #error You must have SERVO_ENDSTOPS defined and have the Z index set to at least 0 or above to use Z_PROBE_AND_ENDSTOP
       #endif
       #ifndef SERVO_ENDSTOP_ANGLES
         #error You must have SERVO_ENDSTOP_ANGLES defined for Z Extend and Retract to use Z_PROBE_AND_ENSTOP
diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index 0dbc2a297b..6bd82cc68e 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -76,6 +76,7 @@ volatile long endstops_stepsTotal, endstops_stepsDone;
 static volatile bool endstop_x_hit = false;
 static volatile bool endstop_y_hit = false;
 static volatile bool endstop_z_hit = false;
+static volatile bool endstop_z_probe_hit = false;
 
 #ifdef ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED
   bool abort_on_endstop_hit = false;
@@ -258,11 +259,11 @@ volatile signed char count_direction[NUM_AXIS] = { 1, 1, 1, 1 };
 #define DISABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 &= ~BIT(OCIE1A)
 
 void endstops_hit_on_purpose() {
-  endstop_x_hit = endstop_y_hit = endstop_z_hit = false;
+  endstop_x_hit = endstop_y_hit = endstop_z_hit = endstop_z_probe_hit = false;
 }
 
 void checkHitEndstops() {
-  if (endstop_x_hit || endstop_y_hit || endstop_z_hit) {
+  if (endstop_x_hit || endstop_y_hit || endstop_z_hit || endstop_z_probe_hit) {
     SERIAL_ECHO_START;
     SERIAL_ECHOPGM(MSG_ENDSTOPS_HIT);
     if (endstop_x_hit) {
@@ -277,6 +278,10 @@ void checkHitEndstops() {
       SERIAL_ECHOPAIR(" Z:", (float)endstops_trigsteps[Z_AXIS] / axis_steps_per_unit[Z_AXIS]);
       LCD_MESSAGEPGM(MSG_ENDSTOPS_HIT "Z");
     }
+    if (endstop_z_probe_hit) {
+    	SERIAL_ECHOPAIR(" Z_PROBE:", (float)endstops_trigsteps[Z_AXIS] / axis_steps_per_unit[Z_AXIS]);
+    	LCD_MESSAGEPGM(MSG_ENDSTOPS_HIT "ZP");
+    }
     SERIAL_EOL;
 
     endstops_hit_on_purpose();
@@ -549,7 +554,7 @@ ISR(TIMER1_COMPA_vect) {
           if(z_probe_endstop && old_z_probe_endstop)
           {
         	  endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
-        	  endstop_z_hit=true;
+        	  endstop_z_probe_hit=true;
 
 //        	  if (z_probe_endstop && old_z_probe_endstop) SERIAL_ECHOLN("z_probe_endstop = true");
           }
@@ -596,7 +601,7 @@ ISR(TIMER1_COMPA_vect) {
           if(z_probe_endstop && old_z_probe_endstop)
           {
         	  endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
-        	  endstop_z_hit=true;
+        	  endstop_z_probe_hit=true;
 //        	  if (z_probe_endstop && old_z_probe_endstop) SERIAL_ECHOLN("z_probe_endstop = true");
           }
           old_z_probe_endstop = z_probe_endstop;

From a508d835dbee8a180d506c85076c7671a66791fa Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Wed, 1 Apr 2015 02:14:55 -0500
Subject: [PATCH 25/37] Changed Z_PROBE_AND_ENDSTOP to Z_PROBE_ENDSTOP. Updated
 documentation in Configuration.h. Cleaned up and commented some code relating
 to Z_PROBE_ENDSTOP. Separated Z_MIN_ENDSTOP and Z_PROBE_ENDSTOP completely.

---
 Marlin/Configuration.h | 22 ++++++++++++----------
 Marlin/Marlin_main.cpp | 18 +++++++++++-------
 Marlin/SanityCheck.h   | 31 ++++++++++++++++---------------
 Marlin/pins_RAMPS_13.h |  2 +-
 Marlin/stepper.cpp     | 16 +++++++++-------
 5 files changed, 49 insertions(+), 40 deletions(-)

diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index 2ca9fa231c..3d6bd50c26 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -335,7 +335,7 @@ const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic
 //#define DISABLE_MIN_ENDSTOPS
 // If you want to enable the Z Probe pin, but disable its use, uncomment the line below.
 // This only affects a Z Probe Endstop if you have separate Z min endstop as well and have
-// activated Z_PROBE_AND_ENDSTOP below. If you are using the Z Min endstop on your Z Probe,
+// activated Z_PROBE_ENDSTOP below. If you are using the Z Min endstop on your Z Probe,
 // this has no effect.
 //#define DISABLE_Z_PROBE_ENDSTOP
 
@@ -500,17 +500,19 @@ const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic
 
   #endif
 
-// Support for concurrent and seperate Z Probe and Z min endstop use.
-// Added by Chris Roadfeldt 3-28-2015
-// If you would like to use both a Z Probe and a Z min endstop at the same time, uncomment #define Z_PROBE_AND_ENDSTOP below
-// You will want to disable Z_SAFE_HOMING above as you will still use the Z min endstop for homing.
-// In order to use this, you must have a Z_PROBE_PIN defined in the pins.h file for your control board.
-// RAMPS 1.3/1.4 boards may be able to use the 5V, Ground and the D32 pin in the Aux 4 section of the RAMPS board for the signal.
+// Support for a dedicated Z PROBE endstop separate from the Z MIN endstop.
+// If you would like to use both a Z PROBE and a Z MIN endstop together or just a Z PROBE with a custom pin, uncomment #define Z_PROBE_ENDSTOP and read the instructions below.
+// If you want to still use the Z min endstop for homing, disable Z_SAFE_HOMING above. Eg; to park the head outside the bed area when homing with G28.
+// WARNING: The Z MIN endstop will need to set properly as it would without a Z PROBE to prevent head crashes and premature stopping during a print.
+// To use a separte Z PROBE endstop, you must have a Z_PROBE_PIN defined in the pins.h file for your control board.
+// If you are using a servo based Z PROBE, you will need to enable NUM_SERVOS, SERVO_ENDSTOPS and SERVO_ENDSTOPS_ANGLES in the R/C Servo below.
+// RAMPS 1.3/1.4 boards may be able to use the 5V, Ground and the D32 pin in the Aux 4 section of the RAMPS board. Use 5V for powered sensors, otherwise connect to ground and D32
+// for normally closed configuration and 5V and D32 for normally open configurations. Normally closed configuration is advised and assumed.
 // The D32 pin in Aux 4 on RAMPS maps to the Arduino D32 pin. Z_PROBE_PIN is setting the pin to use on the Arduino. Since the D32 pin on the RAMPS maps to D32 on Arduino, this works.
-// D32 is currently selected in the RAMPS 1.3/1.4 pin file. Update the pins.h file for your control board to make use of this. Not doing so nullifies Z_PROBE_AND_ENDSTOP
-// WARNING: Setting the wrong pin may have unexpected and disastrous outcomes. Use with caution and do your homework.
+// D32 is currently selected in the RAMPS 1.3/1.4 pin file. All other boards will need changes to the respective pins_XXXXX.h file.
+// WARNING: Setting the wrong pin may have unexpected and potentially disastrous outcomes. Use with caution and do your homework.
 
-//  #define Z_PROBE_AND_ENDSTOP
+//  #define Z_PROBE_ENDSTOP
 
 #endif // ENABLE_AUTO_BED_LEVELING
 
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index d95ad89094..98a4df1a3f 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -1240,7 +1240,7 @@ inline void sync_plan_position() {
 
       st_synchronize();
 
-    #if defined(Z_PROBE_AND_ENDSTOP)
+    #if defined(Z_PROBE_ENDSTOP)
       bool z_probe_endstop = (READ(Z_PROBE_PIN) != Z_PROBE_ENDSTOP_INVERTING);
       if (z_probe_endstop) {
     #else
@@ -1314,7 +1314,7 @@ inline void sync_plan_position() {
 
       st_synchronize();
 
-    #if defined(Z_PROBE_AND_ENDSTOP)
+    #if defined(Z_PROBE_ENDSTOP)
       bool z_probe_endstop = (READ(Z_PROBE_PIN) != Z_PROBE_ENDSTOP_INVERTING);
       if (z_probe_endstop) {
     #else
@@ -2805,13 +2805,17 @@ inline void gcode_M42() {
   } // code_seen('S')
 }
 
-// If Z_PROBE_AND_ENDSTOP is changed to completely break it's bonds from Z_MIN_ENDSTOP and become
-// it's own unique entity, then the following logic will need to be modified
-// so it only uses the Z_PROBE
 #if defined(ENABLE_AUTO_BED_LEVELING) && defined(Z_PROBE_REPEATABILITY_TEST)
 
-  #if (Z_MIN_PIN == -1) && (! defined (Z_PROBE_PIN) || Z_PROBE_PIN == -1)
-    #error "You must have a Z_MIN or Z_PROBE endstop in order to enable calculation of Z-Probe repeatability."
+  // This is redudant since the SanityCheck.h already checks for a valid Z_PROBE_PIN, but here for clarity.
+  #if defined (Z_PROBE_ENDSTOP)
+    #if (! defined (Z_PROBE_PIN) || Z_PROBE_PIN == -1)
+      #error "You must have a Z_PROBE_PIN defined in order to enable calculation of Z-Probe repeatability."
+    #endif
+  #else
+    #if (Z_MIN_PIN == -1) &&
+      #error "You must have a Z_MIN_PIN defined in order to enable calculation of Z-Probe repeatability."
+    #endif
   #endif
 
   /**
diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h
index 4d4153a97f..cac7c55e0c 100644
--- a/Marlin/SanityCheck.h
+++ b/Marlin/SanityCheck.h
@@ -103,26 +103,27 @@
     #endif
 
     /**
-     * Require a Z Probe Pin if Z_PROBE_AND_ENDSTOP is enabled.
+     * Require a Z Probe Pin if Z_PROBE_ENDSTOP is enabled.
      */
-    #if defined(Z_PROBE_AND_ENDSTOP)
+    #if defined(Z_PROBE_ENDSTOP)
       #ifndef Z_PROBE_PIN
-        #error You must have a Z_PROBE_PIN defined in your pins_XXXX.h file if you enable Z_PROBE_AND_ENDSTOP
+        #error You must have a Z_PROBE_PIN defined in your pins_XXXX.h file if you enable Z_PROBE_ENDSTOP
       #endif
       #if Z_PROBE_PIN == -1
-        #error You must set Z_PROBE_PIN to a valid pin if you enable Z_PROBE_AND_ENDSTOP
+        #error You must set Z_PROBE_PIN to a valid pin if you enable Z_PROBE_ENDSTOP
       #endif
-      #ifndef NUM_SERVOS
-        #error You must have NUM_SERVOS defined and there must be at least 1 configured to use Z_PROBE_AND_ENDSTOP
-      #endif
-      #if defined(NUM_SERVOS) && NUM_SERVOS < 1
-        #error You must have at least 1 servo defined for NUM_SERVOS to use Z_PROBE_AND_ENDSTOP
-      #endif
-      #ifndef SERVO_ENDSTOPS
-        #error You must have SERVO_ENDSTOPS defined and have the Z index set to at least 0 or above to use Z_PROBE_AND_ENDSTOP
-      #endif
-      #ifndef SERVO_ENDSTOP_ANGLES
-        #error You must have SERVO_ENDSTOP_ANGLES defined for Z Extend and Retract to use Z_PROBE_AND_ENSTOP
+// Forcing Servo definitions can break some hall effect sensor setups. Leaving these here for further comment.
+//      #ifndef NUM_SERVOS
+//        #error You must have NUM_SERVOS defined and there must be at least 1 configured to use Z_PROBE_ENDSTOP
+//      #endif
+//      #if defined(NUM_SERVOS) && NUM_SERVOS < 1
+//        #error You must have at least 1 servo defined for NUM_SERVOS to use Z_PROBE_ENDSTOP
+//      #endif
+//      #ifndef SERVO_ENDSTOPS
+//        #error You must have SERVO_ENDSTOPS defined and have the Z index set to at least 0 or above to use Z_PROBE_ENDSTOP
+//      #endif
+//      #ifndef SERVO_ENDSTOP_ANGLES
+//        #error You must have SERVO_ENDSTOP_ANGLES defined for Z Extend and Retract to use Z_PROBE_AND_ENSTOP
       #endif
     #endif
     /**
diff --git a/Marlin/pins_RAMPS_13.h b/Marlin/pins_RAMPS_13.h
index 3ca12dd156..11ecddeda2 100644
--- a/Marlin/pins_RAMPS_13.h
+++ b/Marlin/pins_RAMPS_13.h
@@ -62,7 +62,7 @@
   #define FILWIDTH_PIN        5
 #endif
 
-#if defined(Z_PROBE_AND_ENDSTOP)
+#if defined(Z_PROBE_ENDSTOP)
   // Define a pin to use as the signal pin on Arduino for the Z_PROBE endstop.
  #define Z_PROBE_PIN 32
 #endif
diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index 58029c8ccd..0fb4d8a672 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -76,7 +76,7 @@ volatile long endstops_stepsTotal, endstops_stepsDone;
 static volatile bool endstop_x_hit = false;
 static volatile bool endstop_y_hit = false;
 static volatile bool endstop_z_hit = false;
-static volatile bool endstop_z_probe_hit = false;
+static volatile bool endstop_z_probe_hit = false; // Leaving this in even if Z_PROBE_ENDSTOP isn't defined, keeps code below cleaner. #ifdef it and usage below to save space.
 
 #ifdef ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED
   bool abort_on_endstop_hit = false;
@@ -113,7 +113,7 @@ static volatile bool endstop_z_probe_hit = false;
   #endif
 #endif
 
-#ifdef Z_PROBE_AND_ENDSTOP
+#ifdef Z_PROBE_ENDSTOP // No need to check for valid pin, SanityCheck.h already does this.
 static bool old_z_probe_endstop = false;
 #endif
 
@@ -259,11 +259,11 @@ volatile signed char count_direction[NUM_AXIS] = { 1, 1, 1, 1 };
 #define DISABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 &= ~BIT(OCIE1A)
 
 void endstops_hit_on_purpose() {
-  endstop_x_hit = endstop_y_hit = endstop_z_hit = endstop_z_probe_hit = false;
+  endstop_x_hit = endstop_y_hit = endstop_z_hit = endstop_z_probe_hit = false; // #ifdef endstop_z_probe_hit = to save space if needed.
 }
 
 void checkHitEndstops() {
-  if (endstop_x_hit || endstop_y_hit || endstop_z_hit || endstop_z_probe_hit) {
+  if (endstop_x_hit || endstop_y_hit || endstop_z_hit || endstop_z_probe_hit) { // #ifdef || endstop_z_probe_hit to save space if needed.
     SERIAL_ECHO_START;
     SERIAL_ECHOPGM(MSG_ENDSTOPS_HIT);
     if (endstop_x_hit) {
@@ -278,10 +278,12 @@ void checkHitEndstops() {
       SERIAL_ECHOPAIR(" Z:", (float)endstops_trigsteps[Z_AXIS] / axis_steps_per_unit[Z_AXIS]);
       LCD_MESSAGEPGM(MSG_ENDSTOPS_HIT "Z");
     }
+    #ifdef Z_PROBE_ENDSTOP
     if (endstop_z_probe_hit) {
     	SERIAL_ECHOPAIR(" Z_PROBE:", (float)endstops_trigsteps[Z_AXIS] / axis_steps_per_unit[Z_AXIS]);
     	LCD_MESSAGEPGM(MSG_ENDSTOPS_HIT "ZP");
     }
+    #endif
     SERIAL_EOL;
 
     endstops_hit_on_purpose();
@@ -550,7 +552,7 @@ ISR(TIMER1_COMPA_vect) {
           #endif
         #endif
 
-        #if defined(Z_PROBE_PIN) && Z_PROBE_PIN > -1
+        #ifdef Z_PROBE_ENDSTOP
           UPDATE_ENDSTOP(z, Z, probe, PROBE);
           z_probe_endstop=(READ(Z_PROBE_PIN) != Z_PROBE_ENDSTOP_INVERTING);
           if(z_probe_endstop && old_z_probe_endstop)
@@ -597,7 +599,7 @@ ISR(TIMER1_COMPA_vect) {
           #endif
         #endif
 
-        #if defined(Z_PROBE_PIN) && Z_PROBE_PIN > -1
+        #ifdef Z_PROBE_ENDSTOP
           UPDATE_ENDSTOP(z, Z, probe, PROBE);
           z_probe_endstop=(READ(Z_PROBE_PIN) != Z_PROBE_ENDSTOP_INVERTING);
           if(z_probe_endstop && old_z_probe_endstop)
@@ -972,7 +974,7 @@ void st_init() {
     #endif
   #endif  
   
-#if defined(Z_PROBE_PIN) && Z_PROBE_PIN >= 0
+#if (defined(Z_PROBE_PIN) && Z_PROBE_PIN >= 0) && defined(Z_PROBE_ENDSTOP) // Check for Z_PROBE_ENDSTOP so we don't pull a pin high unless it's to be used.
   SET_INPUT(Z_PROBE_PIN);
   #ifdef ENDSTOPPULLUP_ZPROBE
     WRITE(Z_PROBE_PIN,HIGH);

From a57862e29f03e3a1f8dd2f8aa1ff588aa3bbf889 Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Wed, 1 Apr 2015 11:40:24 -0500
Subject: [PATCH 26/37] Cleaning up code in prep for merge with upstream.

---
 Marlin/Configuration.h                        |   1 -
 Marlin/Marlin.h                               |   1 -
 Marlin/Marlin_main.cpp                        |  83 +++++++------
 Marlin/dogm_lcd_implementation.h              |   2 +-
 Marlin/planner.cpp                            |   4 +-
 Marlin/stepper.cpp                            | 116 +++++++++++-------
 Marlin/ultralcd.cpp                           |   2 +-
 .../ultralcd_implementation_hitachi_HD44780.h |  16 +--
 8 files changed, 125 insertions(+), 100 deletions(-)

diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index 3d6bd50c26..f23b1e45b2 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -330,7 +330,6 @@ const bool X_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 const bool Y_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic of the endstop.
 const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic of the endstop.
 const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic of the endstop.
-
 //#define DISABLE_MAX_ENDSTOPS
 //#define DISABLE_MIN_ENDSTOPS
 // If you want to enable the Z Probe pin, but disable its use, uncomment the line below.
diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h
index 336e771fd9..e0441714b1 100644
--- a/Marlin/Marlin.h
+++ b/Marlin/Marlin.h
@@ -231,7 +231,6 @@ void refresh_cmd_timeout(void);
 extern float homing_feedrate[];
 extern bool axis_relative_modes[];
 extern int feedmultiply;
-extern int extrudemultiply; // Sets extrude multiply factor (in percent) for all extruders
 extern bool volumetric_enabled;
 extern int extruder_multiply[EXTRUDERS]; // sets extrude multiply factor (in percent) for each extruder individually
 extern float filament_size[EXTRUDERS]; // cross-sectional area of filament (in millimeters), typically around 1.75 or 2.85, 0 disables the volumetric calculations for the extruder.
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 8c463d91d2..5bc239d470 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -170,10 +170,10 @@
 // M404 - N<dia in mm> Enter the nominal filament width (3mm, 1.75mm ) or will display nominal filament width without parameters
 // M405 - Turn on Filament Sensor extrusion control.  Optional D<delay in cm> to set delay in centimeters between sensor and extruder
 // M406 - Turn off Filament Sensor extrusion control
-// M407 - Displays measured filament diameter
+// M407 - Display measured filament diameter
 // M500 - Store parameters in EEPROM
 // M501 - Read parameters from EEPROM (if you need reset them after you changed them temporarily).
-// M502 - Revert to the default "factory settings".  You still need to store them in EEPROM afterwards if you want to.
+// M502 - Revert to the default "factory settings". You still need to store them in EEPROM afterwards if you want to.
 // M503 - Print the current settings (from memory not from EEPROM). Use S0 to leave off headings.
 // M540 - Use S[0|1] to enable or disable the stop SD card print on endstop hit (requires ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
 // M600 - Pause for filament change X[pos] Y[pos] Z[relative lift] E[initial retract] L[later retract distance for removal]
@@ -272,7 +272,7 @@ int fanSpeed = 0;
 
 #endif // FWRETRACT
 
-#ifdef ULTIPANEL
+#if defined(ULTIPANEL) && HAS_POWER_SWITCH
   bool powersupply = 
     #ifdef PS_DEFAULT_OFF
       false
@@ -311,13 +311,13 @@ bool cancel_heatup = false;
 
 #ifdef FILAMENT_SENSOR
   //Variables for Filament Sensor input
-  float filament_width_nominal=DEFAULT_NOMINAL_FILAMENT_DIA;  //Set nominal filament width, can be changed with M404
-  bool filament_sensor=false;  //M405 turns on filament_sensor control, M406 turns it off
-  float filament_width_meas=DEFAULT_MEASURED_FILAMENT_DIA; //Stores the measured filament diameter
+  float filament_width_nominal = DEFAULT_NOMINAL_FILAMENT_DIA;  //Set nominal filament width, can be changed with M404
+  bool filament_sensor = false;  //M405 turns on filament_sensor control, M406 turns it off
+  float filament_width_meas = DEFAULT_MEASURED_FILAMENT_DIA; //Stores the measured filament diameter
   signed char measurement_delay[MAX_MEASUREMENT_DELAY+1];  //ring buffer to delay measurement  store extruder factor after subtracting 100
-  int delay_index1=0;  //index into ring buffer
-  int delay_index2=-1;  //index into ring buffer - set to -1 on startup to indicate ring buffer needs to be initialized
-  float delay_dist=0; //delay distance counter
+  int delay_index1 = 0;  //index into ring buffer
+  int delay_index2 = -1;  //index into ring buffer - set to -1 on startup to indicate ring buffer needs to be initialized
+  float delay_dist = 0; //delay distance counter
   int meas_delay_cm = MEASUREMENT_DELAY_CM;  //distance delay setting
 #endif
 
@@ -516,8 +516,8 @@ void setup_powerhold()
   #if defined(SUICIDE_PIN) && SUICIDE_PIN > -1
     OUT_WRITE(SUICIDE_PIN, HIGH);
   #endif
-  #if defined(PS_ON_PIN) && PS_ON_PIN > -1
-    #if defined(PS_DEFAULT_OFF)
+  #if HAS_POWER_SWITCH
+    #ifdef PS_DEFAULT_OFF
       OUT_WRITE(PS_ON_PIN, PS_ON_ASLEEP);
     #else
       OUT_WRITE(PS_ON_PIN, PS_ON_AWAKE);
@@ -1100,7 +1100,7 @@ inline void sync_plan_position() {
   static void run_z_probe() {
 
     #ifdef DELTA
-
+    
       float start_z = current_position[Z_AXIS];
       long start_steps = st_get_position(Z_AXIS);
     
@@ -1153,7 +1153,7 @@ inline void sync_plan_position() {
       current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
       // make sure the planner knows where we are as it may be a bit different than we last said to move to
       sync_plan_position();
-
+      
     #endif // !DELTA
   }
 
@@ -1163,7 +1163,7 @@ inline void sync_plan_position() {
     #ifdef DELTA
 
       feedrate = XY_TRAVEL_SPEED;
-
+      
       destination[X_AXIS] = x;
       destination[Y_AXIS] = y;
       destination[Z_AXIS] = z;
@@ -1237,12 +1237,12 @@ inline void sync_plan_position() {
       feedrate = homing_feedrate[X_AXIS]/10;
       destination[X_AXIS] = 0;
       prepare_move_raw();
-
+      
       // Home Y for safety
       feedrate = homing_feedrate[X_AXIS]/2;
       destination[Y_AXIS] = 0;
       prepare_move_raw();
-
+      
       st_synchronize();
 
     #if defined(Z_PROBE_ENDSTOP)
@@ -1250,7 +1250,7 @@ inline void sync_plan_position() {
       if (z_probe_endstop) {
     #else
       bool z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
-      if (!z_min_endstop) {
+      if (z_min_endstop) {
     #endif
         if (!Stopped) {
           SERIAL_ERROR_START;
@@ -1261,7 +1261,7 @@ inline void sync_plan_position() {
       }
 
     #endif // Z_PROBE_ALLEN_KEY
-      
+
   }
 
   static void retract_z_probe() {
@@ -1279,9 +1279,9 @@ inline void sync_plan_position() {
         #if SERVO_LEVELING
           servos[servo_endstops[Z_AXIS]].attach(0);
         #endif
-          
-          servos[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2 + 1]);
-          
+
+        servos[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2 + 1]);
+
         #if SERVO_LEVELING
           delay(PROBE_SERVO_DEACTIVATION_DELAY);
           servos[servo_endstops[Z_AXIS]].detach();
@@ -1305,23 +1305,23 @@ inline void sync_plan_position() {
       feedrate = homing_feedrate[Z_AXIS]/10;
       destination[Z_AXIS] = current_position[Z_AXIS] - Z_PROBE_ALLEN_KEY_RETRACT_DEPTH;
       prepare_move_raw();
-
+      
       // Move up for safety
       feedrate = homing_feedrate[Z_AXIS]/2;
       destination[Z_AXIS] = current_position[Z_AXIS] + Z_PROBE_ALLEN_KEY_RETRACT_DEPTH * 2;
       prepare_move_raw();
-
+      
       // Home XY for safety
       feedrate = homing_feedrate[X_AXIS]/2;
       destination[X_AXIS] = 0;
       destination[Y_AXIS] = 0;
       prepare_move_raw();
-
+      
       st_synchronize();
 
     #if defined(Z_PROBE_ENDSTOP)
       bool z_probe_endstop = (READ(Z_PROBE_PIN) != Z_PROBE_ENDSTOP_INVERTING);
-      if (z_probe_endstop) {
+      if (!z_probe_endstop) {
     #else
       bool z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
       if (!z_min_endstop) {
@@ -3319,7 +3319,7 @@ inline void gcode_M140() {
   if (code_seen('S')) setTargetBed(code_value());
 }
 
-#if defined(PS_ON_PIN) && PS_ON_PIN > -1
+#if HAS_POWER_SWITCH
 
   /**
    * M80: Turn on Power Supply
@@ -3341,10 +3341,12 @@ inline void gcode_M140() {
     #endif
   }
 
-#endif // PS_ON_PIN
+#endif // HAS_POWER_SWITCH
 
 /**
- * M81: Turn off Power Supply
+ * M81: Turn off Power, including Power Supply, if there is one.
+ *
+ *      This code should ALWAYS be available for EMERGENCY SHUTDOWN!
  */
 inline void gcode_M81() {
   disable_heater();
@@ -3359,16 +3361,19 @@ inline void gcode_M81() {
   #if defined(SUICIDE_PIN) && SUICIDE_PIN > -1
     st_synchronize();
     suicide();
-  #elif defined(PS_ON_PIN) && PS_ON_PIN > -1
+  #elif HAS_POWER_SWITCH
     OUT_WRITE(PS_ON_PIN, PS_ON_ASLEEP);
   #endif
   #ifdef ULTIPANEL
-    powersupply = false;
+    #if HAS_POWER_SWITCH
+      powersupply = false;
+    #endif
     LCD_MESSAGEPGM(MACHINE_NAME " " MSG_OFF ".");
     lcd_update();
   #endif
 }
 
+
 /**
  * M82: Set E codes absolute (default)
  */
@@ -4903,15 +4908,15 @@ void process_commands() {
         #endif //HEATER_2_PIN
       #endif //BARICUDA
 
-      #if defined(PS_ON_PIN) && PS_ON_PIN > -1
+      #if HAS_POWER_SWITCH
 
         case 80: // M80 - Turn on Power Supply
           gcode_M80();
           break;
 
-      #endif // PS_ON_PIN
+      #endif // HAS_POWER_SWITCH
 
-      case 81: // M81 - Turn off Power Supply
+      case 81: // M81 - Turn off Power, including Power Supply, if possible
         gcode_M81();
         break;
 
@@ -5882,19 +5887,17 @@ void kill()
   disable_e2();
   disable_e3();
 
-#if defined(PS_ON_PIN) && PS_ON_PIN > -1
-  pinMode(PS_ON_PIN,INPUT);
-#endif
+  #if HAS_POWER_SWITCH
+    pinMode(PS_ON_PIN, INPUT);
+  #endif
+
   SERIAL_ERROR_START;
   SERIAL_ERRORLNPGM(MSG_ERR_KILLED);
   LCD_ALERTMESSAGEPGM(MSG_KILLED);
   
   // FMC small patch to update the LCD before ending
   sei();   // enable interrupts
-  for ( int i=5; i--; lcd_update())
-  {
-     delay(200);
-  }
+  for (int i = 5; i--; lcd_update()) delay(200); // Wait a short time
   cli();   // disable interrupts
   suicide();
   while(1) { /* Intentionally left empty */ } // Wait for reset
diff --git a/Marlin/dogm_lcd_implementation.h b/Marlin/dogm_lcd_implementation.h
index 89cd5e835c..63e99bd3aa 100644
--- a/Marlin/dogm_lcd_implementation.h
+++ b/Marlin/dogm_lcd_implementation.h
@@ -369,7 +369,7 @@ static void lcd_implementation_status_screen() {
       lcd_printPGM(PSTR("dia:"));
       lcd_print(ftostr12ns(filament_width_meas));
       lcd_printPGM(PSTR(" factor:"));
-      lcd_print(itostr3(extrudemultiply));
+      lcd_print(itostr3(volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM]));
       lcd_print('%');
     }
   #endif
diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index 786527d0d7..d98ef63d4d 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -545,7 +545,7 @@ float junction_deviation = 0.1;
   block->steps[Z_AXIS] = labs(dz);
   block->steps[E_AXIS] = labs(de);
   block->steps[E_AXIS] *= volumetric_multiplier[active_extruder];
-  block->steps[E_AXIS] *= extrudemultiply;
+  block->steps[E_AXIS] *= extruder_multiply[active_extruder];
   block->steps[E_AXIS] /= 100;
   block->step_event_count = max(block->steps[X_AXIS], max(block->steps[Y_AXIS], max(block->steps[Z_AXIS], block->steps[E_AXIS])));
 
@@ -679,7 +679,7 @@ float junction_deviation = 0.1;
     delta_mm[Y_AXIS] = dy / axis_steps_per_unit[Y_AXIS];
   #endif
   delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS];
-  delta_mm[E_AXIS] = (de / axis_steps_per_unit[E_AXIS]) * volumetric_multiplier[active_extruder] * extrudemultiply / 100.0;
+  delta_mm[E_AXIS] = (de / axis_steps_per_unit[E_AXIS]) * volumetric_multiplier[active_extruder] * extruder_multiply[active_extruder] / 100.0;
 
   if (block->steps[X_AXIS] <= dropsegments && block->steps[Y_AXIS] <= dropsegments && block->steps[Z_AXIS] <= dropsegments) {
     block->millimeters = fabs(delta_mm[E_AXIS]);
diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index 0fb4d8a672..5c01e2f154 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -524,33 +524,43 @@ ISR(TIMER1_COMPA_vect) {
     }
 
     if (TEST(out_bits, Z_AXIS)) {   // -direction
+
       Z_APPLY_DIR(INVERT_Z_DIR,0);
       count_direction[Z_AXIS] = -1;
-      if (check_endstops) 
-      {
-        #if defined(Z_MIN_PIN) && Z_MIN_PIN > -1
-          #ifndef Z_DUAL_ENDSTOPS
-            UPDATE_ENDSTOP(z, Z, min, MIN);
-          #else
-            bool z_min_endstop=(READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
-            #if defined(Z2_MIN_PIN) && Z2_MIN_PIN > -1
-              bool z2_min_endstop=(READ(Z2_MIN_PIN) != Z2_MIN_ENDSTOP_INVERTING);
-            #else
-              bool z2_min_endstop=z_min_endstop;
-            #endif
-            if(((z_min_endstop && old_z_min_endstop) || (z2_min_endstop && old_z2_min_endstop)) && (current_block->steps[Z_AXIS] > 0))
-            {
+
+      if (check_endstops) {
+
+        #if defined(Z_MIN_PIN) && Z_MIN_PIN >= 0
+
+          #ifdef Z_DUAL_ENDSTOPS
+
+            bool z_min_endstop = READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING,
+                z2_min_endstop =
+                  #if defined(Z2_MIN_PIN) && Z2_MIN_PIN >= 0
+                    READ(Z2_MIN_PIN) != Z2_MIN_ENDSTOP_INVERTING
+                  #else
+                    z_min_endstop
+                  #endif
+                ;
+
+            bool z_min_both = z_min_endstop && old_z_min_endstop,
+                z2_min_both = z2_min_endstop && old_z2_min_endstop;
+            if ((z_min_both || z2_min_both) && current_block->steps[Z_AXIS] > 0) {
               endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
-              endstop_z_hit=true;
-              if (!(performing_homing) || ((performing_homing)&&(z_min_endstop && old_z_min_endstop)&&(z2_min_endstop && old_z2_min_endstop))) //if not performing home or if both endstops were trigged during homing...
-              {
+              endstop_z_hit = true;
+              if (!performing_homing || (performing_homing && z_min_both && z2_min_both)) //if not performing home or if both endstops were trigged during homing...
                 step_events_completed = current_block->step_event_count;
-              } 
             }
             old_z_min_endstop = z_min_endstop;
             old_z2_min_endstop = z2_min_endstop;
-          #endif
-        #endif
+
+          #else // !Z_DUAL_ENDSTOPS
+
+            UPDATE_ENDSTOP(z, Z, min, MIN);
+
+          #endif // !Z_DUAL_ENDSTOPS
+
+        #endif // Z_MIN_PIN
 
         #ifdef Z_PROBE_ENDSTOP
           UPDATE_ENDSTOP(z, Z, probe, PROBE);
@@ -564,41 +574,53 @@ ISR(TIMER1_COMPA_vect) {
           }
           old_z_probe_endstop = z_probe_endstop;
         #endif
-      }
+        
+      } // check_endstops
+
     }
     else { // +direction
+
       Z_APPLY_DIR(!INVERT_Z_DIR,0);
       count_direction[Z_AXIS] = 1;
+
       if (check_endstops) {
+
         #if defined(Z_MAX_PIN) && Z_MAX_PIN >= 0
-          #ifndef Z_DUAL_ENDSTOPS
-            UPDATE_ENDSTOP(z, Z, max, MAX);
-          #else
-            bool z_max_endstop=(READ(Z_MAX_PIN) != Z_MAX_ENDSTOP_INVERTING);
-            #if defined(Z2_MAX_PIN) && Z2_MAX_PIN > -1
-              bool z2_max_endstop=(READ(Z2_MAX_PIN) != Z2_MAX_ENDSTOP_INVERTING);
-            #else
-              bool z2_max_endstop=z_max_endstop;
-            #endif
-            if(((z_max_endstop && old_z_max_endstop) || (z2_max_endstop && old_z2_max_endstop)) && (current_block->steps[Z_AXIS] > 0))
-            {
+
+          #ifdef Z_DUAL_ENDSTOPS
+
+            bool z_max_endstop = READ(Z_MAX_PIN) != Z_MAX_ENDSTOP_INVERTING,
+                z2_max_endstop =
+                  #if defined(Z2_MAX_PIN) && Z2_MAX_PIN >= 0
+                    READ(Z2_MAX_PIN) != Z2_MAX_ENDSTOP_INVERTING
+                  #else
+                    z_max_endstop
+                  #endif
+                ;
+
+            bool z_max_both = z_max_endstop && old_z_max_endstop,
+                z2_max_both = z2_max_endstop && old_z2_max_endstop;
+            if ((z_max_both || z2_max_both) && current_block->steps[Z_AXIS] > 0) {
               endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
-              endstop_z_hit=true;
+              endstop_z_hit = true;
 
-//              if (z_max_endstop && old_z_max_endstop) SERIAL_ECHOLN("z_max_endstop = true");
-//              if (z2_max_endstop && old_z2_max_endstop) SERIAL_ECHOLN("z2_max_endstop = true");
+             // if (z_max_both) SERIAL_ECHOLN("z_max_endstop = true");
+             // if (z2_max_both) SERIAL_ECHOLN("z2_max_endstop = true");
 
-            
-              if (!(performing_homing) || ((performing_homing)&&(z_max_endstop && old_z_max_endstop)&&(z2_max_endstop && old_z2_max_endstop))) //if not performing home or if both endstops were trigged during homing...
-              {
+              if (!performing_homing || (performing_homing && z_max_both && z2_max_both)) //if not performing home or if both endstops were trigged during homing...
                 step_events_completed = current_block->step_event_count;
-              } 
             }
             old_z_max_endstop = z_max_endstop;
             old_z2_max_endstop = z2_max_endstop;
-          #endif
-        #endif
 
+          #else // !Z_DUAL_ENDSTOPS
+
+            UPDATE_ENDSTOP(z, Z, max, MAX);
+
+          #endif // !Z_DUAL_ENDSTOPS
+
+        #endif // Z_MAX_PIN
+        
         #ifdef Z_PROBE_ENDSTOP
           UPDATE_ENDSTOP(z, Z, probe, PROBE);
           z_probe_endstop=(READ(Z_PROBE_PIN) != Z_PROBE_ENDSTOP_INVERTING);
@@ -610,22 +632,24 @@ ISR(TIMER1_COMPA_vect) {
           }
           old_z_probe_endstop = z_probe_endstop;
         #endif
-      }
-    }
+
+      } // check_endstops
+
+    } // +direction
 
     #ifndef ADVANCE
       if (TEST(out_bits, E_AXIS)) {  // -direction
         REV_E_DIR();
-        count_direction[E_AXIS]=-1;
+        count_direction[E_AXIS] = -1;
       }
       else { // +direction
         NORM_E_DIR();
-        count_direction[E_AXIS]=1;
+        count_direction[E_AXIS] = 1;
       }
     #endif //!ADVANCE
 
     // Take multiple steps per interrupt (For high speed moves)
-    for (int8_t i=0; i < step_loops; i++) {
+    for (int8_t i = 0; i < step_loops; i++) {
       #ifndef AT90USB
         MSerial.checkRx(); // Check for serial chars.
       #endif
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index a9930fc143..d2a2e6faaf 100644
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -491,7 +491,7 @@ static void lcd_tune_menu() {
     MENU_MULTIPLIER_ITEM_EDIT(int3, MSG_BED, &target_temperature_bed, 0, BED_MAXTEMP - 15);
   #endif
   MENU_MULTIPLIER_ITEM_EDIT(int3, MSG_FAN_SPEED, &fanSpeed, 0, 255);
-  MENU_ITEM_EDIT(int3, MSG_FLOW, &extrudemultiply, 10, 999);
+  MENU_ITEM_EDIT(int3, MSG_FLOW, &extruder_multiply[active_extruder], 10, 999);
   MENU_ITEM_EDIT(int3, MSG_FLOW MSG_F0, &extruder_multiply[0], 10, 999);
   #if TEMP_SENSOR_1 != 0
     MENU_ITEM_EDIT(int3, MSG_FLOW MSG_F1, &extruder_multiply[1], 10, 999);
diff --git a/Marlin/ultralcd_implementation_hitachi_HD44780.h b/Marlin/ultralcd_implementation_hitachi_HD44780.h
index 583cde662f..4819e3e00b 100644
--- a/Marlin/ultralcd_implementation_hitachi_HD44780.h
+++ b/Marlin/ultralcd_implementation_hitachi_HD44780.h
@@ -624,7 +624,7 @@ static void lcd_implementation_status_screen()
 
 static void lcd_implementation_drawmenu_generic(bool sel, uint8_t row, const char* pstr, char pre_char, char post_char) {
   char c;
-  uint8_t n = LCD_WIDTH - 1 - (LCD_WIDTH < 20 ? 1 : 2);
+  uint8_t n = LCD_WIDTH - 2;
   lcd.setCursor(0, row);
   lcd.print(sel ? pre_char : ' ');
   while ((c = pgm_read_byte(pstr)) && n > 0) {
@@ -633,12 +633,11 @@ static void lcd_implementation_drawmenu_generic(bool sel, uint8_t row, const cha
   }
   while(n--) lcd.print(' ');
   lcd.print(post_char);
-  lcd.print(' ');
 }
 
 static void lcd_implementation_drawmenu_setting_edit_generic(bool sel, uint8_t row, const char* pstr, char pre_char, char* data) {
   char c;
-  uint8_t n = LCD_WIDTH - 1 - (LCD_WIDTH < 20 ? 1 : 2) - lcd_strlen(data);
+  uint8_t n = LCD_WIDTH - 2 - lcd_strlen(data);
   lcd.setCursor(0, row);
   lcd.print(sel ? pre_char : ' ');
   while ((c = pgm_read_byte(pstr)) && n > 0) {
@@ -651,7 +650,7 @@ static void lcd_implementation_drawmenu_setting_edit_generic(bool sel, uint8_t r
 }
 static void lcd_implementation_drawmenu_setting_edit_generic_P(bool sel, uint8_t row, const char* pstr, char pre_char, const char* data) {
   char c;
-  uint8_t n = LCD_WIDTH - 1 - (LCD_WIDTH < 20 ? 1 : 2) - lcd_strlen_P(data);
+  uint8_t n = LCD_WIDTH - 2 - lcd_strlen_P(data);
   lcd.setCursor(0, row);
   lcd.print(sel ? pre_char : ' ');
   while ((c = pgm_read_byte(pstr)) && n > 0) {
@@ -688,11 +687,11 @@ void lcd_implementation_drawedit(const char* pstr, char* value) {
   lcd.setCursor(1, 1);
   lcd_printPGM(pstr);
   lcd.print(':');
-  lcd.setCursor(LCD_WIDTH - (LCD_WIDTH < 20 ? 0 : 1) - lcd_strlen(value), 1);
+  lcd.setCursor(LCD_WIDTH - lcd_strlen(value), 1);
   lcd_print(value);
 }
 
-static void lcd_implementation_drawmenu_sd(bool sel, uint8_t row, const char* pstr, const char* filename, char* longFilename, uint8_t concat) {
+static void lcd_implementation_drawmenu_sd(bool sel, uint8_t row, const char* pstr, const char* filename, char* longFilename, uint8_t concat, char post_char) {
   char c;
   uint8_t n = LCD_WIDTH - concat;
   lcd.setCursor(0, row);
@@ -706,14 +705,15 @@ static void lcd_implementation_drawmenu_sd(bool sel, uint8_t row, const char* ps
     filename++;
   }
   while (n--) lcd.print(' ');
+  lcd.print(post_char);
 }
 
 static void lcd_implementation_drawmenu_sdfile(bool sel, uint8_t row, const char* pstr, const char* filename, char* longFilename) {
-  lcd_implementation_drawmenu_sd(sel, row, pstr, filename, longFilename, 1);
+  lcd_implementation_drawmenu_sd(sel, row, pstr, filename, longFilename, 2, ' ');
 }
 
 static void lcd_implementation_drawmenu_sddirectory(bool sel, uint8_t row, const char* pstr, const char* filename, char* longFilename) {
-  lcd_implementation_drawmenu_sd(sel, row, pstr, filename, longFilename, 2);
+  lcd_implementation_drawmenu_sd(sel, row, pstr, filename, longFilename, 2, LCD_STR_FOLDER[0]);
 }
 
 #define lcd_implementation_drawmenu_back(sel, row, pstr, data) lcd_implementation_drawmenu_generic(sel, row, pstr, LCD_STR_UPLEVEL[0], LCD_STR_UPLEVEL[0])

From bb41edc2efa05160bf7e0d972b84ad7eb3de1617 Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Wed, 1 Apr 2015 12:13:25 -0500
Subject: [PATCH 27/37] Fixed extra #endif

---
 Marlin/SanityCheck.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h
index cac7c55e0c..6c0c9fd09e 100644
--- a/Marlin/SanityCheck.h
+++ b/Marlin/SanityCheck.h
@@ -124,7 +124,7 @@
 //      #endif
 //      #ifndef SERVO_ENDSTOP_ANGLES
 //        #error You must have SERVO_ENDSTOP_ANGLES defined for Z Extend and Retract to use Z_PROBE_AND_ENSTOP
-      #endif
+//      #endif
     #endif
     /**
      * Check if Probe_Offset * Grid Points is greater than Probing Range

From 2966ae2022c51449c02fe76eaf2a470c003d26af Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Wed, 1 Apr 2015 13:32:25 -0500
Subject: [PATCH 28/37] Fix Z_PROBE_PING not declared, allows code to compile
 if Z_PROBE_ENDSTOP is not used.

---
 Marlin/pins.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Marlin/pins.h b/Marlin/pins.h
index 6db56e9f42..0d63298869 100644
--- a/Marlin/pins.h
+++ b/Marlin/pins.h
@@ -187,7 +187,7 @@
   #define Z_MIN_PIN          -1
 #endif
 
-#ifdef DISABLE_Z_PROBE_ENDSTOP
+#if defined (DISABLE_Z_PROBE_ENDSTOP) || ! defined (Z_PROBE_ENDSTOP) // Allow code to compile regardless of Z_PROBE_ENDSTOP setting.
   #define Z_PROBE_PIN        -1
 #endif
 

From 26dc80bf2d53409387b5915580c9f5534ed8b4c6 Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Wed, 1 Apr 2015 13:47:17 -0500
Subject: [PATCH 29/37] Typo fixed...

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

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 5bc239d470..456cfb0a4e 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -2811,7 +2811,7 @@ inline void gcode_M42() {
       #error "You must have a Z_PROBE_PIN defined in order to enable calculation of Z-Probe repeatability."
     #endif
   #else
-    #if (Z_MIN_PIN == -1) &&
+    #if (Z_MIN_PIN == -1)
       #error "You must have a Z_MIN_PIN defined in order to enable calculation of Z-Probe repeatability."
     #endif
   #endif

From 916f59e35f1408a65b68267d0b2ec9d62ccb31b4 Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Wed, 1 Apr 2015 19:22:05 -0500
Subject: [PATCH 30/37] Spaces not tabs in language.h. Catch unlikely but
 possible error and head crash when using Z_PROBE_REPEATABILITY_TEST

---
 Marlin/SanityCheck.h | 2 +-
 Marlin/language.h    | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h
index 6c0c9fd09e..09d1ca3b65 100644
--- a/Marlin/SanityCheck.h
+++ b/Marlin/SanityCheck.h
@@ -93,7 +93,7 @@
      * Require a Z Min pin
      */
     #if Z_MIN_PIN == -1
-      #if Z_PROBE_PIN == -1
+      #if Z_PROBE_PIN == -1 || (! defined (Z_PROBE_ENDSTOP) || defined (DISABLE_Z_PROBE_ENDSTOP)) // It's possible for someone to set a ping for the Z Probe, but not enable it.
         #ifdef Z_PROBE_REPEATABILITY_TEST
           #error You must have a Z_MIN or Z_PROBE endstop to enable Z_PROBE_REPEATABILITY_TEST.
         #else
diff --git a/Marlin/language.h b/Marlin/language.h
index f4a2d2610a..4a4698c90f 100644
--- a/Marlin/language.h
+++ b/Marlin/language.h
@@ -138,7 +138,7 @@
 #define MSG_Z_MIN                           "z_min: "
 #define MSG_Z_MAX                           "z_max: "
 #define MSG_Z2_MAX                          "z2_max: "
-#define MSG_Z_PROBE							"z_probe: "
+#define MSG_Z_PROBE                         "z_probe: "
 #define MSG_M119_REPORT                     "Reporting endstop status"
 #define MSG_ENDSTOP_HIT                     "TRIGGERED"
 #define MSG_ENDSTOP_OPEN                    "open"

From 59994bd519303ad6f205229ba7d926a57833e4d2 Mon Sep 17 00:00:00 2001
From: Chris Roadfeldt <chris@roadfeldt.com>
Date: Wed, 1 Apr 2015 19:24:42 -0500
Subject: [PATCH 31/37] Not doing network admin work, pin not ping... :)

---
 Marlin/SanityCheck.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h
index 09d1ca3b65..8ea9b2e3d0 100644
--- a/Marlin/SanityCheck.h
+++ b/Marlin/SanityCheck.h
@@ -93,7 +93,7 @@
      * Require a Z Min pin
      */
     #if Z_MIN_PIN == -1
-      #if Z_PROBE_PIN == -1 || (! defined (Z_PROBE_ENDSTOP) || defined (DISABLE_Z_PROBE_ENDSTOP)) // It's possible for someone to set a ping for the Z Probe, but not enable it.
+      #if Z_PROBE_PIN == -1 || (! defined (Z_PROBE_ENDSTOP) || defined (DISABLE_Z_PROBE_ENDSTOP)) // It's possible for someone to set a pin for the Z Probe, but not enable it.
         #ifdef Z_PROBE_REPEATABILITY_TEST
           #error You must have a Z_MIN or Z_PROBE endstop to enable Z_PROBE_REPEATABILITY_TEST.
         #else

From 23cd54755f1e4ec63d3507a990e3f37d59216729 Mon Sep 17 00:00:00 2001
From: AnHardt <github@kitelab.de>
Date: Thu, 2 Apr 2015 14:50:31 +0200
Subject: [PATCH 32/37] Fix typo in Marlin_main.cpp

related to current_position[Z_AXIS] and retract_zlift.

'+ =' -> '+='

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

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 6b41be6179..3eda508593 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -1592,7 +1592,7 @@ void refresh_cmd_timeout(void) { previous_millis_cmd = millis(); }
     else {
 
       if (retract_zlift > 0.01) {
-        current_position[Z_AXIS] + =retract_zlift;
+        current_position[Z_AXIS] += retract_zlift;
         #ifdef DELTA
           sync_plan_position_delta();
         #else

From 34c7d45879e20a0fd5d48c53ddf98f6ed342c1c5 Mon Sep 17 00:00:00 2001
From: Richard Wackerbarth <rkw@dataplex.net>
Date: Fri, 6 Feb 2015 10:30:43 -0600
Subject: [PATCH 33/37] Hook for Auto-generated Build Version

The automatic versioning system extracts a
build version number from the SCM system

That versioning information is written to the file
_Version.h, a file that is NOT preserved in the SCM.

If such a file will be present,
we include it here to utilize the parameters
that are defined therein.
---
 Marlin/language.h | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/Marlin/language.h b/Marlin/language.h
index 10ef445d83..690b0b11d0 100644
--- a/Marlin/language.h
+++ b/Marlin/language.h
@@ -36,6 +36,10 @@
   #define LANGUAGE_INCLUDE GENERATE_LANGUAGE_INCLUDE(en)
 #endif
 
+#ifdef HAS_AUTOMATIC_VERSIONING
+  #include "_Version.h"
+#endif
+
 #define PROTOCOL_VERSION "1.0"
 #define FIRMWARE_URL "https://github.com/MarlinFirmware/Marlin"
 

From a33e20b27d32b816e5fbad9877e6879bfa185cb2 Mon Sep 17 00:00:00 2001
From: Richard Wackerbarth <rkw@dataplex.net>
Date: Fri, 6 Feb 2015 09:58:04 -0600
Subject: [PATCH 34/37] Adjust per-project .gitignore

Refer to http://git-scm.com/docs/gitignore
to see why this is appropriate
---
 .gitignore | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/.gitignore b/.gitignore
index cd72efb10d..380a028506 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,3 +1,12 @@
+// Our automatic versioning scheme generates the following file
+// NEVER put it in the repository
+_Version.h
+
+// All of the following OS, IDE and compiler generated file
+// references should be moved from this file
+// They are needed, but they belong in your global .gitignore
+// rather than in a per-project file such as this
+
 *.o
 applet/
 *~

From f4599143ebdaf05d12528e1fedaf3e51f413c274 Mon Sep 17 00:00:00 2001
From: Richard Wackerbarth <rkw@dataplex.net>
Date: Fri, 6 Feb 2015 10:17:17 -0600
Subject: [PATCH 35/37] Allow M115 to better reflect the build

---
 Marlin/language.h | 24 +++++++++++++++++++-----
 1 file changed, 19 insertions(+), 5 deletions(-)

diff --git a/Marlin/language.h b/Marlin/language.h
index 690b0b11d0..77c4f36d41 100644
--- a/Marlin/language.h
+++ b/Marlin/language.h
@@ -41,7 +41,6 @@
 #endif
 
 #define PROTOCOL_VERSION "1.0"
-#define FIRMWARE_URL "https://github.com/MarlinFirmware/Marlin"
 
 #if MB(ULTIMAKER)|| MB(ULTIMAKER_OLD)|| MB(ULTIMAIN_2)
   #undef FIRMWARE_URL
@@ -69,13 +68,28 @@
   #define MACHINE_NAME "HEPHESTOS"
   #undef FIRMWARE_URL
   #define FIRMWARE_URL "http://www.bq.com/gb/downloads-prusa-i3-hephestos.html"
-#else // Default firmware set to Mendel
-  #define MACHINE_NAME "Mendel"
+#else
+  #ifndef MACHINE_NAME
+    #define MACHINE_NAME "Mendel"
+  #endif
 #endif
 
 #ifdef CUSTOM_MENDEL_NAME
+  #warning CUSTOM_MENDEL_NAME deprecated - use CUSTOM_MACHINE_NAME
+  #define CUSTOM_MACHINE_NAME CUSTOM_MENDEL_NAME
+#endif
+
+#ifdef CUSTOM_MACHINE_NAME
   #undef MACHINE_NAME
-  #define MACHINE_NAME CUSTOM_MENDEL_NAME
+  #define MACHINE_NAME CUSTOM_MACHINE_NAME
+#endif
+
+#ifndef FIRMWARE_URL
+  #define FIRMWARE_URL "https://github.com/MarlinFirmware/Marlin"
+#endif
+
+#ifndef BUILD_VERSION
+  #define BUILD_VERSION "V1; Sprinter/grbl mashup for gen6"
 #endif
 
 #ifndef MACHINE_UUID
@@ -126,7 +140,7 @@
 #define MSG_HEATING_COMPLETE                "Heating done."
 #define MSG_BED_HEATING                     "Bed Heating."
 #define MSG_BED_DONE                        "Bed done."
-#define MSG_M115_REPORT                     "FIRMWARE_NAME:Marlin V1; Sprinter/grbl mashup for gen6 FIRMWARE_URL:" FIRMWARE_URL " PROTOCOL_VERSION:" PROTOCOL_VERSION " MACHINE_TYPE:" MACHINE_NAME " EXTRUDER_COUNT:" STRINGIFY(EXTRUDERS) " UUID:" MACHINE_UUID "\n"
+#define MSG_M115_REPORT                     "FIRMWARE_NAME:Marlin " BUILD_VERSION " FIRMWARE_URL:" FIRMWARE_URL " PROTOCOL_VERSION:" PROTOCOL_VERSION " MACHINE_TYPE:" MACHINE_NAME " EXTRUDER_COUNT:" STRINGIFY(EXTRUDERS) " UUID:" MACHINE_UUID "\n"
 #define MSG_COUNT_X                         " Count X: "
 #define MSG_ERR_KILLED                      "Printer halted. kill() called!"
 #define MSG_ERR_STOPPED                     "Printer stopped due to errors. Fix the error and use M999 to restart. (Temperature is reset. Set it after restarting)"

From ccdaea51babd46fca1351affdaa8168438731fc7 Mon Sep 17 00:00:00 2001
From: Richard Wackerbarth <rkw@dataplex.net>
Date: Thu, 2 Apr 2015 08:15:55 -0500
Subject: [PATCH 36/37] Add KosselPro

---
 Marlin/boards.h   | 1 +
 Marlin/language.h | 5 +++++
 2 files changed, 6 insertions(+)

diff --git a/Marlin/boards.h b/Marlin/boards.h
index a8c980097d..8a60f011d5 100644
--- a/Marlin/boards.h
+++ b/Marlin/boards.h
@@ -37,6 +37,7 @@
 #define BOARD_BRAINWAVE         82   // Brainwave (AT90USB646)
 #define BOARD_SAV_MKI           83   // SAV Mk-I (AT90USB1286)
 #define BOARD_TEENSY2           84   // Teensy++2.0 (AT90USB1286) - CLI compile: DEFINES=AT90USBxx_TEENSYPP_ASSIGNMENTS HARDWARE_MOTHERBOARD=84  make
+#define BOARD_BRAINWAVE_PRO     85   // Brainwave Pro (AT90USB1286)
 #define BOARD_GEN3_PLUS         9    // Gen3+
 #define BOARD_GEN3_MONOLITHIC   22   // Gen3 Monolithic Electronics
 #define BOARD_MEGATRONICS       70   // Megatronics
diff --git a/Marlin/language.h b/Marlin/language.h
index 77c4f36d41..5b7c22f45b 100644
--- a/Marlin/language.h
+++ b/Marlin/language.h
@@ -68,6 +68,11 @@
   #define MACHINE_NAME "HEPHESTOS"
   #undef FIRMWARE_URL
   #define FIRMWARE_URL "http://www.bq.com/gb/downloads-prusa-i3-hephestos.html"
+#elif MB(BRAINWAVE_PRO)
+  #define MACHINE_NAME "Kossel Pro"
+  #ifndef FIRMWARE_URL
+    #define FIRMWARE_URL "https://github.com/OpenBeamUSA/Marlin/"
+  #endif
 #else
   #ifndef MACHINE_NAME
     #define MACHINE_NAME "Mendel"

From 8f620de0ff8e0bd07d86bd1937fb77a68b701fa1 Mon Sep 17 00:00:00 2001
From: Richard Wackerbarth <rkw@dataplex.net>
Date: Wed, 11 Feb 2015 09:00:33 -0600
Subject: [PATCH 37/37] Actually activate Automatic Versioning

---
 .../Arduino_1.5.x/hardware/marlin/avr/platform.local.txt         | 1 +
 1 file changed, 1 insertion(+)
 create mode 100644 ArduinoAddons/Arduino_1.5.x/hardware/marlin/avr/platform.local.txt

diff --git a/ArduinoAddons/Arduino_1.5.x/hardware/marlin/avr/platform.local.txt b/ArduinoAddons/Arduino_1.5.x/hardware/marlin/avr/platform.local.txt
new file mode 100644
index 0000000000..ff2ad5d22c
--- /dev/null
+++ b/ArduinoAddons/Arduino_1.5.x/hardware/marlin/avr/platform.local.txt
@@ -0,0 +1 @@
+compiler.cpp.extra_flags=-DHAS_AUTOMATIC_VERSIONING