diff --git a/.gitignore b/.gitignore
index cd72efb10d2..380a0285068 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/
 *~
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 00000000000..ff2ad5d22c6
--- /dev/null
+++ b/ArduinoAddons/Arduino_1.5.x/hardware/marlin/avr/platform.local.txt
@@ -0,0 +1 @@
+compiler.cpp.extra_flags=-DHAS_AUTOMATIC_VERSIONING
diff --git a/Marlin/Conditionals.h b/Marlin/Conditionals.h
index c7e20847245..5d3213e0e35 100644
--- a/Marlin/Conditionals.h
+++ b/Marlin/Conditionals.h
@@ -10,6 +10,8 @@
 
 #ifndef CONFIGURATION_LCD // Get the LCD defines which are needed first
 
+  #define PIN_EXISTS(PN) (defined(PN##_PIN) && PN##_PIN >= 0)
+
   #define CONFIGURATION_LCD
 
   #if defined(MAKRPANEL)
@@ -189,6 +191,9 @@
       #define ENDSTOPPULLUP_YMIN
       #define ENDSTOPPULLUP_ZMIN
     #endif
+    #ifndef DISABLE_Z_PROBE_ENDSTOP
+      #define ENDSTOPPULLUP_ZPROBE
+    #endif
   #endif
 
   /**
@@ -276,7 +281,7 @@
     #define PS_ON_AWAKE  HIGH
     #define PS_ON_ASLEEP LOW
   #endif
-  #define HAS_POWER_SWITCH (POWER_SUPPLY > 0 && defined(PS_ON_PIN) && PS_ON_PIN >= 0)
+  #define HAS_POWER_SWITCH (POWER_SUPPLY > 0 && PIN_EXISTS(PS_ON))
 
   /**
    * Temp Sensor defines
@@ -347,25 +352,81 @@
   #endif
 
   /**
-   * Shorthand for pin tests, for temperature.cpp
+   * Shorthand for pin tests, used wherever needed
    */
-  #define HAS_TEMP_0 (defined(TEMP_0_PIN) && TEMP_0_PIN >= 0 && TEMP_SENSOR_0 != 0 && TEMP_SENSOR_0 != -2)
-  #define HAS_TEMP_1 (defined(TEMP_1_PIN) && TEMP_1_PIN >= 0 && TEMP_SENSOR_1 != 0)
-  #define HAS_TEMP_2 (defined(TEMP_2_PIN) && TEMP_2_PIN >= 0 && TEMP_SENSOR_2 != 0)
-  #define HAS_TEMP_3 (defined(TEMP_3_PIN) && TEMP_3_PIN >= 0 && TEMP_SENSOR_3 != 0)
-  #define HAS_TEMP_BED (defined(TEMP_BED_PIN) && TEMP_BED_PIN >= 0 && TEMP_SENSOR_BED != 0)
-  #define HAS_FILAMENT_SENSOR (defined(FILAMENT_SENSOR) && defined(FILWIDTH_PIN) && FILWIDTH_PIN >= 0)
-  #define HAS_HEATER_0 (defined(HEATER_0_PIN) && HEATER_0_PIN >= 0)
-  #define HAS_HEATER_1 (defined(HEATER_1_PIN) && HEATER_1_PIN >= 0)
-  #define HAS_HEATER_2 (defined(HEATER_2_PIN) && HEATER_2_PIN >= 0)
-  #define HAS_HEATER_3 (defined(HEATER_3_PIN) && HEATER_3_PIN >= 0)
-  #define HAS_HEATER_BED (defined(HEATER_BED_PIN) && HEATER_BED_PIN >= 0)
-  #define HAS_AUTO_FAN_0 (defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN >= 0)
-  #define HAS_AUTO_FAN_1 (defined(EXTRUDER_1_AUTO_FAN_PIN) && EXTRUDER_1_AUTO_FAN_PIN >= 0)
-  #define HAS_AUTO_FAN_2 (defined(EXTRUDER_2_AUTO_FAN_PIN) && EXTRUDER_2_AUTO_FAN_PIN >= 0)
-  #define HAS_AUTO_FAN_3 (defined(EXTRUDER_3_AUTO_FAN_PIN) && EXTRUDER_3_AUTO_FAN_PIN >= 0)
+  #define HAS_TEMP_0 (PIN_EXISTS(TEMP_0) && TEMP_SENSOR_0 != 0 && TEMP_SENSOR_0 != -2)
+  #define HAS_TEMP_1 (PIN_EXISTS(TEMP_1) && TEMP_SENSOR_1 != 0)
+  #define HAS_TEMP_2 (PIN_EXISTS(TEMP_2) && TEMP_SENSOR_2 != 0)
+  #define HAS_TEMP_3 (PIN_EXISTS(TEMP_3) && TEMP_SENSOR_3 != 0)
+  #define HAS_TEMP_BED (PIN_EXISTS(TEMP_BED) && TEMP_SENSOR_BED != 0)
+  #define HAS_HEATER_0 (PIN_EXISTS(HEATER_0))
+  #define HAS_HEATER_1 (PIN_EXISTS(HEATER_1))
+  #define HAS_HEATER_2 (PIN_EXISTS(HEATER_2))
+  #define HAS_HEATER_3 (PIN_EXISTS(HEATER_3))
+  #define HAS_HEATER_BED (PIN_EXISTS(HEATER_BED))
+  #define HAS_AUTO_FAN_0 (PIN_EXISTS(EXTRUDER_0_AUTO_FAN))
+  #define HAS_AUTO_FAN_1 (PIN_EXISTS(EXTRUDER_1_AUTO_FAN))
+  #define HAS_AUTO_FAN_2 (PIN_EXISTS(EXTRUDER_2_AUTO_FAN))
+  #define HAS_AUTO_FAN_3 (PIN_EXISTS(EXTRUDER_3_AUTO_FAN))
   #define HAS_AUTO_FAN (HAS_AUTO_FAN_0 || HAS_AUTO_FAN_1 || HAS_AUTO_FAN_2 || HAS_AUTO_FAN_3)
-  #define HAS_FAN (defined(FAN_PIN) && FAN_PIN >= 0)
+  #define HAS_FAN (PIN_EXISTS(FAN))
+  #define HAS_CONTROLLERFAN (PIN_EXISTS(CONTROLLERFAN))
+  #define HAS_SERVO_0 (PIN_EXISTS(SERVO0))
+  #define HAS_SERVO_1 (PIN_EXISTS(SERVO1))
+  #define HAS_SERVO_2 (PIN_EXISTS(SERVO2))
+  #define HAS_SERVO_3 (PIN_EXISTS(SERVO3))
+  #define HAS_FILAMENT_SENSOR (defined(FILAMENT_SENSOR) && PIN_EXISTS(FILWIDTH))
+  #define HAS_FILRUNOUT (PIN_EXISTS(FILRUNOUT))
+  #define HAS_HOME (PIN_EXISTS(HOME))
+  #define HAS_KILL (PIN_EXISTS(KILL))
+  #define HAS_SUICIDE (PIN_EXISTS(SUICIDE))
+  #define HAS_PHOTOGRAPH (PIN_EXISTS(PHOTOGRAPH))
+  #define HAS_X_MIN (PIN_EXISTS(X_MIN))
+  #define HAS_X_MAX (PIN_EXISTS(X_MAX))
+  #define HAS_Y_MIN (PIN_EXISTS(Y_MIN))
+  #define HAS_Y_MAX (PIN_EXISTS(Y_MAX))
+  #define HAS_Z_MIN (PIN_EXISTS(Z_MIN))
+  #define HAS_Z_MAX (PIN_EXISTS(Z_MAX))
+  #define HAS_Z2_MIN (PIN_EXISTS(Z2_MIN))
+  #define HAS_Z2_MAX (PIN_EXISTS(Z2_MAX))
+  #define HAS_Z_PROBE (PIN_EXISTS(Z_PROBE))
+  #define HAS_SOLENOID_1 (PIN_EXISTS(SOL1))
+  #define HAS_SOLENOID_2 (PIN_EXISTS(SOL2))
+  #define HAS_SOLENOID_3 (PIN_EXISTS(SOL3))
+  #define HAS_MICROSTEPS (PIN_EXISTS(X_MS1))
+  #define HAS_MICROSTEPS_E0 (PIN_EXISTS(E0_MS1))
+  #define HAS_MICROSTEPS_E1 (PIN_EXISTS(E1_MS1))
+  #define HAS_MICROSTEPS_E2 (PIN_EXISTS(E2_MS1))
+  #define HAS_X_ENABLE (PIN_EXISTS(X_ENABLE))
+  #define HAS_X2_ENABLE (PIN_EXISTS(X2_ENABLE))
+  #define HAS_Y_ENABLE (PIN_EXISTS(Y_ENABLE))
+  #define HAS_Y2_ENABLE (PIN_EXISTS(Y2_ENABLE))
+  #define HAS_Z_ENABLE (PIN_EXISTS(Z_ENABLE))
+  #define HAS_Z2_ENABLE (PIN_EXISTS(Z2_ENABLE))
+  #define HAS_E0_ENABLE (PIN_EXISTS(E0_ENABLE))
+  #define HAS_E1_ENABLE (PIN_EXISTS(E1_ENABLE))
+  #define HAS_E2_ENABLE (PIN_EXISTS(E2_ENABLE))
+  #define HAS_E3_ENABLE (PIN_EXISTS(E3_ENABLE))
+  #define HAS_X_DIR (PIN_EXISTS(X_DIR))
+  #define HAS_X2_DIR (PIN_EXISTS(X2_DIR))
+  #define HAS_Y_DIR (PIN_EXISTS(Y_DIR))
+  #define HAS_Y2_DIR (PIN_EXISTS(Y2_DIR))
+  #define HAS_Z_DIR (PIN_EXISTS(Z_DIR))
+  #define HAS_Z2_DIR (PIN_EXISTS(Z2_DIR))
+  #define HAS_E0_DIR (PIN_EXISTS(E0_DIR))
+  #define HAS_E1_DIR (PIN_EXISTS(E1_DIR))
+  #define HAS_E2_DIR (PIN_EXISTS(E2_DIR))
+  #define HAS_E3_DIR (PIN_EXISTS(E3_DIR))
+  #define HAS_X_STEP (PIN_EXISTS(X_STEP))
+  #define HAS_X2_STEP (PIN_EXISTS(X2_STEP))
+  #define HAS_Y_STEP (PIN_EXISTS(Y_STEP))
+  #define HAS_Y2_STEP (PIN_EXISTS(Y2_STEP))
+  #define HAS_Z_STEP (PIN_EXISTS(Z_STEP))
+  #define HAS_Z2_STEP (PIN_EXISTS(Z2_STEP))
+  #define HAS_E0_STEP (PIN_EXISTS(E0_STEP))
+  #define HAS_E1_STEP (PIN_EXISTS(E1_STEP))
+  #define HAS_E2_STEP (PIN_EXISTS(E2_STEP))
+  #define HAS_E3_STEP (PIN_EXISTS(E3_STEP))
 
   /**
    * Helper Macros for heaters and extruder fan
diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index 5a6d483bf90..7de354f8828 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -31,7 +31,7 @@ Here are some standard links for getting your machine calibrated:
 //===========================================================================
 //============================= SCARA Printer ===============================
 //===========================================================================
-// For a Delta printer replace the configuration files with the files in the
+// For a Scara printer replace the configuration files with the files in the
 // example_configurations/SCARA directory.
 //
 
@@ -319,6 +319,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
 #endif
 
 // Mechanical endstop with COM to ground and NC to Signal uses "false" here (most common setup).
@@ -328,8 +329,14 @@ 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_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
@@ -387,11 +394,11 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 // #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
 // #define MESH_BED_LEVELING    // Enable mesh bed leveling
 
-#if defined(MANUAL_BED_LEVELING)
+#ifdef MANUAL_BED_LEVELING
   #define MBL_Z_STEP 0.025
 #endif  // MANUAL_BED_LEVELING
 
-#if defined(MESH_BED_LEVELING)
+#ifdef MESH_BED_LEVELING
   #define MESH_MIN_X 10
   #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
   #define MESH_MIN_Y 10
@@ -496,6 +503,20 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 
   #endif
 
+  // 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 separate 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. 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_ENDSTOP
+
 #endif // ENABLE_AUTO_BED_LEVELING
 
 
@@ -512,8 +533,10 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
   //#define MANUAL_Z_HOME_POS 402 // For delta: Distance between nozzle and print surface after homing.
 #endif
 
-//// MOVEMENT SETTINGS
-#define NUM_AXIS 4 // The axis order in all axis related arrays is X, Y, Z, E
+/**
+ * MOVEMENT SETTINGS
+ */
+
 #define HOMING_FEEDRATE {50*60, 50*60, 4*60, 0}  // set the homing speeds (mm/min)
 
 // default settings
@@ -674,7 +697,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 // Data from: http://www.doc-diy.net/photo/rc-1_hacked/
 // #define PHOTOGRAPH_PIN     23
 
-// SF send wrong arc g-codes when using Arc Point as fillet procedure
+// SkeinForge sends the wrong arc g-codes when using Arc Point as fillet procedure
 //#define SF_ARC_FIX
 
 // Support for the BariCUDA Paste Extruder.
diff --git a/Marlin/ConfigurationStore.cpp b/Marlin/ConfigurationStore.cpp
index b1da94a3006..56de8168485 100644
--- a/Marlin/ConfigurationStore.cpp
+++ b/Marlin/ConfigurationStore.cpp
@@ -3,7 +3,21 @@
  *
  * Configuration and EEPROM storage
  *
- * V16 EEPROM Layout:
+ * IMPORTANT:  Whenever there are changes made to the variables stored in EEPROM
+ * in the functions below, also increment the version number. This makes sure that
+ * the default values are used whenever there is a change to the data, to prevent
+ * wrong data being written to the variables.
+ *
+ * ALSO: Variables in the Store and Retrieve sections must be in the same order.
+ *       If a feature is disabled, some data must still be written that, when read,
+ *       either sets a Sane Default, or results in No Change to the existing value.
+ *
+ */
+
+#define EEPROM_VERSION "V19"
+
+/**
+ * V19 EEPROM Layout:
  *
  *  ver
  *  axis_steps_per_unit (x4)
@@ -47,6 +61,9 @@
  *  Kp[2], Ki[2], Kd[2], Kc[2]
  *  Kp[3], Ki[3], Kd[3], Kc[3]
  *
+ * PIDTEMPBED:
+ *  bedKp, bedKi, bedKd
+ *
  * DOGLCD:
  *  lcd_contrast
  *
@@ -78,7 +95,7 @@
 #include "ultralcd.h"
 #include "ConfigurationStore.h"
 
-#if defined(MESH_BED_LEVELING)
+#ifdef MESH_BED_LEVELING
    #include "mesh_bed_leveling.h"
 #endif  // MESH_BED_LEVELING
 
@@ -111,15 +128,6 @@ void _EEPROM_readData(int &pos, uint8_t* value, uint8_t size) {
 
 #define EEPROM_OFFSET 100
 
-
-// IMPORTANT:  Whenever there are changes made to the variables stored in EEPROM
-// in the functions below, also increment the version number. This makes sure that
-// the default values are used whenever there is a change to the data, to prevent
-// wrong data being written to the variables.
-// ALSO:  always make sure the variables in the Store and retrieve sections are in the same order.
-
-#define EEPROM_VERSION "V18"
-
 #ifdef EEPROM_SETTINGS
 
 void Config_StoreSettings()  {
@@ -194,7 +202,6 @@ void Config_StoreSettings()  {
   EEPROM_WRITE_VAR(i, absPreheatHPBTemp);
   EEPROM_WRITE_VAR(i, absPreheatFanSpeed);
 
-
   for (int e = 0; e < 4; e++) {
 
     #ifdef PIDTEMP
@@ -209,12 +216,10 @@ void Config_StoreSettings()  {
           EEPROM_WRITE_VAR(i, dummy);
         #endif
       }
-      else {
-    #else // !PIDTEMP
-      {
+      else
     #endif // !PIDTEMP
-
-        dummy = DUMMY_PID_VALUE;
+      {
+        dummy = DUMMY_PID_VALUE; // When read, will not change the existing value
         EEPROM_WRITE_VAR(i, dummy);
         dummy = 0.0f;
         for (int q = 3; q--;) EEPROM_WRITE_VAR(i, dummy);
@@ -222,6 +227,14 @@ void Config_StoreSettings()  {
 
   } // Extruders Loop
 
+  #ifndef PIDTEMPBED
+    float bedKp = DUMMY_PID_VALUE, bedKi = DUMMY_PID_VALUE, bedKd = DUMMY_PID_VALUE;
+  #endif
+
+  EEPROM_WRITE_VAR(i, bedKp);
+  EEPROM_WRITE_VAR(i, bedKi);
+  EEPROM_WRITE_VAR(i, bedKd);
+
   #ifndef DOGLCD
     int lcd_contrast = 32;
   #endif
@@ -308,7 +321,7 @@ void Config_RetrieveSettings() {
 
     uint8_t mesh_num_x = 0;
     uint8_t mesh_num_y = 0;
-    #if defined(MESH_BED_LEVELING)
+    #ifdef MESH_BED_LEVELING
       EEPROM_READ_VAR(i, mbl.active);
       EEPROM_READ_VAR(i, mesh_num_x);
       EEPROM_READ_VAR(i, mesh_num_y);
@@ -364,7 +377,7 @@ void Config_RetrieveSettings() {
 
     #ifdef PIDTEMP
       for (int e = 0; e < 4; e++) { // 4 = max extruders currently supported by Marlin
-        EEPROM_READ_VAR(i, dummy);
+        EEPROM_READ_VAR(i, dummy); // Kp
         if (e < EXTRUDERS && dummy != DUMMY_PID_VALUE) {
           // do not need to scale PID values as the values in EEPROM are already scaled
           PID_PARAM(Kp, e) = dummy;
@@ -385,6 +398,20 @@ void Config_RetrieveSettings() {
       for (int q=16; q--;) EEPROM_READ_VAR(i, dummy);  // 4x Kp, Ki, Kd, Kc
     #endif // !PIDTEMP
 
+    #ifndef PIDTEMPBED
+      float bedKp, bedKi, bedKd;
+    #endif
+
+    EEPROM_READ_VAR(i, dummy); // bedKp
+    if (dummy != DUMMY_PID_VALUE) {
+      bedKp = dummy;
+      EEPROM_READ_VAR(i, bedKi);
+      EEPROM_READ_VAR(i, bedKd);
+    }
+    else {
+      for (int q=2; q--;) EEPROM_READ_VAR(i, dummy); // bedKi, bedKd
+    }
+
     #ifndef DOGLCD
       int lcd_contrast;
     #endif
@@ -517,6 +544,12 @@ void Config_ResetDefault() {
     updatePID();
   #endif // PIDTEMP
 
+  #ifdef PIDTEMPBED
+    bedKp = DEFAULT_bedKp;
+    bedKi = scalePID_i(DEFAULT_bedKi);
+    bedKd = scalePID_d(DEFAULT_bedKd);
+  #endif
+
   #ifdef FWRETRACT
     autoretract_enabled = false;
     retract_length = RETRACT_LENGTH;
@@ -660,17 +693,28 @@ void Config_PrintSettings(bool forReplay) {
     SERIAL_EOL;  
   #endif // DELTA
 
-  #ifdef PIDTEMP
+  #if defined(PIDTEMP) || defined(PIDTEMPBED)
     SERIAL_ECHO_START;
     if (!forReplay) {
       SERIAL_ECHOLNPGM("PID settings:");
       SERIAL_ECHO_START;
     }
-    SERIAL_ECHOPAIR("   M301 P", PID_PARAM(Kp, 0)); // for compatibility with hosts, only echos values for E0
-    SERIAL_ECHOPAIR(" I", unscalePID_i(PID_PARAM(Ki, 0)));
-    SERIAL_ECHOPAIR(" D", unscalePID_d(PID_PARAM(Kd, 0)));
-    SERIAL_EOL;
-  #endif // PIDTEMP
+    #if defined(PIDTEMP) && defined(PIDTEMPBED)
+      SERIAL_EOL;
+    #endif
+    #ifdef PIDTEMP
+      SERIAL_ECHOPAIR("  M301 P", PID_PARAM(Kp, 0)); // for compatibility with hosts, only echos values for E0
+      SERIAL_ECHOPAIR(" I", unscalePID_i(PID_PARAM(Ki, 0)));
+      SERIAL_ECHOPAIR(" D", unscalePID_d(PID_PARAM(Kd, 0)));
+      SERIAL_EOL;
+    #endif
+    #ifdef PIDTEMPBED
+      SERIAL_ECHOPAIR("  M304 P", bedKp); // for compatibility with hosts, only echos values for E0
+      SERIAL_ECHOPAIR(" I", unscalePID_i(bedKi));
+      SERIAL_ECHOPAIR(" D", unscalePID_d(bedKd));
+      SERIAL_EOL;
+    #endif
+  #endif
 
   #ifdef FWRETRACT
 
@@ -679,7 +723,7 @@ void Config_PrintSettings(bool forReplay) {
       SERIAL_ECHOLNPGM("Retract: S=Length (mm) F:Speed (mm/m) Z: ZLift (mm)");
       SERIAL_ECHO_START;
     }
-    SERIAL_ECHOPAIR("   M207 S", retract_length);
+    SERIAL_ECHOPAIR("  M207 S", retract_length);
     SERIAL_ECHOPAIR(" F", retract_feedrate*60);
     SERIAL_ECHOPAIR(" Z", retract_zlift);
     SERIAL_EOL;
@@ -688,7 +732,7 @@ void Config_PrintSettings(bool forReplay) {
       SERIAL_ECHOLNPGM("Recover: S=Extra length (mm) F:Speed (mm/m)");
       SERIAL_ECHO_START;
     }
-    SERIAL_ECHOPAIR("   M208 S", retract_recover_length);
+    SERIAL_ECHOPAIR("  M208 S", retract_recover_length);
     SERIAL_ECHOPAIR(" F", retract_recover_feedrate*60);
     SERIAL_EOL;
     SERIAL_ECHO_START;
@@ -696,7 +740,7 @@ void Config_PrintSettings(bool forReplay) {
       SERIAL_ECHOLNPGM("Auto-Retract: S=0 to disable, 1 to interpret extrude-only moves as retracts or recoveries");
       SERIAL_ECHO_START;
     }
-    SERIAL_ECHOPAIR("   M209 S", (unsigned long)(autoretract_enabled ? 1 : 0));
+    SERIAL_ECHOPAIR("  M209 S", (unsigned long)(autoretract_enabled ? 1 : 0));
     SERIAL_EOL;
 
     #if EXTRUDERS > 1
@@ -720,20 +764,20 @@ void Config_PrintSettings(bool forReplay) {
       SERIAL_ECHOLNPGM("Filament settings:");
       SERIAL_ECHO_START;
     }
-    SERIAL_ECHOPAIR("   M200 D", filament_size[0]);
+    SERIAL_ECHOPAIR("  M200 D", filament_size[0]);
     SERIAL_EOL;
 
     #if EXTRUDERS > 1
       SERIAL_ECHO_START;
-      SERIAL_ECHOPAIR("   M200 T1 D", filament_size[1]);
+      SERIAL_ECHOPAIR("  M200 T1 D", filament_size[1]);
       SERIAL_EOL;
       #if EXTRUDERS > 2
         SERIAL_ECHO_START;
-        SERIAL_ECHOPAIR("   M200 T2 D", filament_size[2]);
+        SERIAL_ECHOPAIR("  M200 T2 D", filament_size[2]);
         SERIAL_EOL;
         #if EXTRUDERS > 3
           SERIAL_ECHO_START;
-          SERIAL_ECHOPAIR("   M200 T3 D", filament_size[3]);
+          SERIAL_ECHOPAIR("  M200 T3 D", filament_size[3]);
           SERIAL_EOL;
         #endif
       #endif
@@ -752,7 +796,7 @@ void Config_PrintSettings(bool forReplay) {
         SERIAL_ECHOLNPGM("Z-Probe Offset (mm):");
         SERIAL_ECHO_START;
       }
-      SERIAL_ECHOPAIR("   M", (unsigned long)CUSTOM_M_CODE_SET_Z_PROBE_OFFSET);
+      SERIAL_ECHOPAIR("  M", (unsigned long)CUSTOM_M_CODE_SET_Z_PROBE_OFFSET);
       SERIAL_ECHOPAIR(" Z", -zprobe_zoffset);
     #else
       if (!forReplay) {
diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h
index d689ac47f89..4ed188946d8 100644
--- a/Marlin/Configuration_adv.h
+++ b/Marlin/Configuration_adv.h
@@ -175,9 +175,9 @@
 #endif //DUAL_X_CARRIAGE
 
 //homing hits the endstop, then retracts by this distance, before it tries to slowly bump again:
-#define X_HOME_RETRACT_MM 5
-#define Y_HOME_RETRACT_MM 5
-#define Z_HOME_RETRACT_MM 2
+#define X_HOME_BUMP_MM 5
+#define Y_HOME_BUMP_MM 5
+#define Z_HOME_BUMP_MM 2
 #define HOMING_BUMP_DIVISOR {2, 2, 4}  // Re-Bump Speed Divisor (Divides the Homing Feedrate)
 //#define QUICK_HOME  //if this is defined, if both x and y are to be homed, a diagonal move will be performed initially.
 
diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h
index e0441714b19..36e054eec0e 100644
--- a/Marlin/Marlin.h
+++ b/Marlin/Marlin.h
@@ -62,59 +62,57 @@
   #define MYSERIAL MSerial
 #endif
 
-#define SERIAL_PROTOCOL(x) (MYSERIAL.print(x))
-#define SERIAL_PROTOCOL_F(x,y) (MYSERIAL.print(x,y))
-#define SERIAL_PROTOCOLPGM(x) (serialprintPGM(PSTR(x)))
-#define SERIAL_PROTOCOLLN(x) (MYSERIAL.print(x),MYSERIAL.write('\n'))
-#define SERIAL_PROTOCOLLNPGM(x) (serialprintPGM(PSTR(x)),MYSERIAL.write('\n'))
+#define SERIAL_CHAR(x) MYSERIAL.write(x)
+#define SERIAL_EOL SERIAL_CHAR('\n')
+
+#define SERIAL_PROTOCOLCHAR(x) SERIAL_CHAR(x)
+#define SERIAL_PROTOCOL(x) MYSERIAL.print(x)
+#define SERIAL_PROTOCOL_F(x,y) MYSERIAL.print(x,y)
+#define SERIAL_PROTOCOLPGM(x) serialprintPGM(PSTR(x))
+#define SERIAL_PROTOCOLLN(x) do{ MYSERIAL.print(x),MYSERIAL.write('\n'); }while(0)
+#define SERIAL_PROTOCOLLNPGM(x) do{ serialprintPGM(PSTR(x)),MYSERIAL.write('\n'); }while(0)
 
 
 extern const char errormagic[] PROGMEM;
 extern const char echomagic[] PROGMEM;
 
-#define SERIAL_ERROR_START (serialprintPGM(errormagic))
+#define SERIAL_ERROR_START serialprintPGM(errormagic)
 #define SERIAL_ERROR(x) SERIAL_PROTOCOL(x)
 #define SERIAL_ERRORPGM(x) SERIAL_PROTOCOLPGM(x)
 #define SERIAL_ERRORLN(x) SERIAL_PROTOCOLLN(x)
 #define SERIAL_ERRORLNPGM(x) SERIAL_PROTOCOLLNPGM(x)
 
-#define SERIAL_ECHO_START (serialprintPGM(echomagic))
+#define SERIAL_ECHO_START serialprintPGM(echomagic)
 #define SERIAL_ECHO(x) SERIAL_PROTOCOL(x)
 #define SERIAL_ECHOPGM(x) SERIAL_PROTOCOLPGM(x)
 #define SERIAL_ECHOLN(x) SERIAL_PROTOCOLLN(x)
 #define SERIAL_ECHOLNPGM(x) SERIAL_PROTOCOLLNPGM(x)
 
-#define SERIAL_ECHOPAIR(name,value) (serial_echopair_P(PSTR(name),(value)))
-
-#define SERIAL_EOL MYSERIAL.write('\n')
+#define SERIAL_ECHOPAIR(name,value) do{ serial_echopair_P(PSTR(name),(value)); }while(0)
 
 void serial_echopair_P(const char *s_P, float v);
 void serial_echopair_P(const char *s_P, double v);
 void serial_echopair_P(const char *s_P, unsigned long v);
 
 
-//Things to write to serial from Program memory. Saves 400 to 2k of RAM.
-FORCE_INLINE void serialprintPGM(const char *str)
-{
-  char ch=pgm_read_byte(str);
-  while(ch)
-  {
+// Things to write to serial from Program memory. Saves 400 to 2k of RAM.
+FORCE_INLINE void serialprintPGM(const char *str) {
+  char ch;
+  while ((ch = pgm_read_byte(str))) {
     MYSERIAL.write(ch);
-    ch=pgm_read_byte(++str);
+    str++;
   }
 }
 
-
 void get_command();
 void process_commands();
 
 void manage_inactivity(bool ignore_stepper_queue=false);
 
-#if defined(DUAL_X_CARRIAGE) && defined(X_ENABLE_PIN) && X_ENABLE_PIN > -1 \
-    && defined(X2_ENABLE_PIN) && X2_ENABLE_PIN > -1
+#if defined(DUAL_X_CARRIAGE) && HAS_X_ENABLE && HAS_X2_ENABLE
   #define  enable_x() do { X_ENABLE_WRITE( X_ENABLE_ON); X2_ENABLE_WRITE( X_ENABLE_ON); } while (0)
   #define disable_x() do { X_ENABLE_WRITE(!X_ENABLE_ON); X2_ENABLE_WRITE(!X_ENABLE_ON); axis_known_position[X_AXIS] = false; } while (0)
-#elif defined(X_ENABLE_PIN) && X_ENABLE_PIN > -1
+#elif HAS_X_ENABLE
   #define  enable_x() X_ENABLE_WRITE( X_ENABLE_ON)
   #define disable_x() { X_ENABLE_WRITE(!X_ENABLE_ON); axis_known_position[X_AXIS] = false; }
 #else
@@ -122,7 +120,7 @@ void manage_inactivity(bool ignore_stepper_queue=false);
   #define disable_x() ;
 #endif
 
-#if defined(Y_ENABLE_PIN) && Y_ENABLE_PIN > -1
+#if HAS_Y_ENABLE
   #ifdef Y_DUAL_STEPPER_DRIVERS
     #define  enable_y() { Y_ENABLE_WRITE( Y_ENABLE_ON); Y2_ENABLE_WRITE(Y_ENABLE_ON); }
     #define disable_y() { Y_ENABLE_WRITE(!Y_ENABLE_ON); Y2_ENABLE_WRITE(!Y_ENABLE_ON); axis_known_position[Y_AXIS] = false; }
@@ -135,7 +133,7 @@ void manage_inactivity(bool ignore_stepper_queue=false);
   #define disable_y() ;
 #endif
 
-#if defined(Z_ENABLE_PIN) && Z_ENABLE_PIN > -1
+#if HAS_Z_ENABLE
   #ifdef Z_DUAL_STEPPER_DRIVERS
     #define  enable_z() { Z_ENABLE_WRITE( Z_ENABLE_ON); Z2_ENABLE_WRITE(Z_ENABLE_ON); }
     #define disable_z() { Z_ENABLE_WRITE(!Z_ENABLE_ON); Z2_ENABLE_WRITE(!Z_ENABLE_ON); axis_known_position[Z_AXIS] = false; }
@@ -148,40 +146,53 @@ void manage_inactivity(bool ignore_stepper_queue=false);
   #define disable_z() ;
 #endif
 
-#if defined(E0_ENABLE_PIN) && (E0_ENABLE_PIN > -1)
-  #define enable_e0() E0_ENABLE_WRITE(E_ENABLE_ON)
+#if HAS_E0_ENABLE
+  #define enable_e0()  E0_ENABLE_WRITE( E_ENABLE_ON)
   #define disable_e0() E0_ENABLE_WRITE(!E_ENABLE_ON)
 #else
   #define enable_e0()  /* nothing */
   #define disable_e0() /* nothing */
 #endif
 
-#if (EXTRUDERS > 1) && defined(E1_ENABLE_PIN) && (E1_ENABLE_PIN > -1)
-  #define enable_e1() E1_ENABLE_WRITE(E_ENABLE_ON)
+#if (EXTRUDERS > 1) && HAS_E1_ENABLE
+  #define enable_e1()  E1_ENABLE_WRITE( E_ENABLE_ON)
   #define disable_e1() E1_ENABLE_WRITE(!E_ENABLE_ON)
 #else
   #define enable_e1()  /* nothing */
   #define disable_e1() /* nothing */
 #endif
 
-#if (EXTRUDERS > 2) && defined(E2_ENABLE_PIN) && (E2_ENABLE_PIN > -1)
-  #define enable_e2() E2_ENABLE_WRITE(E_ENABLE_ON)
+#if (EXTRUDERS > 2) && HAS_E2_ENABLE
+  #define enable_e2()  E2_ENABLE_WRITE( E_ENABLE_ON)
   #define disable_e2() E2_ENABLE_WRITE(!E_ENABLE_ON)
 #else
   #define enable_e2()  /* nothing */
   #define disable_e2() /* nothing */
 #endif
 
-#if (EXTRUDERS > 3) && defined(E3_ENABLE_PIN) && (E3_ENABLE_PIN > -1)
-  #define enable_e3() E3_ENABLE_WRITE(E_ENABLE_ON)
+#if (EXTRUDERS > 3) && HAS_E3_ENABLE
+  #define enable_e3()  E3_ENABLE_WRITE( E_ENABLE_ON)
   #define disable_e3() E3_ENABLE_WRITE(!E_ENABLE_ON)
 #else
   #define enable_e3()  /* nothing */
   #define disable_e3() /* nothing */
 #endif
 
+/**
+ * The axis order in all axis related arrays is X, Y, Z, E
+ */
+#define NUM_AXIS 4
+
+/**
+ * Axis indices as enumerated constants
+ *
+ * A_AXIS and B_AXIS are used by COREXY printers
+ * X_HEAD and Y_HEAD is used for systems that don't have a 1:1 relationship between X_AXIS and X Head movement, like CoreXY bots.
+ */
 enum AxisEnum {X_AXIS=0, Y_AXIS=1, A_AXIS=0, B_AXIS=1, Z_AXIS=2, E_AXIS=3, X_HEAD=4, Y_HEAD=5};
-//X_HEAD and Y_HEAD is used for systems that don't have a 1:1 relationship between X_AXIS and X Head movement, like CoreXY bots.
+
+void enable_all_steppers();
+void disable_all_steppers();
 
 void FlushSerialRequestResend();
 void ClearToSend();
@@ -194,7 +205,6 @@ void get_coordinates();
     void adjust_delta(float cartesian[3]);
   #endif
   extern float delta[3];
-  void prepare_move_raw();
 #endif
 #ifdef SCARA
   void calculate_delta(float cartesian[3]);
@@ -217,7 +227,8 @@ void enquecommands_P(const char *cmd); //put one or many ASCII commands at the e
 void prepare_arc_move(char isclockwise);
 void clamp_to_software_endstops(float target[3]);
 
-void refresh_cmd_timeout(void);
+extern unsigned long previous_millis_cmd;
+inline void refresh_cmd_timeout() { previous_millis_cmd = millis(); }
 
 #ifdef FAST_PWM_FAN
   void setPwmFrequency(uint8_t pin, int val);
@@ -226,7 +237,7 @@ void refresh_cmd_timeout(void);
 #ifndef CRITICAL_SECTION_START
   #define CRITICAL_SECTION_START  unsigned char _sreg = SREG; cli();
   #define CRITICAL_SECTION_END    SREG = _sreg;
-#endif //CRITICAL_SECTION_START
+#endif
 
 extern float homing_feedrate[];
 extern bool axis_relative_modes[];
@@ -235,8 +246,9 @@ 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.
 extern float volumetric_multiplier[EXTRUDERS]; // reciprocal of cross-sectional area of filament (in square millimeters), stored this way to reduce computational burden in planner
-extern float current_position[NUM_AXIS] ;
+extern float current_position[NUM_AXIS];
 extern float home_offset[3];
+
 #ifdef DELTA
   extern float endstop_adj[3];
   extern float delta_radius;
@@ -244,18 +256,23 @@ extern float home_offset[3];
   extern float delta_segments_per_second;
   void recalc_delta_settings(float radius, float diagonal_rod);
 #elif defined(Z_DUAL_ENDSTOPS)
-extern float z_endstop_adj;
+  extern float z_endstop_adj;
 #endif
+
 #ifdef SCARA
   extern float axis_scaling[3];  // Build size scaling
 #endif
+
 extern float min_pos[3];
 extern float max_pos[3];
 extern bool axis_known_position[3];
+
 #ifdef ENABLE_AUTO_BED_LEVELING
   extern float zprobe_zoffset;
 #endif
+
 extern int fanSpeed;
+
 #ifdef BARICUDA
   extern int ValvePressure;
   extern int EtoPPressure;
diff --git a/Marlin/MarlinSerial.cpp b/Marlin/MarlinSerial.cpp
index d8477b6db1d..dc36e14c6c9 100644
--- a/Marlin/MarlinSerial.cpp
+++ b/Marlin/MarlinSerial.cpp
@@ -268,8 +268,7 @@ void MarlinSerial::printFloat(double number, uint8_t digits) {
   print(int_part);
 
   // Print the decimal point, but only if there are digits beyond
-  if (digits > 0)
-    print("."); 
+  if (digits > 0) print('.');
 
   // Extract digits from the remainder one at a time
   while (digits-- > 0) {
@@ -288,7 +287,6 @@ MarlinSerial MSerial;
 #endif // !AT90USB
 
 // For AT90USB targets use the UART for BT interfacing
-#if defined(AT90USB) && defined (BTENABLED)
-   HardwareSerial bt;
+#if defined(AT90USB) && defined(BTENABLED)
+  HardwareSerial bt;
 #endif
-
diff --git a/Marlin/MarlinSerial.h b/Marlin/MarlinSerial.h
index b56880c23f6..dbad3fd7626 100644
--- a/Marlin/MarlinSerial.h
+++ b/Marlin/MarlinSerial.h
@@ -153,8 +153,8 @@ extern MarlinSerial MSerial;
 #endif // !AT90USB
 
 // Use the UART for BT in AT90USB configurations
-#if defined(AT90USB) && defined (BTENABLED)
-   extern HardwareSerial bt;
+#if defined(AT90USB) && defined(BTENABLED)
+  extern HardwareSerial bt;
 #endif
 
 #endif
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index a327f209d85..1b9260de535 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -36,11 +36,11 @@
   #endif
 #endif // ENABLE_AUTO_BED_LEVELING
 
-#define SERVO_LEVELING defined(ENABLE_AUTO_BED_LEVELING) && PROBE_SERVO_DEACTIVATION_DELAY > 0
+#define SERVO_LEVELING (defined(ENABLE_AUTO_BED_LEVELING) && PROBE_SERVO_DEACTIVATION_DELAY > 0)
 
-#if defined(MESH_BED_LEVELING)
+#ifdef MESH_BED_LEVELING
   #include "mesh_bed_leveling.h"
-#endif  // MESH_BED_LEVELING
+#endif
 
 #include "ultralcd.h"
 #include "planner.h"
@@ -110,6 +110,7 @@
 //        Call gcode file : "M32 P !filename#" and return to caller file after finishing (similar to #include).
 //        The '#' is necessary when calling from within sd files, as it stops buffer prereading
 // M42  - Change pin status via gcode Use M42 Px Sy to set pin x to value y, when omitting Px the onboard led will be used.
+// M48  - Measure Z_Probe repeatability. M48 [n # of points] [X position] [Y position] [V_erboseness #] [E_ngage Probe] [L # of legs of travel]
 // M80  - Turn on Power Supply
 // M81  - Turn off Power Supply
 // M82  - Set E codes absolute (default)
@@ -202,10 +203,6 @@
 #endif
 
 float homing_feedrate[] = HOMING_FEEDRATE;
-#ifdef ENABLE_AUTO_BED_LEVELING
-  int xy_travel_speed = XY_TRAVEL_SPEED;
-  float zprobe_zoffset = -Z_PROBE_OFFSET_FROM_EXTRUDER;
-#endif
 int homing_bump_divisor[] = HOMING_BUMP_DIVISOR;
 bool axis_relative_modes[] = AXIS_RELATIVE_MODES;
 int feedmultiply = 100; //100->1 200->2
@@ -216,15 +213,49 @@ float filament_size[EXTRUDERS] = ARRAY_BY_EXTRUDERS(DEFAULT_NOMINAL_FILAMENT_DIA
 float volumetric_multiplier[EXTRUDERS] = ARRAY_BY_EXTRUDERS(1.0, 1.0, 1.0, 1.0);
 float current_position[NUM_AXIS] = { 0.0 };
 float home_offset[3] = { 0 };
-#ifdef DELTA
-  float endstop_adj[3] = { 0 };
-#elif defined(Z_DUAL_ENDSTOPS)
-  float z_endstop_adj = 0;
-#endif
-
 float min_pos[3] = { X_MIN_POS, Y_MIN_POS, Z_MIN_POS };
 float max_pos[3] = { X_MAX_POS, Y_MAX_POS, Z_MAX_POS };
 bool axis_known_position[3] = { false };
+uint8_t active_extruder = 0;
+int fanSpeed = 0;
+bool cancel_heatup = false;
+const char errormagic[] PROGMEM = "Error:";
+const char echomagic[] PROGMEM = "echo:";
+const char axis_codes[NUM_AXIS] = {'X', 'Y', 'Z', 'E'};
+static float destination[NUM_AXIS] = { 0 };
+static float offset[3] = { 0 };
+static float feedrate = 1500.0, next_feedrate, saved_feedrate;
+static long gcode_N, gcode_LastN, Stopped_gcode_LastN = 0;
+static bool relative_mode = false;  //Determines Absolute or Relative Coordinates
+static char cmdbuffer[BUFSIZE][MAX_CMD_SIZE];
+static int bufindr = 0;
+static int bufindw = 0;
+static int buflen = 0;
+static char serial_char;
+static int serial_count = 0;
+static boolean comment_mode = false;
+static char *strchr_pointer; ///< A pointer to find chars in the command string (X, Y, Z, E, etc.)
+const char* queued_commands_P= NULL; /* pointer to the current line in the active sequence of commands, or NULL when none */
+const int sensitive_pins[] = SENSITIVE_PINS; ///< Sensitive pin list for M42
+// Inactivity shutdown
+unsigned long previous_millis_cmd = 0;
+static unsigned long max_inactive_time = 0;
+static unsigned long stepper_inactive_time = DEFAULT_STEPPER_DEACTIVE_TIME*1000l;
+unsigned long starttime = 0; ///< Print job start time
+unsigned long stoptime = 0;  ///< Print job stop time
+static uint8_t target_extruder;
+bool Stopped = false;
+bool CooldownNoWait = true;
+bool target_direction;
+
+#ifdef ENABLE_AUTO_BED_LEVELING
+  int xy_travel_speed = XY_TRAVEL_SPEED;
+  float zprobe_zoffset = -Z_PROBE_OFFSET_FROM_EXTRUDER;
+#endif
+
+#if defined(Z_DUAL_ENDSTOPS) && !defined(DELTA)
+  float z_endstop_adj = 0;
+#endif
 
 // Extruder offsets
 #if EXTRUDERS > 1
@@ -243,9 +274,6 @@ bool axis_known_position[3] = { false };
   };
 #endif
 
-uint8_t active_extruder = 0;
-int fanSpeed = 0;
-
 #ifdef SERVO_ENDSTOPS
   int servo_endstops[] = SERVO_ENDSTOPS;
   int servo_endstop_angles[] = SERVO_ENDSTOP_ANGLES;
@@ -283,9 +311,10 @@ int fanSpeed = 0;
 #endif
 
 #ifdef DELTA
-  float delta[3] = { 0, 0, 0 };
+  float delta[3] = { 0 };
   #define SIN_60 0.8660254037844386
   #define COS_60 0.5
+  float endstop_adj[3] = { 0 };
   // these are the default values, can be overriden with M665
   float delta_radius = DELTA_RADIUS;
   float delta_tower1_x = -SIN_60 * delta_radius; // front left tower
@@ -298,17 +327,18 @@ int fanSpeed = 0;
   float delta_diagonal_rod_2 = sq(delta_diagonal_rod);
   float delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND;
   #ifdef ENABLE_AUTO_BED_LEVELING
+    int delta_grid_spacing[2] = { 0, 0 };
     float bed_level[AUTO_BED_LEVELING_GRID_POINTS][AUTO_BED_LEVELING_GRID_POINTS];
   #endif
+#else
+  static bool home_all_axis = true;
 #endif
 
 #ifdef SCARA
+  static float delta[3] = { 0 };
   float axis_scaling[3] = { 1, 1, 1 };    // Build size scaling, default to 1
-  static float delta[3] = { 0, 0, 0 };		
 #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
@@ -325,67 +355,21 @@ bool cancel_heatup = false;
    static bool filrunoutEnqued = false;
 #endif
 
-const char errormagic[] PROGMEM = "Error:";
-const char echomagic[] PROGMEM = "echo:";
-
-const char axis_codes[NUM_AXIS] = {'X', 'Y', 'Z', 'E'};
-static float destination[NUM_AXIS] = { 0 };
-
-static float offset[3] = { 0 };
-
-#ifndef DELTA
-  static bool home_all_axis = true;
-#endif
-
-static float feedrate = 1500.0, next_feedrate, saved_feedrate;
-static long gcode_N, gcode_LastN, Stopped_gcode_LastN = 0;
-
-static bool relative_mode = false;  //Determines Absolute or Relative Coordinates
-
-static char cmdbuffer[BUFSIZE][MAX_CMD_SIZE];
 #ifdef SDSUPPORT
   static bool fromsd[BUFSIZE];
 #endif
-static int bufindr = 0;
-static int bufindw = 0;
-static int buflen = 0;
-
-static char serial_char;
-static int serial_count = 0;
-static boolean comment_mode = false;
-static char *strchr_pointer; ///< A pointer to find chars in the command string (X, Y, Z, E, etc.)
-
-const char* queued_commands_P= NULL; /* pointer to the current line in the active sequence of commands, or NULL when none */
-
-const int sensitive_pins[] = SENSITIVE_PINS; ///< Sensitive pin list for M42
-
-// Inactivity shutdown
-static unsigned long previous_millis_cmd = 0;
-static unsigned long max_inactive_time = 0;
-static unsigned long stepper_inactive_time = DEFAULT_STEPPER_DEACTIVE_TIME*1000l;
-
-unsigned long starttime = 0; ///< Print job start time
-unsigned long stoptime = 0;  ///< Print job stop time
-
-static uint8_t tmp_extruder;
-
-
-bool Stopped = false;
 
 #if NUM_SERVOS > 0
   Servo servos[NUM_SERVOS];
 #endif
 
-bool CooldownNoWait = true;
-bool target_direction;
-
 #ifdef CHDK
   unsigned long chdkHigh = 0;
   boolean chdkActive = false;
 #endif
 
 //===========================================================================
-//=============================Routines======================================
+//================================ Functions ================================
 //===========================================================================
 
 void get_arc_coordinates();
@@ -422,26 +406,24 @@ void serial_echopair_P(const char *s_P, unsigned long v)
 
 //Injects the next command from the pending sequence of commands, when possible
 //Return false if and only if no command was pending
-static bool drain_queued_commands_P()
-{
-  char cmd[30];
-  if(!queued_commands_P)
-    return false;
+static bool drain_queued_commands_P() {
+  if (!queued_commands_P) return false;
+
   // Get the next 30 chars from the sequence of gcodes to run
-  strncpy_P(cmd, queued_commands_P, sizeof(cmd)-1);
-  cmd[sizeof(cmd)-1]= 0;
+  char cmd[30];
+  strncpy_P(cmd, queued_commands_P, sizeof(cmd) - 1);
+  cmd[sizeof(cmd) - 1] = '\0';
+
   // Look for the end of line, or the end of sequence
-  size_t i= 0;
+  size_t i = 0;
   char c;
-  while( (c= cmd[i]) && c!='\n' )
-    ++i; // look for the end of this gcode command
-  cmd[i]= 0;
-  if(enquecommand(cmd)) // buffer was not full (else we will retry later)
-  {
-    if(c)
-      queued_commands_P+= i+1; // move to next command
+  while((c = cmd[i]) && c != '\n') i++; // find the end of this gcode command
+  cmd[i] = '\0';
+  if (enquecommand(cmd)) {        // buffer was not full (else we will retry later)
+    if (c)
+      queued_commands_P += i + 1; // move to next command
     else
-      queued_commands_P= NULL; // will have no more commands in the sequence
+      queued_commands_P = NULL;   // will have no more commands in the sequence
   }
   return true;
 }
@@ -449,10 +431,9 @@ static bool drain_queued_commands_P()
 //Record one or many commands to run from program memory.
 //Aborts the current queue, if any.
 //Note: drain_queued_commands_P() must be called repeatedly to drain the commands afterwards
-void enquecommands_P(const char* pgcode)
-{
-    queued_commands_P= pgcode;
-    drain_queued_commands_P(); // first command exectuted asap (when possible)
+void enquecommands_P(const char* pgcode) {
+    queued_commands_P = pgcode;
+    drain_queued_commands_P(); // first command executed asap (when possible)
 }
 
 //adds a single command to the main command buffer, from RAM
@@ -478,42 +459,42 @@ bool enquecommand(const char *cmd)
 
 void setup_killpin()
 {
-  #if defined(KILL_PIN) && KILL_PIN > -1
+  #if HAS_KILL
     SET_INPUT(KILL_PIN);
-    WRITE(KILL_PIN,HIGH);
+    WRITE(KILL_PIN, HIGH);
   #endif
 }
 
 void setup_filrunoutpin()
 {
-#if defined(FILRUNOUT_PIN) && FILRUNOUT_PIN > -1
-   pinMode(FILRUNOUT_PIN,INPUT);
-   #if defined(ENDSTOPPULLUP_FIL_RUNOUT)
-      WRITE(FILLRUNOUT_PIN,HIGH);
-   #endif
-#endif
+  #if HAS_FILRUNOUT
+    pinMode(FILRUNOUT_PIN, INPUT);
+    #ifdef ENDSTOPPULLUP_FIL_RUNOUT
+      WRITE(FILLRUNOUT_PIN, HIGH);
+    #endif
+  #endif
 }
 
 // Set home pin
 void setup_homepin(void)
 {
-#if defined(HOME_PIN) && HOME_PIN > -1
-   SET_INPUT(HOME_PIN);
-   WRITE(HOME_PIN,HIGH);
-#endif
+  #if HAS_HOME
+    SET_INPUT(HOME_PIN);
+    WRITE(HOME_PIN, HIGH);
+  #endif
 }
 
 
 void setup_photpin()
 {
-  #if defined(PHOTOGRAPH_PIN) && PHOTOGRAPH_PIN > -1
+  #if HAS_PHOTOGRAPH
     OUT_WRITE(PHOTOGRAPH_PIN, LOW);
   #endif
 }
 
 void setup_powerhold()
 {
-  #if defined(SUICIDE_PIN) && SUICIDE_PIN > -1
+  #if HAS_SUICIDE
     OUT_WRITE(SUICIDE_PIN, HIGH);
   #endif
   #if HAS_POWER_SWITCH
@@ -527,37 +508,31 @@ void setup_powerhold()
 
 void suicide()
 {
-  #if defined(SUICIDE_PIN) && SUICIDE_PIN > -1
+  #if HAS_SUICIDE
     OUT_WRITE(SUICIDE_PIN, LOW);
   #endif
 }
 
 void servo_init()
 {
-  #if (NUM_SERVOS >= 1) && defined(SERVO0_PIN) && (SERVO0_PIN > -1)
+  #if NUM_SERVOS >= 1 && HAS_SERVO_0
     servos[0].attach(SERVO0_PIN);
   #endif
-  #if (NUM_SERVOS >= 2) && defined(SERVO1_PIN) && (SERVO1_PIN > -1)
+  #if NUM_SERVOS >= 2 && HAS_SERVO_1
     servos[1].attach(SERVO1_PIN);
   #endif
-  #if (NUM_SERVOS >= 3) && defined(SERVO2_PIN) && (SERVO2_PIN > -1)
+  #if NUM_SERVOS >= 3 && HAS_SERVO_2
     servos[2].attach(SERVO2_PIN);
   #endif
-  #if (NUM_SERVOS >= 4) && defined(SERVO3_PIN) && (SERVO3_PIN > -1)
+  #if NUM_SERVOS >= 4 && HAS_SERVO_3
     servos[3].attach(SERVO3_PIN);
   #endif
-  #if (NUM_SERVOS >= 5)
-    #error "TODO: enter initalisation code for more servos"
-  #endif
 
   // Set position of Servo Endstops that are defined
   #ifdef SERVO_ENDSTOPS
-  for(int8_t i = 0; i < 3; i++)
-  {
-    if(servo_endstops[i] > -1) {
+  for (int i = 0; i < 3; i++)
+    if (servo_endstops[i] >= 0)
       servos[servo_endstops[i]].write(servo_endstop_angles[i * 2 + 1]);
-    }
-  }
   #endif
 
   #if SERVO_LEVELING
@@ -624,7 +599,7 @@ void setup()
   lcd_init();
   _delay_ms(1000);  // wait 1sec to display the splash screen
 
-  #if defined(CONTROLLERFAN_PIN) && CONTROLLERFAN_PIN > -1
+  #if HAS_CONTROLLERFAN
     SET_OUTPUT(CONTROLLERFAN_PIN); //Set pin used for driver cooling fan
   #endif
 
@@ -648,47 +623,37 @@ void setup()
 }
 
 
-void loop()
-{
-  if(buflen < (BUFSIZE-1))
-    get_command();
+void loop() {
+  if (buflen < BUFSIZE - 1) get_command();
+
   #ifdef SDSUPPORT
-  card.checkautostart(false);
+    card.checkautostart(false);
   #endif
-  if(buflen)
-  {
+
+  if (buflen) {
     #ifdef SDSUPPORT
-      if(card.saving)
-      {
-        if(strstr_P(cmdbuffer[bufindr], PSTR("M29")) == NULL)
-        {
+      if (card.saving) {
+        if (strstr_P(cmdbuffer[bufindr], PSTR("M29")) == NULL) {
           card.write_command(cmdbuffer[bufindr]);
-          if(card.logging)
-          {
+          if (card.logging)
             process_commands();
-          }
           else
-          {
             SERIAL_PROTOCOLLNPGM(MSG_OK);
-          }
         }
-        else
-        {
+        else {
           card.closefile();
           SERIAL_PROTOCOLLNPGM(MSG_FILE_SAVED);
         }
       }
       else
-      {
         process_commands();
-      }
     #else
       process_commands();
-    #endif //SDSUPPORT
-    buflen = (buflen-1);
-    bufindr = (bufindr + 1)%BUFSIZE;
+    #endif // SDSUPPORT
+    buflen--;
+    bufindr = (bufindr + 1) % BUFSIZE;
   }
-  //check heater every n milliseconds
+  // Check heater every n milliseconds
   manage_heater();
   manage_inactivity();
   checkHitEndstops();
@@ -697,7 +662,7 @@ void loop()
 
 void get_command()
 {
-  if(drain_queued_commands_P()) // priority is given to non-serial commands
+  if (drain_queued_commands_P()) // priority is given to non-serial commands
     return;
   
   while( MYSERIAL.available() > 0  && buflen < BUFSIZE) {
@@ -892,7 +857,9 @@ float code_value() {
   return ret;
 }
 
-long code_value_long() { return (strtol(strchr_pointer + 1, NULL, 10)); }
+long code_value_long() { return strtol(strchr_pointer + 1, NULL, 10); }
+
+int16_t code_value_short() { return (int16_t)strtol(strchr_pointer + 1, NULL, 10); }
 
 bool code_seen(char code) {
   strchr_pointer = strchr(cmdbuffer[bufindr], code);
@@ -916,7 +883,7 @@ XYZ_CONSTS_FROM_CONFIG(float, base_min_pos,    MIN_POS);
 XYZ_CONSTS_FROM_CONFIG(float, base_max_pos,    MAX_POS);
 XYZ_CONSTS_FROM_CONFIG(float, base_home_pos,   HOME_POS);
 XYZ_CONSTS_FROM_CONFIG(float, max_length,      MAX_LENGTH);
-XYZ_CONSTS_FROM_CONFIG(float, home_retract_mm, HOME_RETRACT_MM);
+XYZ_CONSTS_FROM_CONFIG(float, home_bump_mm, HOME_BUMP_MM);
 XYZ_CONSTS_FROM_CONFIG(signed char, home_dir,  HOME_DIR);
 
 #ifdef DUAL_X_CARRIAGE
@@ -1040,9 +1007,23 @@ inline void sync_plan_position() {
     plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
   }
 #endif
+inline void set_current_to_destination() { memcpy(current_position, destination, sizeof(current_position)); }
+inline void set_destination_to_current() { memcpy(destination, current_position, sizeof(destination)); }
 
 #ifdef ENABLE_AUTO_BED_LEVELING
 
+  #ifdef DELTA
+    /**
+     * Calculate delta, start a line, and set current_position to destination
+     */
+    void prepare_move_raw() {
+      refresh_cmd_timeout();
+      calculate_delta(destination);
+      plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], (feedrate/60)*(feedmultiply/100.0), active_extruder);
+      set_current_to_destination();
+    }
+  #endif
+
   #ifdef AUTO_BED_LEVELING_GRID
 
     #ifndef DELTA
@@ -1132,7 +1113,7 @@ inline void sync_plan_position() {
       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);
+      zPosition += home_bump_mm(Z_AXIS);
       line_to_z(zPosition);
       st_synchronize();
       endstops_hit_on_purpose(); // clear endstop hit flags
@@ -1145,7 +1126,7 @@ inline void sync_plan_position() {
         SERIAL_ECHOLN("Warning: The Homing Bump Feedrate Divisor cannot be less than 1");
       }
 
-      zPosition -= home_retract_mm(Z_AXIS) * 2;
+      zPosition -= home_bump_mm(Z_AXIS) * 2;
       line_to_z(zPosition);
       st_synchronize();
       endstops_hit_on_purpose(); // clear endstop hit flags
@@ -1157,6 +1138,9 @@ inline void sync_plan_position() {
     #endif // !DELTA
   }
 
+  /**
+   * 
+   */
   static void do_blocking_move_to(float x, float y, float z) {
     float oldFeedRate = feedrate;
 
@@ -1194,7 +1178,7 @@ inline void sync_plan_position() {
     saved_feedrate = feedrate;
     saved_feedmultiply = feedmultiply;
     feedmultiply = 100;
-    previous_millis_cmd = millis();
+    refresh_cmd_timeout();
     enable_endstops(true);
   }
 
@@ -1204,10 +1188,10 @@ inline void sync_plan_position() {
     #endif
     feedrate = saved_feedrate;
     feedmultiply = saved_feedmultiply;
-    previous_millis_cmd = millis();
+    refresh_cmd_timeout();
   }
 
-  static void engage_z_probe() {
+  static void deploy_z_probe() {
 
     #ifdef SERVO_ENDSTOPS
 
@@ -1244,9 +1228,15 @@ inline void sync_plan_position() {
       prepare_move_raw();
       
       st_synchronize();
-      
+
+    #ifdef Z_PROBE_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) {
           SERIAL_ERROR_START;
           SERIAL_ERRORLNPGM("Z-Probe failed to engage!");
@@ -1259,7 +1249,7 @@ inline void sync_plan_position() {
 
   }
 
-  static void retract_z_probe() {
+  static void stow_z_probe() {
 
     #ifdef SERVO_ENDSTOPS
 
@@ -1291,19 +1281,19 @@ inline void sync_plan_position() {
       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;
+      destination[X_AXIS] = Z_PROBE_ALLEN_KEY_STOW_X;
+      destination[Y_AXIS] = Z_PROBE_ALLEN_KEY_STOW_Y;
+      destination[Z_AXIS] = Z_PROBE_ALLEN_KEY_STOW_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;
+      destination[Z_AXIS] = current_position[Z_AXIS] - Z_PROBE_ALLEN_KEY_STOW_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;
+      destination[Z_AXIS] = current_position[Z_AXIS] + Z_PROBE_ALLEN_KEY_STOW_DEPTH * 2;
       prepare_move_raw();
       
       // Home XY for safety
@@ -1313,9 +1303,15 @@ inline void sync_plan_position() {
       prepare_move_raw();
       
       st_synchronize();
-      
+
+    #ifdef Z_PROBE_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) {
           SERIAL_ERROR_START;
           SERIAL_ERRORLNPGM("Z-Probe failed to retract!");
@@ -1329,20 +1325,20 @@ inline void sync_plan_position() {
   }
 
   enum ProbeAction {
-    ProbeStay             = 0,
-    ProbeEngage           = BIT(0),
-    ProbeRetract          = BIT(1),
-    ProbeEngageAndRetract = (ProbeEngage | ProbeRetract)
+    ProbeStay          = 0,
+    ProbeDeploy        = BIT(0),
+    ProbeStow          = BIT(1),
+    ProbeDeployAndStow = (ProbeDeploy | ProbeStow)
   };
 
   // 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) {
+  static float probe_pt(float x, float y, float z_before, ProbeAction retract_action=ProbeDeployAndStow, 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();
+      if (retract_action & ProbeDeploy) deploy_z_probe();
     #endif
 
     run_z_probe();
@@ -1356,7 +1352,7 @@ inline void sync_plan_position() {
     #endif
 
     #if !defined(Z_PROBE_SLED) && !defined(Z_PROBE_ALLEN_KEY)
-      if (retract_action & ProbeRetract) retract_z_probe();
+      if (retract_action & ProbeStow) stow_z_probe();
     #endif
 
     if (verbose_level > 2) {
@@ -1416,9 +1412,9 @@ inline void sync_plan_position() {
       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_PROTOCOLCHAR(' ');
         }
-        SERIAL_ECHOLN("");
+        SERIAL_EOL;
       }
     }
 
@@ -1460,10 +1456,10 @@ static void homeaxis(int axis) {
     sync_plan_position();
 
     // Engage Servo endstop if enabled
-    #ifdef SERVO_ENDSTOPS && !defined(Z_PROBE_SLED)
+    #if defined(SERVO_ENDSTOPS) && !defined(Z_PROBE_SLED)
 
       #if SERVO_LEVELING
-        if (axis == Z_AXIS) engage_z_probe(); else
+        if (axis == Z_AXIS) deploy_z_probe(); else
       #endif
         {
           if (servo_endstops[axis] > -1)
@@ -1486,8 +1482,8 @@ static void homeaxis(int axis) {
     current_position[axis] = 0;
     sync_plan_position();
 
-    // Move away from the endstop by the axis HOME_RETRACT_MM
-    destination[axis] = -home_retract_mm(axis) * axis_home_dir;
+    // Move away from the endstop by the axis HOME_BUMP_MM
+    destination[axis] = -home_bump_mm(axis) * axis_home_dir;
     line_to_destination();
     st_synchronize();
 
@@ -1500,7 +1496,7 @@ static void homeaxis(int axis) {
     }
 
     // Move slowly towards the endstop until triggered
-    destination[axis] = 2 * home_retract_mm(axis) * axis_home_dir;
+    destination[axis] = 2 * home_bump_mm(axis) * axis_home_dir;
     line_to_destination();
     st_synchronize();
 
@@ -1554,14 +1550,12 @@ static void homeaxis(int axis) {
     #endif
 
     #if SERVO_LEVELING && !defined(Z_PROBE_SLED)
-      if (axis == Z_AXIS) retract_z_probe();
+      if (axis == Z_AXIS) stow_z_probe();
     #endif
 
   }
 }
 
-void refresh_cmd_timeout(void) { previous_millis_cmd = millis(); }
-
 #ifdef FWRETRACT
 
   void retract(bool retracting, bool swapretract = false) {
@@ -1570,7 +1564,7 @@ void refresh_cmd_timeout(void) { previous_millis_cmd = millis(); }
 
     float oldFeedrate = feedrate;
 
-    for (int i = 0; i < NUM_AXIS; i++) destination[i] = current_position[i];
+    set_destination_to_current();
 
     if (retracting) {
 
@@ -1592,7 +1586,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
@@ -1693,7 +1687,7 @@ inline void gcode_G2_G3(bool clockwise) {
  * G4: Dwell S<seconds> or P<milliseconds>
  */
 inline void gcode_G4() {
-  unsigned long codenum=0;
+  unsigned long codenum = 0;
 
   LCD_MESSAGEPGM(MSG_DWELL);
 
@@ -1701,9 +1695,9 @@ inline void gcode_G4() {
   if (code_seen('S')) codenum = code_value_long() * 1000; // seconds to wait
 
   st_synchronize();
-  previous_millis_cmd = millis();
+  refresh_cmd_timeout();
   codenum += previous_millis_cmd;  // keep track of when we started waiting
-  while(millis() < codenum) {
+  while (millis() < codenum) {
     manage_heater();
     manage_inactivity();
     lcd_update();
@@ -1719,7 +1713,7 @@ inline void gcode_G4() {
   inline void gcode_G10_G11(bool doRetract=false) {
     #if EXTRUDERS > 1
       if (doRetract) {
-        retracted_swap[active_extruder] = (code_seen('S') && code_value_long() == 1); // checks for swap retract argument
+        retracted_swap[active_extruder] = (code_seen('S') && code_value_short() == 1); // checks for swap retract argument
       }
     #endif
     retract(doRetract
@@ -1753,14 +1747,17 @@ inline void gcode_G4() {
  *  Zn  Home Z, setting Z to n + home_offset[Z_AXIS]
  */
 inline void gcode_G28() {
+
+  // For auto bed leveling, clear the level matrix
   #ifdef ENABLE_AUTO_BED_LEVELING
-    plan_bed_level_matrix.set_to_identity();  //Reset the plane ("erase" all leveling data)
+    plan_bed_level_matrix.set_to_identity();
     #ifdef DELTA
       reset_bed_level();
     #endif
   #endif
 
-  #if defined(MESH_BED_LEVELING)
+  // For manual bed leveling deactivate the matrix temporarily
+  #ifdef MESH_BED_LEVELING
     uint8_t mbl_was_active = mbl.active;
     mbl.active = 0;
   #endif
@@ -1768,11 +1765,11 @@ inline void gcode_G28() {
   saved_feedrate = feedrate;
   saved_feedmultiply = feedmultiply;
   feedmultiply = 100;
-  previous_millis_cmd = millis();
+  refresh_cmd_timeout();
 
   enable_endstops(true);
 
-  for (int i = 0; i < NUM_AXIS; i++) destination[i] = current_position[i]; // includes E_AXIS
+  set_destination_to_current();
 
   feedrate = 0.0;
 
@@ -1780,10 +1777,11 @@ inline void gcode_G28() {
     // A delta can only safely home all axis at the same time
     // all axis have to home at the same time
 
-    // Move all carriages up together until the first endstop is hit.
+    // Pretend the current position is 0,0,0
     for (int i = X_AXIS; i <= Z_AXIS; i++) current_position[i] = 0;
     sync_plan_position();
 
+    // Move all carriages up together until the first endstop is hit.
     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();
@@ -1817,7 +1815,7 @@ inline void gcode_G28() {
       // Raise Z before homing any other axes
       if (home_all_axis || homeZ) {
         destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS);    // Set destination away from bed
-        feedrate = max_feedrate[Z_AXIS];
+        feedrate = max_feedrate[Z_AXIS] * 60;
         line_to_destination();
         st_synchronize();
       }
@@ -1950,7 +1948,7 @@ inline void gcode_G28() {
               current_position[Z_AXIS] = 0;
               plan_set_position(cpx, cpy, 0, 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];
+              feedrate = max_feedrate[Z_AXIS] * 60;  // max_feedrate is in mm/s. line_to_destination is feedrate/60.
               line_to_destination();
               st_synchronize();
               HOMEAXIS(Z);
@@ -1994,14 +1992,12 @@ inline void gcode_G28() {
     enable_endstops(false);
   #endif
 
-  #if defined(MESH_BED_LEVELING)
+  // For manual leveling move back to 0,0
+  #ifdef MESH_BED_LEVELING
     if (mbl_was_active) {
       current_position[X_AXIS] = mbl.get_x(0);
       current_position[Y_AXIS] = mbl.get_y(0);
-      destination[X_AXIS] = current_position[X_AXIS];
-      destination[Y_AXIS] = current_position[Y_AXIS];
-      destination[Z_AXIS] = current_position[Z_AXIS];
-      destination[E_AXIS] = current_position[E_AXIS];
+      set_destination_to_current();
       feedrate = homing_feedrate[X_AXIS];
       line_to_destination();
       st_synchronize();
@@ -2013,25 +2009,14 @@ inline void gcode_G28() {
 
   feedrate = saved_feedrate;
   feedmultiply = saved_feedmultiply;
-  previous_millis_cmd = millis();
+  refresh_cmd_timeout();
   endstops_hit_on_purpose(); // clear endstop hit flags
 }
 
-#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
 
+  enum MeshLevelingState { MeshReport, MeshStart, MeshNext, MeshSet };
+
   /**
    * G29: Mesh-based Z-Probe, probes a grid and produces a
    *      mesh to compensate for variable bed height
@@ -2053,111 +2038,114 @@ 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')) {
-      state = code_value_long();
-      if (state < 0 || state > 3) {
-        SERIAL_PROTOCOLPGM("S out of range (0-3).\n");
-        return;
-      }
+    MeshLevelingState state = code_seen('S') || code_seen('s') ? (MeshLevelingState)code_value_short() : MeshReport;
+    if (state < 0 || state > 3) {
+      SERIAL_PROTOCOLLNPGM("S out of range (0-3).");
+      return;
     }
 
-    if (state == 0) { // Produce a mesh report
-      if (mbl.active) {
-        SERIAL_PROTOCOLPGM("Num X,Y: ");
-        SERIAL_PROTOCOL(MESH_NUM_X_POINTS);
-        SERIAL_PROTOCOLPGM(",");
-        SERIAL_PROTOCOL(MESH_NUM_Y_POINTS);
-        SERIAL_PROTOCOLPGM("\nZ search height: ");
-        SERIAL_PROTOCOL(MESH_HOME_SEARCH_Z);
-        SERIAL_PROTOCOLPGM("\nMeasured points:\n");              
-        for (int y=0; y<MESH_NUM_Y_POINTS; y++) {
-          for (int x=0; x<MESH_NUM_X_POINTS; x++) {
-            SERIAL_PROTOCOLPGM("  ");              
-            SERIAL_PROTOCOL_F(mbl.z_values[y][x], 5);
+    switch(state) {
+      case MeshReport:
+        if (mbl.active) {
+          SERIAL_PROTOCOLPGM("Num X,Y: ");
+          SERIAL_PROTOCOL(MESH_NUM_X_POINTS);
+          SERIAL_PROTOCOLCHAR(',');
+          SERIAL_PROTOCOL(MESH_NUM_Y_POINTS);
+          SERIAL_PROTOCOLPGM("\nZ search height: ");
+          SERIAL_PROTOCOL(MESH_HOME_SEARCH_Z);
+          SERIAL_PROTOCOLLNPGM("\nMeasured points:");
+          for (int y = 0; y < MESH_NUM_Y_POINTS; y++) {
+            for (int x = 0; x < MESH_NUM_X_POINTS; x++) {
+              SERIAL_PROTOCOLPGM("  ");
+              SERIAL_PROTOCOL_F(mbl.z_values[y][x], 5);
+            }
+            SERIAL_EOL;
           }
-          SERIAL_EOL;
         }
-      } else {
-        SERIAL_PROTOCOLPGM("Mesh bed leveling not active.\n");
-      }
+        else
+          SERIAL_PROTOCOLLNPGM("Mesh bed leveling not active.");
+        break;
 
-    } else if (state == 1) { // Start probing mesh points
+      case MeshStart:
+        mbl.reset();
+        probe_point = 0;
+        enquecommands_P(PSTR("G28\nG29 S2"));
+        break;
 
-      mbl.reset();
-      probe_point = 0;
-      enquecommands_P(PSTR("G28"));
-      enquecommands_P(PSTR("G29 S2"));
-
-    } else if (state == 2) { // Probe the next mesh point
-
-      if (probe_point < 0) {
-        SERIAL_PROTOCOLPGM("Start mesh probing with \"G29 S1\" first.\n");
-        return;
-      }
-      int ix, iy;
-      if (probe_point == 0) {
-        current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
-        sync_plan_position();
-      } else {
-        ix = (probe_point-1) % MESH_NUM_X_POINTS;
-        iy = (probe_point-1) / MESH_NUM_X_POINTS;
-        if (iy & 1) ix = (MESH_NUM_X_POINTS - 1) - ix; // zig-zag
-        mbl.set_z(ix, iy, current_position[Z_AXIS]);
-        current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
-        plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[X_AXIS]/60, active_extruder);
-        st_synchronize();
-      }
-      if (probe_point == MESH_NUM_X_POINTS * MESH_NUM_Y_POINTS) {
-        SERIAL_PROTOCOLPGM("Mesh probing done.\n");
-        probe_point = -1;
-        mbl.active = 1;
-        enquecommands_P(PSTR("G28"));
-        return;
-      }
-      ix = probe_point % MESH_NUM_X_POINTS;
-      iy = probe_point / MESH_NUM_X_POINTS;
-      if (iy & 1) ix = (MESH_NUM_X_POINTS - 1) - ix; // zig-zag
-      current_position[X_AXIS] = mbl.get_x(ix);
-      current_position[Y_AXIS] = mbl.get_y(iy);
-      plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[X_AXIS]/60, active_extruder);
-      st_synchronize();
-      probe_point++;
-    } else if (state == 3) { // Manually modify a single point
-      int ix, iy;
-      float z;
-      if (code_seen('X') || code_seen('x')) {
-        ix = code_value_long()-1;
-        if (ix < 0 || ix >= MESH_NUM_X_POINTS) {
-          SERIAL_PROTOCOLPGM("X out of range (1-" STRINGIFY(MESH_NUM_X_POINTS) ").\n");
+      case MeshNext:
+        if (probe_point < 0) {
+          SERIAL_PROTOCOLLNPGM("Start mesh probing with \"G29 S1\" first.");
           return;
         }
-      } else {
-          SERIAL_PROTOCOLPGM("X not entered.\n");
-          return;
-      }
-      if (code_seen('Y') || code_seen('y')) {
-        iy = code_value_long()-1;
-        if (iy < 0 || iy >= MESH_NUM_Y_POINTS) {
-          SERIAL_PROTOCOLPGM("Y out of range (1-" STRINGIFY(MESH_NUM_Y_POINTS) ").\n");
-          return;
+        int ix, iy;
+        if (probe_point == 0) {
+          // Set Z to a positive value before recording the first Z.
+          current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
+          sync_plan_position();
         }
-      } else {
-          SERIAL_PROTOCOLPGM("Y not entered.\n");
-          return;
-      }
-      if (code_seen('Z') || code_seen('z')) {
-        z = code_value();
-      } else {
+        else {
+          // For others, save the Z of the previous point, then raise Z again.
+          ix = (probe_point - 1) % MESH_NUM_X_POINTS;
+          iy = (probe_point - 1) / MESH_NUM_X_POINTS;
+          if (iy & 1) ix = (MESH_NUM_X_POINTS - 1) - ix; // zig-zag
+          mbl.set_z(ix, iy, current_position[Z_AXIS]);
+          current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
+          plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[X_AXIS]/60, active_extruder);
+          st_synchronize();
+        }
+        // Is there another point to sample? Move there.
+        if (probe_point < MESH_NUM_X_POINTS * MESH_NUM_Y_POINTS) {
+          ix = probe_point % MESH_NUM_X_POINTS;
+          iy = probe_point / MESH_NUM_X_POINTS;
+          if (iy & 1) ix = (MESH_NUM_X_POINTS - 1) - ix; // zig-zag
+          current_position[X_AXIS] = mbl.get_x(ix);
+          current_position[Y_AXIS] = mbl.get_y(iy);
+          plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[X_AXIS]/60, active_extruder);
+          st_synchronize();
+          probe_point++;
+        }
+        else {
+          // After recording the last point, activate the mbl and home
+          SERIAL_PROTOCOLLNPGM("Mesh probing done.");
+          probe_point = -1;
+          mbl.active = 1;
+          enquecommands_P(PSTR("G28"));
+        }
+        break;
+
+      case MeshSet:
+        int ix, iy;
+        float z;
+        if (code_seen('X') || code_seen('x')) {
+          ix = code_value_long()-1;
+          if (ix < 0 || ix >= MESH_NUM_X_POINTS) {
+            SERIAL_PROTOCOLPGM("X out of range (1-" STRINGIFY(MESH_NUM_X_POINTS) ").\n");
+            return;
+          }
+        } else {
+            SERIAL_PROTOCOLPGM("X not entered.\n");
+            return;
+        }
+        if (code_seen('Y') || code_seen('y')) {
+          iy = code_value_long()-1;
+          if (iy < 0 || iy >= MESH_NUM_Y_POINTS) {
+            SERIAL_PROTOCOLPGM("Y out of range (1-" STRINGIFY(MESH_NUM_Y_POINTS) ").\n");
+            return;
+          }
+        } else {
+            SERIAL_PROTOCOLPGM("Y not entered.\n");
+            return;
+        }
+        if (code_seen('Z') || code_seen('z')) {
+          z = code_value();
+        } else {
           SERIAL_PROTOCOLPGM("Z not entered.\n");
           return;
-      }
-      mbl.z_values[iy][ix] = z;
-    }
+        }
+        mbl.z_values[iy][ix] = z;
+
+    } // switch(state)
   }
 
 #elif defined(ENABLE_AUTO_BED_LEVELING)
@@ -2202,21 +2190,22 @@ inline void gcode_G28() {
    */
   inline void gcode_G29() {
 
-    // Prevent leveling without first homing in X and Y
-    if (!can_run_bed_leveling()) return;
-
-    int verbose_level = 1;
-
-    if (code_seen('V') || code_seen('v')) {
-      verbose_level = code_value_long();
-      if (verbose_level < 0 || verbose_level > 4) {
-        SERIAL_PROTOCOLPGM("?(V)erbose Level is implausible (0-4).\n");
-        return;
-      }
+    // Don't allow auto-leveling without homing first
+    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;
     }
 
-    bool dryrun = code_seen('D') || code_seen('d');
-    bool engage_probe_for_each_reading = code_seen('E') || code_seen('e');
+    int verbose_level = code_seen('V') || code_seen('v') ? code_value_short() : 1;
+    if (verbose_level < 0 || verbose_level > 4) {
+      SERIAL_ECHOLNPGM("?(V)erbose Level is implausible (0-4).");
+      return;
+    }
+
+    bool dryrun = code_seen('D') || code_seen('d'),
+         deploy_probe_for_each_reading = code_seen('E') || code_seen('e');
 
     #ifdef AUTO_BED_LEVELING_GRID
 
@@ -2226,24 +2215,24 @@ inline void gcode_G28() {
 
       if (verbose_level > 0) {
         SERIAL_PROTOCOLPGM("G29 Auto Bed Leveling\n");
-        if (dryrun) SERIAL_ECHOLN("Running in DRY-RUN mode");
+        if (dryrun) SERIAL_ECHOLNPGM("Running in DRY-RUN mode");
       }
 
       int auto_bed_leveling_grid_points = AUTO_BED_LEVELING_GRID_POINTS;
       #ifndef DELTA
-        if (code_seen('P')) auto_bed_leveling_grid_points = code_value_long();
+        if (code_seen('P')) auto_bed_leveling_grid_points = code_value_short();
         if (auto_bed_leveling_grid_points < 2) {
           SERIAL_PROTOCOLPGM("?Number of probed (P)oints is implausible (2 minimum).\n");
           return;
         }
       #endif
 
-      xy_travel_speed = code_seen('S') ? code_value_long() : XY_TRAVEL_SPEED;
+      xy_travel_speed = code_seen('S') ? code_value_short() : XY_TRAVEL_SPEED;
 
-      int left_probe_bed_position = code_seen('L') ? code_value_long() : LEFT_PROBE_BED_POSITION,
-          right_probe_bed_position = code_seen('R') ? code_value_long() : RIGHT_PROBE_BED_POSITION,
-          front_probe_bed_position = code_seen('F') ? code_value_long() : FRONT_PROBE_BED_POSITION,
-          back_probe_bed_position = code_seen('B') ? code_value_long() : BACK_PROBE_BED_POSITION;
+      int left_probe_bed_position = code_seen('L') ? code_value_short() : LEFT_PROBE_BED_POSITION,
+          right_probe_bed_position = code_seen('R') ? code_value_short() : RIGHT_PROBE_BED_POSITION,
+          front_probe_bed_position = code_seen('F') ? code_value_short() : FRONT_PROBE_BED_POSITION,
+          back_probe_bed_position = code_seen('B') ? code_value_short() : BACK_PROBE_BED_POSITION;
 
       bool left_out_l = left_probe_bed_position < MIN_PROBE_X,
            left_out = left_out_l || left_probe_bed_position > right_probe_bed_position - MIN_PROBE_EDGE,
@@ -2279,7 +2268,7 @@ inline void gcode_G28() {
     #ifdef Z_PROBE_SLED
       dock_sled(false); // engage (un-dock) the probe
     #elif defined(Z_PROBE_ALLEN_KEY) //|| defined(SERVO_LEVELING)
-      engage_z_probe();
+      deploy_z_probe();
     #endif
 
     st_synchronize();
@@ -2360,7 +2349,7 @@ inline void gcode_G28() {
 
           // raise extruder
           float measured_z,
-                z_before = Z_RAISE_BETWEEN_PROBINGS + (probePointCounter ? current_position[Z_AXIS] : 0);
+                z_before = probePointCounter ? Z_RAISE_BETWEEN_PROBINGS + current_position[Z_AXIS] : Z_RAISE_BEFORE_PROBING;
 
           #ifdef DELTA
             // Avoid probing the corners (outside the round or hexagon print surface) on a delta printer.
@@ -2368,14 +2357,13 @@ inline void gcode_G28() {
             if (distance_from_center > DELTA_PROBABLE_RADIUS) continue;
           #endif //DELTA
 
-          // Enhanced G29 - Do not retract servo between probes
           ProbeAction act;
-          if (engage_probe_for_each_reading)
-            act = ProbeEngageAndRetract;
-          else if (yProbe == front_probe_bed_position && xCount == 0)
-            act = ProbeEngage;
-          else if (yProbe == front_probe_bed_position + (yGridSpacing * (auto_bed_leveling_grid_points - 1)) && xCount == auto_bed_leveling_grid_points - 1)
-            act = ProbeRetract;
+          if (deploy_probe_for_each_reading) // G29 E - Stow between probes
+            act = ProbeDeployAndStow;
+          else if (yCount == 0 && xCount == 0)
+            act = ProbeDeploy;
+          else if (yCount == auto_bed_leveling_grid_points - 1 && xCount == auto_bed_leveling_grid_points - 1)
+            act = ProbeStow;
           else
             act = ProbeStay;
 
@@ -2447,7 +2435,7 @@ inline void gcode_G28() {
               if (diff >= 0.0)
                 SERIAL_PROTOCOLPGM(" +");   // Include + for column alignment
               else
-                SERIAL_PROTOCOLPGM(" ");
+                SERIAL_PROTOCOLCHAR(' ');
               SERIAL_PROTOCOL_F(diff, 5);
             } // xx
             SERIAL_EOL;
@@ -2466,10 +2454,10 @@ inline void gcode_G28() {
 
       // Actions for each probe
       ProbeAction p1, p2, p3;
-      if (engage_probe_for_each_reading)
-        p1 = p2 = p3 = ProbeEngageAndRetract;
+      if (deploy_probe_for_each_reading)
+        p1 = p2 = p3 = ProbeDeployAndStow;
       else
-        p1 = ProbeEngage, p2 = ProbeStay, p3 = ProbeRetract;
+        p1 = ProbeDeploy, p2 = ProbeStay, p3 = ProbeStow;
 
       // Probe at 3 arbitrary points
       float z_at_pt_1 = probe_pt(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, Z_RAISE_BEFORE_PROBING, p1, verbose_level),
@@ -2502,7 +2490,7 @@ inline void gcode_G28() {
     #ifdef Z_PROBE_SLED
       dock_sled(true, -SLED_DOCKING_OFFSET); // dock the probe, correcting for over-travel
     #elif defined(Z_PROBE_ALLEN_KEY) //|| defined(SERVO_LEVELING)
-      retract_z_probe();
+      stow_z_probe();
     #endif
 
     #ifdef Z_PROBE_END_SCRIPT
@@ -2514,7 +2502,7 @@ inline void gcode_G28() {
   #ifndef Z_PROBE_SLED
 
     inline void gcode_G30() {
-      engage_z_probe(); // Engage Z Servo endstop if available
+      deploy_z_probe(); // Engage Z Servo endstop if available
       st_synchronize();
       // TODO: make sure the bed_level_rotation_matrix is identity or the planner will get set incorectly
       setup_for_endstop_move();
@@ -2532,7 +2520,7 @@ inline void gcode_G28() {
       SERIAL_EOL;
 
       clean_up_after_endstop_move();
-      retract_z_probe(); // Retract Z Servo endstop if available
+      stow_z_probe(); // Retract Z Servo endstop if available
     }
 
   #endif //!Z_PROBE_SLED
@@ -2571,11 +2559,11 @@ inline void gcode_G92() {
     unsigned long codenum = 0;
     bool hasP = false, hasS = false;
     if (code_seen('P')) {
-      codenum = code_value(); // milliseconds to wait
+      codenum = code_value_short(); // milliseconds to wait
       hasP = codenum > 0;
     }
     if (code_seen('S')) {
-      codenum = code_value() * 1000; // seconds to wait
+      codenum = code_value_short() * 1000UL; // seconds to wait
       hasS = codenum > 0;
     }
     char* starpos = strchr(src, '*');
@@ -2592,7 +2580,7 @@ inline void gcode_G92() {
 
     lcd_ignore_click();
     st_synchronize();
-    previous_millis_cmd = millis();
+    refresh_cmd_timeout();
     if (codenum > 0) {
       codenum += previous_millis_cmd;  // keep track of when we started waiting
       while(millis() < codenum && !lcd_clicked()) {
@@ -2623,13 +2611,7 @@ inline void gcode_G92() {
  */
 inline void gcode_M17() {
   LCD_MESSAGEPGM(MSG_NO_MOVE);
-  enable_x();
-  enable_y();
-  enable_z();
-  enable_e0();
-  enable_e1();
-  enable_e2();
-  enable_e3();
+  enable_all_steppers();
 }
 
 #ifdef SDSUPPORT
@@ -2687,7 +2669,7 @@ inline void gcode_M17() {
    */
   inline void gcode_M26() {
     if (card.cardOK && code_seen('S'))
-      card.setIndex(code_value_long());
+      card.setIndex(code_value_short());
   }
 
   /**
@@ -2778,7 +2760,7 @@ inline void gcode_M31() {
       card.openFile(namestartpos, true, !call_procedure);
 
       if (code_seen('S') && strchr_pointer < namestartpos) // "S" (must occur _before_ the filename!)
-        card.setIndex(code_value_long());
+        card.setIndex(code_value_short());
 
       card.startFileprint();
       if (!call_procedure)
@@ -2806,11 +2788,11 @@ inline void gcode_M31() {
  */
 inline void gcode_M42() {
   if (code_seen('S')) {
-    int pin_status = code_value(),
+    int pin_status = code_value_short(),
         pin_number = LED_PIN;
 
     if (code_seen('P') && pin_status >= 0 && pin_status <= 255)
-      pin_number = code_value();
+      pin_number = code_value_short();
 
     for (int8_t i = 0; i < (int8_t)(sizeof(sensitive_pins) / sizeof(*sensitive_pins)); i++) {
       if (sensitive_pins[i] == pin_number) {
@@ -2819,7 +2801,7 @@ inline void gcode_M42() {
       }
     }
 
-    #if defined(FAN_PIN) && FAN_PIN > -1
+    #if HAS_FAN
       if (pin_number == FAN_PIN) fanSpeed = pin_status;
     #endif
 
@@ -2831,11 +2813,15 @@ inline void gcode_M42() {
   } // code_seen('S')
 }
 
-
 #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."
+  // This is redundant since the SanityCheck.h already checks for a valid Z_PROBE_PIN, but here for clarity.
+  #ifdef Z_PROBE_ENDSTOP
+    #if !HAS_Z_PROBE
+      #error You must define Z_PROBE_PIN to enable Z-Probe repeatability calculation.
+    #endif
+  #elif !HAS_Z_MIN
+    #error You must define Z_MIN_PIN to enable Z-Probe repeatability calculation.
   #endif
 
   /**
@@ -2865,7 +2851,7 @@ inline void gcode_M42() {
     int verbose_level = 1, n_samples = 10, n_legs = 0;
     
     if (code_seen('V') || code_seen('v')) {
-      verbose_level = code_value();
+      verbose_level = code_value_short();
       if (verbose_level < 0 || verbose_level > 4 ) {
         SERIAL_PROTOCOLPGM("?Verbose Level not plausible (0-4).\n");
         return;
@@ -2876,7 +2862,7 @@ inline void gcode_M42() {
       SERIAL_PROTOCOLPGM("M48 Z-Probe Repeatability test\n");
 
     if (code_seen('P') || code_seen('p') || code_seen('n')) { // `n` for legacy support only - please use `P`!
-      n_samples = code_value();
+      n_samples = code_value_short();
       if (n_samples < 4 || n_samples > 50) {
         SERIAL_PROTOCOLPGM("?Sample size not plausible (4-50).\n");
         return;
@@ -2890,7 +2876,7 @@ inline void gcode_M42() {
            Z_start_location = Z_current + Z_RAISE_BEFORE_PROBING,
            ext_position = st_get_position_mm(E_AXIS);
 
-    bool engage_probe_for_each_reading = code_seen('E') || code_seen('e');
+    bool deploy_probe_for_each_reading = code_seen('E') || code_seen('e');
 
     if (code_seen('X') || code_seen('x')) {
       X_probe_location = code_value() - X_PROBE_OFFSET_FROM_EXTRUDER;
@@ -2909,7 +2895,7 @@ inline void gcode_M42() {
     }
 
     if (code_seen('L') || code_seen('l')) {
-      n_legs = code_value();
+      n_legs = code_value_short();
       if (n_legs == 1) n_legs = 2;
       if (n_legs < 0 || n_legs > 15) {
         SERIAL_PROTOCOLPGM("?Number of legs in movement not plausible (0-15).\n");
@@ -2953,7 +2939,7 @@ inline void gcode_M42() {
     // Then retrace the right amount and use that in subsequent probes
     //
 
-    engage_z_probe();
+    deploy_z_probe();
 
     setup_for_endstop_move();
     run_z_probe();
@@ -2968,7 +2954,7 @@ inline void gcode_M42() {
     st_synchronize();
     current_position[Z_AXIS] = Z_current = st_get_position_mm(Z_AXIS);
 
-    if (engage_probe_for_each_reading) retract_z_probe();
+    if (deploy_probe_for_each_reading) stow_z_probe();
 
     for (uint16_t n=0; n < n_samples; n++) {
 
@@ -3010,8 +2996,8 @@ inline void gcode_M42() {
 
       } // n_legs
 
-      if (engage_probe_for_each_reading)  {
-        engage_z_probe(); 
+      if (deploy_probe_for_each_reading)  {
+        deploy_z_probe(); 
         delay(1000);
       }
 
@@ -3057,14 +3043,14 @@ inline void gcode_M42() {
       plan_buffer_line(X_probe_location, Y_probe_location, Z_start_location, current_position[E_AXIS], homing_feedrate[Z_AXIS]/60, active_extruder);
       st_synchronize();
 
-      if (engage_probe_for_each_reading) {
-        retract_z_probe();
+      if (deploy_probe_for_each_reading) {
+        stow_z_probe();
         delay(1000);
       }
     }
 
-    if (!engage_probe_for_each_reading) {
-      retract_z_probe();
+    if (!deploy_probe_for_each_reading) {
+      stow_z_probe();
       delay(1000);
     }
 
@@ -3091,12 +3077,15 @@ inline void gcode_M42() {
 inline void gcode_M104() {
   if (setTargetedHotend(104)) return;
 
-  if (code_seen('S')) setTargetHotend(code_value(), tmp_extruder);
-  #ifdef DUAL_X_CARRIAGE
-    if (dual_x_carriage_mode == DXC_DUPLICATION_MODE && tmp_extruder == 0)
-      setTargetHotend1(code_value() == 0.0 ? 0.0 : code_value() + duplicate_extruder_temp_offset);
-  #endif
-  setWatch();
+  if (code_seen('S')) {
+    float temp = code_value();
+    setTargetHotend(temp, target_extruder);
+    #ifdef DUAL_X_CARRIAGE
+      if (dual_x_carriage_mode == DXC_DUPLICATION_MODE && target_extruder == 0)
+        setTargetHotend1(temp == 0.0 ? 0.0 : temp + duplicate_extruder_temp_offset);
+    #endif
+    setWatch();
+  }
 }
 
 /**
@@ -3105,48 +3094,51 @@ inline void gcode_M104() {
 inline void gcode_M105() {
   if (setTargetedHotend(105)) return;
 
-  #if defined(TEMP_0_PIN) && TEMP_0_PIN > -1
-    SERIAL_PROTOCOLPGM("ok T:");
-    SERIAL_PROTOCOL_F(degHotend(tmp_extruder),1);
-    SERIAL_PROTOCOLPGM(" /");
-    SERIAL_PROTOCOL_F(degTargetHotend(tmp_extruder),1);
-    #if defined(TEMP_BED_PIN) && TEMP_BED_PIN > -1
+  #if HAS_TEMP_0 || HAS_TEMP_BED
+    SERIAL_PROTOCOLPGM("ok");
+    #if HAS_TEMP_0
+      SERIAL_PROTOCOLPGM(" T:");
+      SERIAL_PROTOCOL_F(degHotend(target_extruder), 1);
+      SERIAL_PROTOCOLPGM(" /");
+      SERIAL_PROTOCOL_F(degTargetHotend(target_extruder), 1);
+    #endif
+    #if HAS_TEMP_BED
       SERIAL_PROTOCOLPGM(" B:");
-      SERIAL_PROTOCOL_F(degBed(),1);
+      SERIAL_PROTOCOL_F(degBed(), 1);
       SERIAL_PROTOCOLPGM(" /");
-      SERIAL_PROTOCOL_F(degTargetBed(),1);
-    #endif //TEMP_BED_PIN
-    for (int8_t cur_extruder = 0; cur_extruder < EXTRUDERS; ++cur_extruder) {
+      SERIAL_PROTOCOL_F(degTargetBed(), 1);
+    #endif
+    for (int8_t e = 0; e < EXTRUDERS; ++e) {
       SERIAL_PROTOCOLPGM(" T");
-      SERIAL_PROTOCOL(cur_extruder);
-      SERIAL_PROTOCOLPGM(":");
-      SERIAL_PROTOCOL_F(degHotend(cur_extruder),1);
+      SERIAL_PROTOCOL(e);
+      SERIAL_PROTOCOLCHAR(':');
+      SERIAL_PROTOCOL_F(degHotend(e), 1);
       SERIAL_PROTOCOLPGM(" /");
-      SERIAL_PROTOCOL_F(degTargetHotend(cur_extruder),1);
+      SERIAL_PROTOCOL_F(degTargetHotend(e), 1);
     }
-  #else
+  #else // !HAS_TEMP_0 && !HAS_TEMP_BED
     SERIAL_ERROR_START;
     SERIAL_ERRORLNPGM(MSG_ERR_NO_THERMISTORS);
   #endif
 
   SERIAL_PROTOCOLPGM(" @:");
   #ifdef EXTRUDER_WATTS
-    SERIAL_PROTOCOL((EXTRUDER_WATTS * getHeaterPower(tmp_extruder))/127);
-    SERIAL_PROTOCOLPGM("W");
+    SERIAL_PROTOCOL((EXTRUDER_WATTS * getHeaterPower(target_extruder))/127);
+    SERIAL_PROTOCOLCHAR('W');
   #else
-    SERIAL_PROTOCOL(getHeaterPower(tmp_extruder));
+    SERIAL_PROTOCOL(getHeaterPower(target_extruder));
   #endif
 
   SERIAL_PROTOCOLPGM(" B@:");
   #ifdef BED_WATTS
     SERIAL_PROTOCOL((BED_WATTS * getHeaterPower(-1))/127);
-    SERIAL_PROTOCOLPGM("W");
+    SERIAL_PROTOCOLCHAR('W');
   #else
     SERIAL_PROTOCOL(getHeaterPower(-1));
   #endif
 
   #ifdef SHOW_TEMP_ADC_VALUES
-    #if defined(TEMP_BED_PIN) && TEMP_BED_PIN > -1
+    #if HAS_TEMP_BED
       SERIAL_PROTOCOLPGM("    ADC B:");
       SERIAL_PROTOCOL_F(degBed(),1);
       SERIAL_PROTOCOLPGM("C->");
@@ -3155,29 +3147,29 @@ inline void gcode_M105() {
     for (int8_t cur_extruder = 0; cur_extruder < EXTRUDERS; ++cur_extruder) {
       SERIAL_PROTOCOLPGM("  T");
       SERIAL_PROTOCOL(cur_extruder);
-      SERIAL_PROTOCOLPGM(":");
+      SERIAL_PROTOCOLCHAR(':');
       SERIAL_PROTOCOL_F(degHotend(cur_extruder),1);
       SERIAL_PROTOCOLPGM("C->");
       SERIAL_PROTOCOL_F(rawHotendTemp(cur_extruder)/OVERSAMPLENR,0);
     }
   #endif
 
-  SERIAL_PROTOCOLLN("");
+  SERIAL_EOL;
 }
 
-#if defined(FAN_PIN) && FAN_PIN > -1
+#if HAS_FAN
 
   /**
    * M106: Set Fan Speed
    */
-  inline void gcode_M106() { fanSpeed = code_seen('S') ? constrain(code_value(), 0, 255) : 255; }
+  inline void gcode_M106() { fanSpeed = code_seen('S') ? constrain(code_value_short(), 0, 255) : 255; }
 
   /**
    * M107: Fan Off
    */
   inline void gcode_M107() { fanSpeed = 0; }
 
-#endif //FAN_PIN
+#endif // HAS_FAN
 
 /**
  * M109: Wait for extruder(s) to reach temperature
@@ -3189,10 +3181,11 @@ inline void gcode_M109() {
 
   CooldownNoWait = code_seen('S');
   if (CooldownNoWait || code_seen('R')) {
-    setTargetHotend(code_value(), tmp_extruder);
+    float temp = code_value();
+    setTargetHotend(temp, target_extruder);
     #ifdef DUAL_X_CARRIAGE
-      if (dual_x_carriage_mode == DXC_DUPLICATION_MODE && tmp_extruder == 0)
-        setTargetHotend1(code_value() == 0.0 ? 0.0 : code_value() + duplicate_extruder_temp_offset);
+      if (dual_x_carriage_mode == DXC_DUPLICATION_MODE && target_extruder == 0)
+        setTargetHotend1(temp == 0.0 ? 0.0 : temp + duplicate_extruder_temp_offset);
     #endif
   }
 
@@ -3208,7 +3201,7 @@ inline void gcode_M109() {
   unsigned long timetemp = millis();
 
   /* See if we are heating up or cooling down */
-  target_direction = isHeatingHotend(tmp_extruder); // true if heating, false if cooling
+  target_direction = isHeatingHotend(target_extruder); // true if heating, false if cooling
 
   cancel_heatup = false;
 
@@ -3219,15 +3212,15 @@ inline void gcode_M109() {
     while((!cancel_heatup)&&((residencyStart == -1) ||
           (residencyStart >= 0 && (((unsigned int) (millis() - residencyStart)) < (TEMP_RESIDENCY_TIME * 1000UL)))) )
   #else
-    while ( target_direction ? (isHeatingHotend(tmp_extruder)) : (isCoolingHotend(tmp_extruder)&&(CooldownNoWait==false)) )
+    while ( target_direction ? (isHeatingHotend(target_extruder)) : (isCoolingHotend(target_extruder)&&(CooldownNoWait==false)) )
   #endif //TEMP_RESIDENCY_TIME
 
     { // while loop
       if (millis() > timetemp + 1000UL) { //Print temp & remaining time every 1s while waiting
         SERIAL_PROTOCOLPGM("T:");
-        SERIAL_PROTOCOL_F(degHotend(tmp_extruder),1);
+        SERIAL_PROTOCOL_F(degHotend(target_extruder),1);
         SERIAL_PROTOCOLPGM(" E:");
-        SERIAL_PROTOCOL((int)tmp_extruder);
+        SERIAL_PROTOCOL((int)target_extruder);
         #ifdef TEMP_RESIDENCY_TIME
           SERIAL_PROTOCOLPGM(" W:");
           if (residencyStart > -1) {
@@ -3235,10 +3228,10 @@ inline void gcode_M109() {
             SERIAL_PROTOCOLLN( timetemp );
           }
           else {
-            SERIAL_PROTOCOLLN( "?" );
+            SERIAL_PROTOCOLLNPGM("?");
           }
         #else
-          SERIAL_PROTOCOLLN("");
+          SERIAL_EOL;
         #endif
         timetemp = millis();
       }
@@ -3248,9 +3241,9 @@ inline void gcode_M109() {
       #ifdef TEMP_RESIDENCY_TIME
         // start/restart the TEMP_RESIDENCY_TIME timer whenever we reach target temp for the first time
         // or when current temp falls outside the hysteresis after target temp was reached
-        if ((residencyStart == -1 &&  target_direction && (degHotend(tmp_extruder) >= (degTargetHotend(tmp_extruder)-TEMP_WINDOW))) ||
-            (residencyStart == -1 && !target_direction && (degHotend(tmp_extruder) <= (degTargetHotend(tmp_extruder)+TEMP_WINDOW))) ||
-            (residencyStart > -1 && labs(degHotend(tmp_extruder) - degTargetHotend(tmp_extruder)) > TEMP_HYSTERESIS) )
+        if ((residencyStart == -1 &&  target_direction && (degHotend(target_extruder) >= (degTargetHotend(target_extruder)-TEMP_WINDOW))) ||
+            (residencyStart == -1 && !target_direction && (degHotend(target_extruder) <= (degTargetHotend(target_extruder)+TEMP_WINDOW))) ||
+            (residencyStart > -1 && labs(degHotend(target_extruder) - degTargetHotend(target_extruder)) > TEMP_HYSTERESIS) )
         {
           residencyStart = millis();
         }
@@ -3258,10 +3251,11 @@ inline void gcode_M109() {
     }
 
   LCD_MESSAGEPGM(MSG_HEATING_COMPLETE);
-  starttime = previous_millis_cmd = millis();
+  refresh_cmd_timeout();
+  starttime = previous_millis_cmd;
 }
 
-#if defined(TEMP_BED_PIN) && TEMP_BED_PIN > -1
+#if HAS_TEMP_BED
 
   /**
    * M190: Sxxx Wait for bed current temp to reach target temp. Waits only when heating
@@ -3289,17 +3283,17 @@ inline void gcode_M109() {
         SERIAL_PROTOCOL((int)active_extruder);
         SERIAL_PROTOCOLPGM(" B:");
         SERIAL_PROTOCOL_F(degBed(), 1);
-        SERIAL_PROTOCOLLN("");
+        SERIAL_EOL;
       }
       manage_heater();
       manage_inactivity();
       lcd_update();
     }
     LCD_MESSAGEPGM(MSG_BED_DONE);
-    previous_millis_cmd = millis();
+    refresh_cmd_timeout();
   }
 
-#endif // TEMP_BED_PIN > -1
+#endif // HAS_TEMP_BED
 
 /**
  * M112: Emergency Stop
@@ -3310,7 +3304,7 @@ inline void gcode_M112() {
 
 #ifdef BARICUDA
 
-  #if defined(HEATER_1_PIN) && HEATER_1_PIN > -1
+  #if HAS_HEATER_1
     /**
      * M126: Heater 1 valve open
      */
@@ -3321,7 +3315,7 @@ inline void gcode_M112() {
     inline void gcode_M127() { ValvePressure = 0; }
   #endif
 
-  #if defined(HEATER_2_PIN) && HEATER_2_PIN > -1
+  #if HAS_HEATER_2
     /**
      * M128: Heater 2 valve open
      */
@@ -3352,7 +3346,7 @@ inline void gcode_M140() {
     // If you have a switch on suicide pin, this is useful
     // if you want to start another print with suicide feature after
     // a print without suicide...
-    #if defined(SUICIDE_PIN) && SUICIDE_PIN > -1
+    #if HAS_SUICIDE
       OUT_WRITE(SUICIDE_PIN, HIGH);
     #endif
 
@@ -3380,7 +3374,7 @@ inline void gcode_M81() {
   finishAndDisableSteppers();
   fanSpeed = 0;
   delay(1000); // Wait 1 second before switching off
-  #if defined(SUICIDE_PIN) && SUICIDE_PIN > -1
+  #if HAS_SUICIDE
     st_synchronize();
     suicide();
   #elif HAS_POWER_SWITCH
@@ -3490,27 +3484,26 @@ inline void gcode_M114() {
   SERIAL_PROTOCOLPGM(" Z:");
   SERIAL_PROTOCOL(float(st_get_position(Z_AXIS))/axis_steps_per_unit[Z_AXIS]);
 
-  SERIAL_PROTOCOLLN("");
+  SERIAL_EOL;
 
   #ifdef SCARA
     SERIAL_PROTOCOLPGM("SCARA Theta:");
     SERIAL_PROTOCOL(delta[X_AXIS]);
     SERIAL_PROTOCOLPGM("   Psi+Theta:");
     SERIAL_PROTOCOL(delta[Y_AXIS]);
-    SERIAL_PROTOCOLLN("");
+    SERIAL_EOL;
     
     SERIAL_PROTOCOLPGM("SCARA Cal - Theta:");
     SERIAL_PROTOCOL(delta[X_AXIS]+home_offset[X_AXIS]);
     SERIAL_PROTOCOLPGM("   Psi+Theta (90):");
     SERIAL_PROTOCOL(delta[Y_AXIS]-delta[X_AXIS]-90+home_offset[Y_AXIS]);
-    SERIAL_PROTOCOLLN("");
+    SERIAL_EOL;
     
     SERIAL_PROTOCOLPGM("SCARA step Cal - Theta:");
     SERIAL_PROTOCOL(delta[X_AXIS]/90*axis_steps_per_unit[X_AXIS]);
     SERIAL_PROTOCOLPGM("   Psi+Theta:");
     SERIAL_PROTOCOL((delta[Y_AXIS]-delta[X_AXIS])/90*axis_steps_per_unit[Y_AXIS]);
-    SERIAL_PROTOCOLLN("");
-    SERIAL_PROTOCOLLN("");
+    SERIAL_EOL; SERIAL_EOL;
   #endif
 }
 
@@ -3536,35 +3529,38 @@ inline void gcode_M117() {
  */
 inline void gcode_M119() {
   SERIAL_PROTOCOLLN(MSG_M119_REPORT);
-  #if defined(X_MIN_PIN) && X_MIN_PIN > -1
+  #if HAS_X_MIN
     SERIAL_PROTOCOLPGM(MSG_X_MIN);
     SERIAL_PROTOCOLLN(((READ(X_MIN_PIN)^X_MIN_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
   #endif
-  #if defined(X_MAX_PIN) && X_MAX_PIN > -1
+  #if HAS_X_MAX
     SERIAL_PROTOCOLPGM(MSG_X_MAX);
     SERIAL_PROTOCOLLN(((READ(X_MAX_PIN)^X_MAX_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
   #endif
-  #if defined(Y_MIN_PIN) && Y_MIN_PIN > -1
+  #if HAS_Y_MIN
     SERIAL_PROTOCOLPGM(MSG_Y_MIN);
     SERIAL_PROTOCOLLN(((READ(Y_MIN_PIN)^Y_MIN_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
   #endif
-  #if defined(Y_MAX_PIN) && Y_MAX_PIN > -1
+  #if HAS_Y_MAX
     SERIAL_PROTOCOLPGM(MSG_Y_MAX);
     SERIAL_PROTOCOLLN(((READ(Y_MAX_PIN)^Y_MAX_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
   #endif
-  #if defined(Z_MIN_PIN) && Z_MIN_PIN > -1
+  #if HAS_Z_MIN
     SERIAL_PROTOCOLPGM(MSG_Z_MIN);
     SERIAL_PROTOCOLLN(((READ(Z_MIN_PIN)^Z_MIN_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
   #endif
-  #if defined(Z_MAX_PIN) && Z_MAX_PIN > -1
+  #if HAS_Z_MAX
     SERIAL_PROTOCOLPGM(MSG_Z_MAX);
     SERIAL_PROTOCOLLN(((READ(Z_MAX_PIN)^Z_MAX_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
   #endif
-  #if defined(Z2_MAX_PIN) && Z2_MAX_PIN > -1
+  #if HAS_Z2_MAX
     SERIAL_PROTOCOLPGM(MSG_Z2_MAX);
     SERIAL_PROTOCOLLN(((READ(Z2_MAX_PIN)^Z2_MAX_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
   #endif
-  
+  #if HAS_Z_PROBE
+    SERIAL_PROTOCOLPGM(MSG_Z_PROBE);
+    SERIAL_PROTOCOLLN(((READ(Z_PROBE_PIN)^Z_PROBE_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
+  #endif
 }
 
 /**
@@ -3584,9 +3580,9 @@ inline void gcode_M121() { enable_endstops(true); }
    */
   inline void gcode_M150() {
     SendColors(
-      code_seen('R') ? (byte)code_value() : 0,
-      code_seen('U') ? (byte)code_value() : 0,
-      code_seen('B') ? (byte)code_value() : 0
+      code_seen('R') ? (byte)code_value_short() : 0,
+      code_seen('U') ? (byte)code_value_short() : 0,
+      code_seen('B') ? (byte)code_value_short() : 0
     );
   }
 
@@ -3598,9 +3594,9 @@ inline void gcode_M121() { enable_endstops(true); }
  *       D<millimeters>
  */
 inline void gcode_M200() {
-  tmp_extruder = active_extruder;
+  int tmp_extruder = active_extruder;
   if (code_seen('T')) {
-    tmp_extruder = code_value();
+    tmp_extruder = code_value_short();
     if (tmp_extruder >= EXTRUDERS) {
       SERIAL_ECHO_START;
       SERIAL_ECHO(MSG_M200_INVALID_EXTRUDER);
@@ -3671,27 +3667,23 @@ inline void gcode_M203() {
  *  Also sets minimum segment time in ms (B20000) to prevent buffer under-runs and M20 minimum feedrate
  */
 inline void gcode_M204() {
-  if (code_seen('S'))   // Kept for legacy compatibility. Should NOT BE USED for new developments.
-  {
+  if (code_seen('S')) {  // Kept for legacy compatibility. Should NOT BE USED for new developments.
     acceleration = code_value();
     travel_acceleration = acceleration;
-    SERIAL_ECHOPAIR("Setting Printing and Travelling Acceleration: ", acceleration );
+    SERIAL_ECHOPAIR("Setting Print and Travel Acceleration: ", acceleration );
     SERIAL_EOL;
   }
-  if (code_seen('P'))
-  {
+  if (code_seen('P')) {
     acceleration = code_value();
-    SERIAL_ECHOPAIR("Setting Printing Acceleration: ", acceleration );
+    SERIAL_ECHOPAIR("Setting Print Acceleration: ", acceleration );
     SERIAL_EOL;
   }
-  if (code_seen('R'))
-  {
+  if (code_seen('R')) {
     retract_acceleration = code_value();
     SERIAL_ECHOPAIR("Setting Retract Acceleration: ", retract_acceleration );
     SERIAL_EOL;
   }
-  if (code_seen('T'))
-  {
+  if (code_seen('T')) {
     travel_acceleration = code_value();
     SERIAL_ECHOPAIR("Setting Travel Acceleration: ", travel_acceleration );
     SERIAL_EOL;
@@ -3794,7 +3786,7 @@ inline void gcode_M206() {
    */
   inline void gcode_M209() {
     if (code_seen('S')) {
-      int t = code_value();
+      int t = code_value_short();
       switch(t) {
         case 0:
           autoretract_enabled = false;
@@ -3823,23 +3815,23 @@ inline void gcode_M206() {
   inline void gcode_M218() {
     if (setTargetedHotend(218)) return;
 
-    if (code_seen('X')) extruder_offset[X_AXIS][tmp_extruder] = code_value();
-    if (code_seen('Y')) extruder_offset[Y_AXIS][tmp_extruder] = code_value();
+    if (code_seen('X')) extruder_offset[X_AXIS][target_extruder] = code_value();
+    if (code_seen('Y')) extruder_offset[Y_AXIS][target_extruder] = code_value();
 
     #ifdef DUAL_X_CARRIAGE
-      if (code_seen('Z')) extruder_offset[Z_AXIS][tmp_extruder] = code_value();
+      if (code_seen('Z')) extruder_offset[Z_AXIS][target_extruder] = code_value();
     #endif
 
     SERIAL_ECHO_START;
     SERIAL_ECHOPGM(MSG_HOTEND_OFFSET);
-    for (tmp_extruder = 0; tmp_extruder < EXTRUDERS; tmp_extruder++) {
-      SERIAL_ECHO(" ");
-      SERIAL_ECHO(extruder_offset[X_AXIS][tmp_extruder]);
-      SERIAL_ECHO(",");
-      SERIAL_ECHO(extruder_offset[Y_AXIS][tmp_extruder]);
+    for (int e = 0; e < EXTRUDERS; e++) {
+      SERIAL_CHAR(' ');
+      SERIAL_ECHO(extruder_offset[X_AXIS][e]);
+      SERIAL_CHAR(',');
+      SERIAL_ECHO(extruder_offset[Y_AXIS][e]);
       #ifdef DUAL_X_CARRIAGE
-        SERIAL_ECHO(",");
-        SERIAL_ECHO(extruder_offset[Z_AXIS][tmp_extruder]);
+        SERIAL_CHAR(',');
+        SERIAL_ECHO(extruder_offset[Z_AXIS][e]);
       #endif
     }
     SERIAL_EOL;
@@ -3862,7 +3854,7 @@ inline void gcode_M221() {
     int sval = code_value();
     if (code_seen('T')) {
       if (setTargetedHotend(221)) return;
-      extruder_multiply[tmp_extruder] = sval;
+      extruder_multiply[target_extruder] = sval;
     }
     else {
       extruder_multiply[active_extruder] = sval;
@@ -3953,7 +3945,7 @@ inline void gcode_M226() {
       SERIAL_PROTOCOL(servo_index);
       SERIAL_PROTOCOL(": ");
       SERIAL_PROTOCOL(servos[servo_index].read());
-      SERIAL_PROTOCOLLN("");
+      SERIAL_EOL;
     }
   }
 
@@ -4021,7 +4013,7 @@ inline void gcode_M226() {
         //Kc does not have scaling applied above, or in resetting defaults
         SERIAL_PROTOCOL(PID_PARAM(Kc, e));
       #endif
-      SERIAL_PROTOCOLLN("");    
+      SERIAL_EOL;    
     }
     else {
       SERIAL_ECHO_START;
@@ -4046,12 +4038,12 @@ inline void gcode_M226() {
     SERIAL_PROTOCOL(unscalePID_i(bedKi));
     SERIAL_PROTOCOL(" d:");
     SERIAL_PROTOCOL(unscalePID_d(bedKd));
-    SERIAL_PROTOCOLLN("");
+    SERIAL_EOL;
   }
 
 #endif // PIDTEMPBED
 
-#if defined(CHDK) || (defined(PHOTOGRAPH_PIN) && PHOTOGRAPH_PIN > -1)
+#if defined(CHDK) || HAS_PHOTOGRAPH
 
   /**
    * M240: Trigger a camera by emulating a Canon RC-1
@@ -4064,7 +4056,7 @@ inline void gcode_M226() {
        chdkHigh = millis();
        chdkActive = true;
      
-    #elif defined(PHOTOGRAPH_PIN) && PHOTOGRAPH_PIN > -1
+    #elif HAS_PHOTOGRAPH
 
       const uint8_t NUM_PULSES = 16;
       const float PULSE_LENGTH = 0.01524;
@@ -4082,7 +4074,7 @@ inline void gcode_M226() {
         _delay_ms(PULSE_LENGTH);
       }
 
-    #endif // !CHDK && PHOTOGRAPH_PIN > -1
+    #endif // !CHDK && HAS_PHOTOGRAPH
   }
 
 #endif // CHDK || PHOTOGRAPH_PIN
@@ -4093,10 +4085,10 @@ inline void gcode_M226() {
    * M250: Read and optionally set the LCD contrast
    */
   inline void gcode_M250() {
-    if (code_seen('C')) lcd_setcontrast(code_value_long() & 0x3F);
+    if (code_seen('C')) lcd_setcontrast(code_value_short() & 0x3F);
     SERIAL_PROTOCOLPGM("lcd contrast value: ");
     SERIAL_PROTOCOL(lcd_contrast);
-    SERIAL_PROTOCOLLN("");
+    SERIAL_EOL;
   }
 
 #endif // DOGLCD
@@ -4119,8 +4111,8 @@ inline void gcode_M226() {
  *       C<cycles>
  */
 inline void gcode_M303() {
-  int e = code_seen('E') ? code_value_long() : 0;
-  int c = code_seen('C') ? code_value_long() : 5;
+  int e = code_seen('E') ? code_value_short() : 0;
+  int c = code_seen('C') ? code_value_short() : 5;
   float temp = code_seen('S') ? code_value() : (e < 0 ? 70.0 : 150.0);
   PID_autotune(temp, e, c);
 }
@@ -4203,17 +4195,17 @@ inline void gcode_M303() {
       case 0:
         OUT_WRITE(SOL0_PIN, HIGH);
         break;
-        #if defined(SOL1_PIN) && SOL1_PIN > -1
+        #if HAS_SOLENOID_1
           case 1:
             OUT_WRITE(SOL1_PIN, HIGH);
             break;
         #endif
-        #if defined(SOL2_PIN) && SOL2_PIN > -1
+        #if HAS_SOLENOID_2
           case 2:
             OUT_WRITE(SOL2_PIN, HIGH);
             break;
         #endif
-        #if defined(SOL3_PIN) && SOL3_PIN > -1
+        #if HAS_SOLENOID_3
           case 3:
             OUT_WRITE(SOL3_PIN, HIGH);
             break;
@@ -4256,11 +4248,11 @@ inline void gcode_M400() { st_synchronize(); }
   /**
    * M401: Engage Z Servo endstop if available
    */
-  inline void gcode_M401() { engage_z_probe(); }
+  inline void gcode_M401() { deploy_z_probe(); }
   /**
    * M402: Retract Z Servo endstop if enabled
    */
-  inline void gcode_M402() { retract_z_probe(); }
+  inline void gcode_M402() { stow_z_probe(); }
 
 #endif
 
@@ -4270,7 +4262,7 @@ inline void gcode_M400() { st_synchronize(); }
    * M404: Display or set the nominal filament width (3mm, 1.75mm ) W<3.0>
    */
   inline void gcode_M404() {
-    #if FILWIDTH_PIN > -1
+    #if HAS_FILWIDTH
       if (code_seen('W')) {
         filament_width_nominal = code_value();
       }
@@ -4369,7 +4361,7 @@ inline void gcode_M503() {
         zprobe_zoffset = -value; // compare w/ line 278 of ConfigurationStore.cpp
         SERIAL_ECHO_START;
         SERIAL_ECHOLNPGM(MSG_ZPROBE_ZOFFSET " " MSG_OK);
-        SERIAL_PROTOCOLLN("");
+        SERIAL_EOL;
       }
       else {
         SERIAL_ECHO_START;
@@ -4378,14 +4370,14 @@ inline void gcode_M503() {
         SERIAL_ECHO(Z_PROBE_OFFSET_RANGE_MIN);
         SERIAL_ECHOPGM(MSG_Z_MAX);
         SERIAL_ECHO(Z_PROBE_OFFSET_RANGE_MAX);
-        SERIAL_PROTOCOLLN("");
+        SERIAL_EOL;
       }
     }
     else {
       SERIAL_ECHO_START;
       SERIAL_ECHOLNPGM(MSG_ZPROBE_ZOFFSET " : ");
       SERIAL_ECHO(-zprobe_zoffset);
-      SERIAL_PROTOCOLLN("");
+      SERIAL_EOL;
     }
   }
 
@@ -4529,13 +4521,13 @@ inline void gcode_M503() {
         if (code_seen('R')) duplicate_extruder_temp_offset = code_value();
         SERIAL_ECHO_START;
         SERIAL_ECHOPGM(MSG_HOTEND_OFFSET);
-        SERIAL_ECHO(" ");
+        SERIAL_CHAR(' ');
         SERIAL_ECHO(extruder_offset[X_AXIS][0]);
-        SERIAL_ECHO(",");
+        SERIAL_CHAR(',');
         SERIAL_ECHO(extruder_offset[Y_AXIS][0]);
-        SERIAL_ECHO(" ");
+        SERIAL_CHAR(' ');
         SERIAL_ECHO(duplicate_extruder_x_offset);
-        SERIAL_ECHO(",");
+        SERIAL_CHAR(',');
         SERIAL_ECHOLN(extruder_offset[Y_AXIS][1]);
         break;
       case DXC_FULL_CONTROL_MODE:
@@ -4593,23 +4585,22 @@ inline void gcode_M907() {
 
 #endif // HAS_DIGIPOTSS
 
-// M350 Set microstepping mode. Warning: Steps per unit remains unchanged. S code sets stepping mode for all drivers.
-inline void gcode_M350() {
-  #if defined(X_MS1_PIN) && X_MS1_PIN > -1
+#if HAS_MICROSTEPS
+
+  // M350 Set microstepping mode. Warning: Steps per unit remains unchanged. S code sets stepping mode for all drivers.
+  inline void gcode_M350() {
     if(code_seen('S')) for(int i=0;i<=4;i++) microstep_mode(i,code_value());
     for(int i=0;i<NUM_AXIS;i++) if(code_seen(axis_codes[i])) microstep_mode(i,(uint8_t)code_value());
     if(code_seen('B')) microstep_mode(4,code_value());
     microstep_readings();
-  #endif
-}
+  }
 
-/**
- * M351: Toggle MS1 MS2 pins directly with axis codes X Y Z E B
- *       S# determines MS1 or MS2, X# sets the pin high/low.
- */
-inline void gcode_M351() {
-  #if defined(X_MS1_PIN) && X_MS1_PIN > -1
-    if (code_seen('S')) switch(code_value_long()) {
+  /**
+   * M351: Toggle MS1 MS2 pins directly with axis codes X Y Z E B
+   *       S# determines MS1 or MS2, X# sets the pin high/low.
+   */
+  inline void gcode_M351() {
+    if (code_seen('S')) switch(code_value_short()) {
       case 1:
         for(int i=0;i<NUM_AXIS;i++) if (code_seen(axis_codes[i])) microstep_ms(i, code_value(), -1);
         if (code_seen('B')) microstep_ms(4, code_value(), -1);
@@ -4620,8 +4611,9 @@ inline void gcode_M351() {
         break;
     }
     microstep_readings();
-  #endif
-}
+  }
+
+#endif // HAS_MICROSTEPS
 
 /**
  * M999: Restart after being stopped
@@ -4634,28 +4626,33 @@ inline void gcode_M999() {
 }
 
 inline void gcode_T() {
-  tmp_extruder = code_value();
+  int tmp_extruder = code_value();
   if (tmp_extruder >= EXTRUDERS) {
     SERIAL_ECHO_START;
-    SERIAL_ECHO("T");
+    SERIAL_CHAR('T');
     SERIAL_ECHO(tmp_extruder);
     SERIAL_ECHOLN(MSG_INVALID_EXTRUDER);
   }
   else {
+    target_extruder = tmp_extruder;
+
     #if EXTRUDERS > 1
       bool make_move = false;
     #endif
+
     if (code_seen('F')) {
+
       #if EXTRUDERS > 1
         make_move = true;
       #endif
+
       next_feedrate = code_value();
       if (next_feedrate > 0.0) feedrate = next_feedrate;
     }
     #if EXTRUDERS > 1
       if (tmp_extruder != active_extruder) {
         // Save current position to return to after applying extruder offset
-        memcpy(destination, current_position, sizeof(destination));
+        set_destination_to_current();
         #ifdef DUAL_X_CARRIAGE
           if (dual_x_carriage_mode == DXC_AUTO_PARK_MODE && Stopped == false &&
                 (delayed_move_time != 0 || current_position[X_AXIS] != x_home_pos(active_extruder))) {
@@ -4733,11 +4730,12 @@ inline void gcode_T() {
 
 /**
  * Process Commands and dispatch them to handlers
+ * This is called from the main loop()
  */
 void process_commands() {
   if (code_seen('G')) {
 
-    int gCode = code_value_long();
+    int gCode = code_value_short();
 
     switch(gCode) {
 
@@ -4812,7 +4810,7 @@ void process_commands() {
   }
 
   else if (code_seen('M')) {
-    switch( code_value_long() ) {
+    switch(code_value_short()) {
       #ifdef ULTIPANEL
         case 0: // M0 - Unconditional stop - Wait for user button press on LCD
         case 1: // M1 - Conditional stop - Wait for user button press on LCD
@@ -4890,42 +4888,42 @@ void process_commands() {
         gcode_M109();
         break;
 
-      #if defined(TEMP_BED_PIN) && TEMP_BED_PIN > -1
+      #if HAS_TEMP_BED
         case 190: // M190 - Wait for bed heater to reach target.
           gcode_M190();
           break;
-      #endif //TEMP_BED_PIN
+      #endif // HAS_TEMP_BED
 
-      #if defined(FAN_PIN) && FAN_PIN > -1
+      #if HAS_FAN
         case 106: //M106 Fan On
           gcode_M106();
           break;
         case 107: //M107 Fan Off
           gcode_M107();
           break;
-      #endif //FAN_PIN
+      #endif // HAS_FAN
 
       #ifdef BARICUDA
         // PWM for HEATER_1_PIN
-        #if defined(HEATER_1_PIN) && HEATER_1_PIN > -1
+        #if HAS_HEATER_1
           case 126: // M126 valve open
             gcode_M126();
             break;
           case 127: // M127 valve closed
             gcode_M127();
             break;
-        #endif //HEATER_1_PIN
+        #endif // HAS_HEATER_1
 
         // PWM for HEATER_2_PIN
-        #if defined(HEATER_2_PIN) && HEATER_2_PIN > -1
+        #if HAS_HEATER_2
           case 128: // M128 valve open
             gcode_M128();
             break;
           case 129: // M129 valve closed
             gcode_M129();
             break;
-        #endif //HEATER_2_PIN
-      #endif //BARICUDA
+        #endif // HAS_HEATER_2
+      #endif // BARICUDA
 
       #if HAS_POWER_SWITCH
 
@@ -5073,7 +5071,7 @@ void process_commands() {
           break;
       #endif // PIDTEMPBED
 
-      #if defined(CHDK) || (defined(PHOTOGRAPH_PIN) && PHOTOGRAPH_PIN > -1)
+      #if defined(CHDK) || HAS_PHOTOGRAPH
         case 240: // M240  Triggers a camera by emulating a Canon RC-1 : http://www.doc-diy.net/photo/rc-1_hacked/
           gcode_M240();
           break;
@@ -5191,13 +5189,17 @@ void process_commands() {
           break;
       #endif // HAS_DIGIPOTSS
 
-      case 350: // M350 Set microstepping mode. Warning: Steps per unit remains unchanged. S code sets stepping mode for all drivers.
-        gcode_M350();
-        break;
+      #if HAS_MICROSTEPS
 
-      case 351: // M351 Toggle MS1 MS2 pins directly, S# determines MS1 or MS2, X# sets the pin high/low.
-        gcode_M351();
-        break;
+        case 350: // M350 Set microstepping mode. Warning: Steps per unit remains unchanged. S code sets stepping mode for all drivers.
+          gcode_M350();
+          break;
+
+        case 351: // M351 Toggle MS1 MS2 pins directly, S# determines MS1 or MS2, X# sets the pin high/low.
+          gcode_M351();
+          break;
+
+      #endif // HAS_MICROSTEPS
 
       case 999: // M999: Restart after being Stopped
         gcode_M999();
@@ -5219,8 +5221,7 @@ void process_commands() {
   ClearToSend();
 }
 
-void FlushSerialRequestResend()
-{
+void FlushSerialRequestResend() {
   //char cmdbuffer[bufindr][100]="Resend:";
   MYSERIAL.flush();
   SERIAL_PROTOCOLPGM(MSG_RESEND);
@@ -5228,13 +5229,11 @@ void FlushSerialRequestResend()
   ClearToSend();
 }
 
-void ClearToSend()
-{
-  previous_millis_cmd = millis();
+void ClearToSend() {
+  refresh_cmd_timeout();
   #ifdef SDSUPPORT
-  if(fromsd[bufindr])
-    return;
-  #endif //SDSUPPORT
+    if (fromsd[bufindr]) return;
+  #endif
   SERIAL_PROTOCOLLNPGM(MSG_OK);
 }
 
@@ -5251,29 +5250,18 @@ void get_coordinates() {
   }
 }
 
-void get_arc_coordinates()
-{
-#ifdef SF_ARC_FIX
-   bool relative_mode_backup = relative_mode;
-   relative_mode = true;
-#endif
-   get_coordinates();
-#ifdef SF_ARC_FIX
-   relative_mode=relative_mode_backup;
-#endif
+void get_arc_coordinates() {
+  #ifdef SF_ARC_FIX
+    bool relative_mode_backup = relative_mode;
+    relative_mode = true;
+  #endif
+    get_coordinates();
+  #ifdef SF_ARC_FIX
+    relative_mode = relative_mode_backup;
+  #endif
 
-   if(code_seen('I')) {
-     offset[0] = code_value();
-   }
-   else {
-     offset[0] = 0.0;
-   }
-   if(code_seen('J')) {
-     offset[1] = code_value();
-   }
-   else {
-     offset[1] = 0.0;
-   }
+  offset[0] = code_seen('I') ? code_value() : 0;
+  offset[1] = code_seen('J') ? code_value() : 0;
 }
 
 void clamp_to_software_endstops(float target[3])
@@ -5337,7 +5325,6 @@ void clamp_to_software_endstops(float target[3])
   #ifdef ENABLE_AUTO_BED_LEVELING
 
     // Adjust print surface height by linear interpolation over the bed_level array.
-    int delta_grid_spacing[2] = { 0, 0 };
     void adjust_delta(float cartesian[3]) {
       if (delta_grid_spacing[0] == 0 || delta_grid_spacing[1] == 0) return; // G29 not done!
 
@@ -5377,16 +5364,9 @@ void clamp_to_software_endstops(float target[3])
     }
   #endif // ENABLE_AUTO_BED_LEVELING
 
-  void prepare_move_raw() {
-    previous_millis_cmd = millis();
-    calculate_delta(destination);
-    plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], (feedrate/60)*(feedmultiply/100.0), active_extruder);
-    for (int i = 0; i < NUM_AXIS; i++) current_position[i] = destination[i];
-  }
-
 #endif // DELTA
 
-#if defined(MESH_BED_LEVELING)
+#ifdef MESH_BED_LEVELING
 
   #if !defined(MIN)
     #define MIN(_v1, _v2) (((_v1) < (_v2)) ? (_v1) : (_v2))
@@ -5397,9 +5377,7 @@ void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_
 {
   if (!mbl.active) {
     plan_buffer_line(x, y, z, e, feed_rate, extruder);
-    for(int8_t i=0; i < NUM_AXIS; i++) {
-      current_position[i] = destination[i];
-    }
+    set_current_to_destination();
     return;
   }
   int pix = mbl.select_x_index(current_position[X_AXIS]);
@@ -5413,9 +5391,7 @@ void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_
   if (pix == ix && piy == iy) {
     // Start and end on same mesh square
     plan_buffer_line(x, y, z, e, feed_rate, extruder);
-    for(int8_t i=0; i < NUM_AXIS; i++) {
-      current_position[i] = destination[i];
-    }
+    set_current_to_destination();
     return;
   }
   float nx, ny, ne, normalized_dist;
@@ -5446,9 +5422,7 @@ void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_
   } else {
     // Already split on a border
     plan_buffer_line(x, y, z, e, feed_rate, extruder);
-    for(int8_t i=0; i < NUM_AXIS; i++) {
-      current_position[i] = destination[i];
-    }
+    set_current_to_destination();
     return;
   }
   // Do the split and look for more borders
@@ -5465,7 +5439,7 @@ void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_
 
 void prepare_move() {
   clamp_to_software_endstops(destination);
-  previous_millis_cmd = millis();
+  refresh_cmd_timeout();
   
   #ifdef SCARA //for now same as delta-code
 
@@ -5536,64 +5510,58 @@ void prepare_move() {
 
   #endif // DELTA
 
-#ifdef DUAL_X_CARRIAGE
-  if (active_extruder_parked)
-  {
-    if (dual_x_carriage_mode == DXC_DUPLICATION_MODE && active_extruder == 0)
-    {
-      // move duplicate extruder into correct duplication position.
-      plan_set_position(inactive_extruder_x_pos, current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-      plan_buffer_line(current_position[X_AXIS] + duplicate_extruder_x_offset, current_position[Y_AXIS], current_position[Z_AXIS],
-          current_position[E_AXIS], max_feedrate[X_AXIS], 1);
-      sync_plan_position();
-      st_synchronize();
-      extruder_duplication_enabled = true;
-      active_extruder_parked = false;
-    }
-    else if (dual_x_carriage_mode == DXC_AUTO_PARK_MODE) // handle unparking of head
-    {
-      if (current_position[E_AXIS] == destination[E_AXIS])
-      {
-        // this is a travel move - skit it but keep track of current position (so that it can later
-        // be used as start of first non-travel move)
-        if (delayed_move_time != 0xFFFFFFFFUL)
-        {
-          memcpy(current_position, destination, sizeof(current_position));
-          if (destination[Z_AXIS] > raised_parked_position[Z_AXIS])
-            raised_parked_position[Z_AXIS] = destination[Z_AXIS];
-          delayed_move_time = millis();
-          return;
-        }
+  #ifdef DUAL_X_CARRIAGE
+    if (active_extruder_parked) {
+      if (dual_x_carriage_mode == DXC_DUPLICATION_MODE && active_extruder == 0) {
+        // move duplicate extruder into correct duplication position.
+        plan_set_position(inactive_extruder_x_pos, current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+        plan_buffer_line(current_position[X_AXIS] + duplicate_extruder_x_offset, current_position[Y_AXIS], current_position[Z_AXIS],
+            current_position[E_AXIS], max_feedrate[X_AXIS], 1);
+        sync_plan_position();
+        st_synchronize();
+        extruder_duplication_enabled = true;
+        active_extruder_parked = false;
+      }
+      else if (dual_x_carriage_mode == DXC_AUTO_PARK_MODE) { // handle unparking of head
+        if (current_position[E_AXIS] == destination[E_AXIS]) {
+          // this is a travel move - skit it but keep track of current position (so that it can later
+          // be used as start of first non-travel move)
+          if (delayed_move_time != 0xFFFFFFFFUL) {
+            set_current_to_destination();
+            if (destination[Z_AXIS] > raised_parked_position[Z_AXIS])
+              raised_parked_position[Z_AXIS] = destination[Z_AXIS];
+            delayed_move_time = millis();
+            return;
+          }
+        }
+        delayed_move_time = 0;
+        // unpark extruder: 1) raise, 2) move into starting XY position, 3) lower
+        plan_buffer_line(raised_parked_position[X_AXIS], raised_parked_position[Y_AXIS], raised_parked_position[Z_AXIS],    current_position[E_AXIS], max_feedrate[Z_AXIS], active_extruder);
+        plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], raised_parked_position[Z_AXIS],
+            current_position[E_AXIS], min(max_feedrate[X_AXIS],max_feedrate[Y_AXIS]), active_extruder);
+        plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS],
+            current_position[E_AXIS], max_feedrate[Z_AXIS], active_extruder);
+        active_extruder_parked = false;
       }
-      delayed_move_time = 0;
-      // unpark extruder: 1) raise, 2) move into starting XY position, 3) lower
-      plan_buffer_line(raised_parked_position[X_AXIS], raised_parked_position[Y_AXIS], raised_parked_position[Z_AXIS],    current_position[E_AXIS], max_feedrate[Z_AXIS], active_extruder);
-      plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], raised_parked_position[Z_AXIS],
-          current_position[E_AXIS], min(max_feedrate[X_AXIS],max_feedrate[Y_AXIS]), active_extruder);
-      plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS],
-          current_position[E_AXIS], max_feedrate[Z_AXIS], active_extruder);
-      active_extruder_parked = false;
     }
-  }
-#endif //DUAL_X_CARRIAGE
+  #endif // DUAL_X_CARRIAGE
 
-#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();
-  } 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);
-    return;
-#else
-    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)
+  #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();
+    }
+    else {
+      #ifdef 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);
+        return;
+      #else
+        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)
 
-  for(int8_t i=0; i < NUM_AXIS; i++) {
-    current_position[i] = destination[i];
-  }
+  set_current_to_destination();
 }
 
 void prepare_arc_move(char isclockwise) {
@@ -5605,19 +5573,11 @@ void prepare_arc_move(char isclockwise) {
   // As far as the parser is concerned, the position is now == target. In reality the
   // motion control system might still be processing the action and the real tool position
   // in any intermediate location.
-  for(int8_t i=0; i < NUM_AXIS; i++) {
-    current_position[i] = destination[i];
-  }
-  previous_millis_cmd = millis();
+  set_current_to_destination();
+  refresh_cmd_timeout();
 }
 
-#if defined(CONTROLLERFAN_PIN) && CONTROLLERFAN_PIN > -1
-
-#if defined(FAN_PIN)
-  #if CONTROLLERFAN_PIN == FAN_PIN
-    #error "You cannot set CONTROLLERFAN_PIN equal to FAN_PIN"
-  #endif
-#endif
+#if HAS_CONTROLLERFAN
 
 unsigned long lastMotor = 0; // Last time a motor was turned on
 unsigned long lastMotorCheck = 0; // Last time the state was checked
@@ -5630,7 +5590,7 @@ void controllerFan() {
       || E0_ENABLE_READ == E_ENABLE_ON // If any of the drivers are enabled...
       #if EXTRUDERS > 1
         || E1_ENABLE_READ == E_ENABLE_ON
-        #if defined(X2_ENABLE_PIN) && X2_ENABLE_PIN > -1
+        #if HAS_X2_ENABLE
           || X2_ENABLE_READ == X_ENABLE_ON
         #endif
         #if EXTRUDERS > 2
@@ -5742,7 +5702,7 @@ void handle_status_leds(void) {
        max_temp = max(max_temp, degHotend(cur_extruder));
        max_temp = max(max_temp, degTargetHotend(cur_extruder));
     }
-    #if defined(TEMP_BED_PIN) && TEMP_BED_PIN > -1
+    #if HAS_TEMP_BED
       max_temp = max(max_temp, degTargetBed());
       max_temp = max(max_temp, degBed());
     #endif
@@ -5762,134 +5722,175 @@ void handle_status_leds(void) {
 }
 #endif
 
-void manage_inactivity(bool ignore_stepper_queue/*=false*/) //default argument set in Marlin.h
-{
-  
-#if defined(KILL_PIN) && KILL_PIN > -1
-  static int killCount = 0;   // make the inactivity button a bit less responsive
-   const int KILL_DELAY = 750;
-#endif
+void enable_all_steppers() {
+  enable_x();
+  enable_y();
+  enable_z();
+  enable_e0();
+  enable_e1();
+  enable_e2();
+  enable_e3();
+}
 
-#if defined(FILRUNOUT_PIN) && FILRUNOUT_PIN > -1
-    if(card.sdprinting) {
-      if(!(READ(FILRUNOUT_PIN))^FIL_RUNOUT_INVERTING)
-      filrunout();        }
-#endif
+void disable_all_steppers() {
+  disable_x();
+  disable_y();
+  disable_z();
+  disable_e0();
+  disable_e1();
+  disable_e2();
+  disable_e3();
+}
 
-#if defined(HOME_PIN) && HOME_PIN > -1
-   static int homeDebounceCount = 0;   // poor man's debouncing count
-   const int HOME_DEBOUNCE_DELAY = 750;
-#endif
-   
+/**
+ * Manage several activities:
+ *  - Check for Filament Runout
+ *  - Keep the command buffer full
+ *  - Check for maximum inactive time between commands
+ *  - Check for maximum inactive time between stepper commands
+ *  - Check if pin CHDK needs to go LOW
+ *  - Check for KILL button held down
+ *  - Check for HOME button held down
+ *  - Check if cooling fan needs to be switched on
+ *  - Check if an idle but hot extruder needs filament extruded (EXTRUDER_RUNOUT_PREVENT)
+ */
+void manage_inactivity(bool ignore_stepper_queue/*=false*/) {
   
-  if(buflen < (BUFSIZE-1))
-    get_command();
+  #if HAS_FILRUNOUT
+    if (card.sdprinting && !(READ(FILRUNOUT_PIN) ^ FIL_RUNOUT_INVERTING))
+      filrunout();
+  #endif
 
-  if( (millis() - previous_millis_cmd) >  max_inactive_time )
-    if(max_inactive_time)
-      kill();
-  if(stepper_inactive_time)  {
-    if( (millis() - previous_millis_cmd) >  stepper_inactive_time )
-    {
-      if(blocks_queued() == false && ignore_stepper_queue == false) {
-        disable_x();
-        disable_y();
-        disable_z();
-        disable_e0();
-        disable_e1();
-        disable_e2();
-        disable_e3();
-      }
-    }
-  }
-  
-  #ifdef CHDK //Check if pin should be set to LOW after M240 set it to HIGH
-    if (chdkActive && (millis() - chdkHigh > CHDK_DELAY))
-    {
+  if (buflen < BUFSIZE - 1) get_command();
+
+  unsigned long ms = millis();
+
+  if (max_inactive_time && ms > previous_millis_cmd + max_inactive_time) kill();
+
+  if (stepper_inactive_time && ms > previous_millis_cmd + stepper_inactive_time
+      && !ignore_stepper_queue && !blocks_queued())
+    disable_all_steppers();
+
+  #ifdef CHDK // Check if pin should be set to LOW after M240 set it to HIGH
+    if (chdkActive && ms > chdkHigh + CHDK_DELAY) {
       chdkActive = false;
       WRITE(CHDK, LOW);
     }
   #endif
-  
-  #if defined(KILL_PIN) && KILL_PIN > -1
+
+  #if HAS_KILL
     
     // Check if the kill button was pressed and wait just in case it was an accidental
     // key kill key press
     // -------------------------------------------------------------------------------
-    if( 0 == READ(KILL_PIN) )
-    {
+    static int killCount = 0;   // make the inactivity button a bit less responsive
+    const int KILL_DELAY = 750;
+    if (!READ(KILL_PIN))
        killCount++;
-    }
     else if (killCount > 0)
-    {
        killCount--;
-    }
+
     // Exceeded threshold and we can confirm that it was not accidental
     // KILL the machine
     // ----------------------------------------------------------------
-    if ( killCount >= KILL_DELAY)
-    {
-       kill();
+    if (killCount >= KILL_DELAY) kill();
+  #endif
+
+  #if HAS_HOME
+    // Check to see if we have to home, use poor man's debouncer
+    // ---------------------------------------------------------
+    static int homeDebounceCount = 0;   // poor man's debouncing count
+    const int HOME_DEBOUNCE_DELAY = 750;
+    if (!READ(HOME_PIN)) {
+      if (!homeDebounceCount) {
+        enquecommands_P(PSTR("G28"));
+        LCD_ALERTMESSAGEPGM(MSG_AUTO_HOME);
+      }
+      if (homeDebounceCount < HOME_DEBOUNCE_DELAY)
+        homeDebounceCount++;
+      else
+        homeDebounceCount = 0;
+    }
+  #endif
+    
+  #if HAS_CONTROLLERFAN
+    controllerFan(); // Check if fan should be turned on to cool stepper drivers down
+  #endif
+
+  #ifdef EXTRUDER_RUNOUT_PREVENT
+    if (ms > previous_millis_cmd + EXTRUDER_RUNOUT_SECONDS * 1000)
+    if (degHotend(active_extruder) > EXTRUDER_RUNOUT_MINTEMP) {
+      bool oldstatus;
+      switch(active_extruder) {
+        case 0:
+          oldstatus = E0_ENABLE_READ;
+          enable_e0();
+          break;
+        #if EXTRUDERS > 1
+          case 1:
+            oldstatus = E1_ENABLE_READ;
+            enable_e1();
+            break;
+          #if EXTRUDERS > 2
+            case 2:
+              oldstatus = E2_ENABLE_READ;
+              enable_e2();
+              break;
+            #if EXTRUDERS > 3
+              case 3:
+                oldstatus = E3_ENABLE_READ;
+                enable_e3();
+                break;
+            #endif
+          #endif
+        #endif
+      }
+      float oldepos = current_position[E_AXIS], oldedes = destination[E_AXIS];
+      plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS],
+                      destination[E_AXIS] + EXTRUDER_RUNOUT_EXTRUDE * EXTRUDER_RUNOUT_ESTEPS / axis_steps_per_unit[E_AXIS],
+                      EXTRUDER_RUNOUT_SPEED / 60. * EXTRUDER_RUNOUT_ESTEPS / axis_steps_per_unit[E_AXIS], active_extruder);
+      current_position[E_AXIS] = oldepos;
+      destination[E_AXIS] = oldedes;
+      plan_set_e_position(oldepos);
+      previous_millis_cmd = ms; // refresh_cmd_timeout()
+      st_synchronize();
+      switch(active_extruder) {
+        case 0:
+          E0_ENABLE_WRITE(oldstatus);
+          break;
+        #if EXTRUDERS > 1
+          case 1:
+            E1_ENABLE_WRITE(oldstatus);
+            break;
+          #if EXTRUDERS > 2
+            case 2:
+              E2_ENABLE_WRITE(oldstatus);
+              break;
+            #if EXTRUDERS > 3
+              case 3:
+                E3_ENABLE_WRITE(oldstatus);
+                break;
+            #endif
+          #endif
+        #endif
+      }
     }
   #endif
 
-#if defined(HOME_PIN) && HOME_PIN > -1
-    // Check to see if we have to home, use poor man's debouncer
-    // ---------------------------------------------------------
-    if ( 0 == READ(HOME_PIN) )
-    {
-       if (homeDebounceCount == 0)
-       {
-          enquecommands_P((PSTR("G28")));
-          homeDebounceCount++;
-          LCD_ALERTMESSAGEPGM(MSG_AUTO_HOME);
-       }
-       else if (homeDebounceCount < HOME_DEBOUNCE_DELAY)
-       {
-          homeDebounceCount++;
-       }
-       else
-       {
-          homeDebounceCount = 0;
-       }
-    }
-#endif
-    
-  #if defined(CONTROLLERFAN_PIN) && CONTROLLERFAN_PIN > -1
-    controllerFan(); //Check if fan should be turned on to cool stepper drivers down
-  #endif
-  #ifdef EXTRUDER_RUNOUT_PREVENT
-    if( (millis() - previous_millis_cmd) >  EXTRUDER_RUNOUT_SECONDS*1000 )
-    if(degHotend(active_extruder)>EXTRUDER_RUNOUT_MINTEMP)
-    {
-     bool oldstatus=E0_ENABLE_READ;
-     enable_e0();
-     float oldepos=current_position[E_AXIS];
-     float oldedes=destination[E_AXIS];
-     plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS],
-                      destination[E_AXIS]+EXTRUDER_RUNOUT_EXTRUDE*EXTRUDER_RUNOUT_ESTEPS/axis_steps_per_unit[E_AXIS],
-                      EXTRUDER_RUNOUT_SPEED/60.*EXTRUDER_RUNOUT_ESTEPS/axis_steps_per_unit[E_AXIS], active_extruder);
-     current_position[E_AXIS]=oldepos;
-     destination[E_AXIS]=oldedes;
-     plan_set_e_position(oldepos);
-     previous_millis_cmd=millis();
-     st_synchronize();
-     E0_ENABLE_WRITE(oldstatus);
-    }
-  #endif
-  #if defined(DUAL_X_CARRIAGE)
+  #ifdef DUAL_X_CARRIAGE
     // handle delayed move timeout
-    if (delayed_move_time != 0 && (millis() - delayed_move_time) > 1000 && Stopped == false)
-    {
+    if (delayed_move_time && ms > delayed_move_time + 1000 && !Stopped) {
       // travel moves have been received so enact them
       delayed_move_time = 0xFFFFFFFFUL; // force moves to be done
-      memcpy(destination,current_position,sizeof(destination));
+      set_destination_to_current();
       prepare_move();
     }
   #endif
+
   #ifdef TEMP_STAT_LEDS
-      handle_status_leds();
+    handle_status_leds();
   #endif
+
   check_axes_activity();
 }
 
@@ -5898,13 +5899,7 @@ void kill()
   cli(); // Stop interrupts
   disable_heater();
 
-  disable_x();
-  disable_y();
-  disable_z();
-  disable_e0();
-  disable_e1();
-  disable_e2();
-  disable_e3();
+  disable_all_steppers();
 
   #if HAS_POWER_SWITCH
     pinMode(PS_ON_PIN, INPUT);
@@ -6017,10 +6012,10 @@ void setPwmFrequency(uint8_t pin, int val)
 #endif //FAST_PWM_FAN
 
 bool setTargetedHotend(int code){
-  tmp_extruder = active_extruder;
-  if(code_seen('T')) {
-    tmp_extruder = code_value();
-    if(tmp_extruder >= EXTRUDERS) {
+  target_extruder = active_extruder;
+  if (code_seen('T')) {
+    target_extruder = code_value_short();
+    if (target_extruder >= EXTRUDERS) {
       SERIAL_ECHO_START;
       switch(code){
         case 104:
@@ -6039,7 +6034,7 @@ bool setTargetedHotend(int code){
           SERIAL_ECHO(MSG_M221_INVALID_EXTRUDER);
           break;
       }
-      SERIAL_ECHOLN(tmp_extruder);
+      SERIAL_ECHOLN(target_extruder);
       return true;
     }
   }
diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h
index 8c05f83bc91..d92938c1d35 100644
--- a/Marlin/SanityCheck.h
+++ b/Marlin/SanityCheck.h
@@ -56,7 +56,7 @@
   #if EXTRUDERS > 1
 
     #if EXTRUDERS > 4
-      #error The maximum number of EXTRUDERS is 4.
+      #error The maximum number of EXTRUDERS in Marlin is 4.
     #endif
 
     #ifdef TEMP_SENSOR_1_AS_REDUNDANT
@@ -77,6 +77,13 @@
 
   #endif // EXTRUDERS > 1
 
+  /**
+   * Limited number of servos
+   */
+  #if NUM_SERVOS > 4
+    #error The maximum number of SERVOS in Marlin is 4.
+  #endif
+
   /**
    * Required LCD language
    */
@@ -93,13 +100,39 @@
      * 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 || (!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
+          #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_ENDSTOP is enabled.
+     */
+    #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_ENDSTOP
+      #endif
+      #if Z_PROBE_PIN == -1
+        #error You must set Z_PROBE_PIN to a valid pin if you enable Z_PROBE_ENDSTOP
+      #endif
+// 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
     /**
      * Check if Probe_Offset * Grid Points is greater than Probing Range
      */
@@ -209,9 +242,9 @@
    */
   #ifdef DUAL_X_CARRIAGE
     #if EXTRUDERS == 1 || defined(COREXY) \
-        || !defined(X2_ENABLE_PIN) || !defined(X2_STEP_PIN) || !defined(X2_DIR_PIN) \
+        || !HAS_X2_ENABLE || !HAS_X2_STEP || !HAS_X2_DIR \
         || !defined(X2_HOME_POS) || !defined(X2_MIN_POS) || !defined(X2_MAX_POS) \
-        || !defined(X_MAX_PIN) || X_MAX_PIN < 0
+        || !HAS_X_MAX
       #error Missing or invalid definitions for DUAL_X_CARRIAGE mode.
     #endif
     #if X_HOME_DIR != -1 || X2_HOME_DIR != 1
@@ -234,6 +267,10 @@
     #endif
   #endif
 
+  #if HAS_FAN && CONTROLLERFAN_PIN == FAN_PIN
+    #error You cannot set CONTROLLERFAN_PIN equal to FAN_PIN
+  #endif
+
   /**
    * Test required HEATER defines
    */
@@ -254,4 +291,11 @@
     #error HEATER_0_PIN not defined for this board
   #endif
 
+  /**
+   * Warnings for old configurations
+   */
+  #ifdef X_HOME_RETRACT_MM
+    #error [XYZ]_HOME_RETRACT_MM settings have been renamed [XYZ]_HOME_BUMP_MM
+  #endif
+
 #endif //SANITYCHECK_H
diff --git a/Marlin/Servo.h b/Marlin/Servo.h
index bbdf6bf0ab0..682a3b37981 100644
--- a/Marlin/Servo.h
+++ b/Marlin/Servo.h
@@ -123,7 +123,7 @@ class Servo {
     int read();                        // returns current pulse width as an angle between 0 and 180 degrees
     int readMicroseconds();            // returns current pulse width in microseconds for this servo (was read_us() in first release)
     bool attached();                   // return true if this servo is attached, otherwise false
-    #if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
+    #if defined(ENABLE_AUTO_BED_LEVELING) && PROBE_SERVO_DEACTIVATION_DELAY > 0
       int pin;                           // store the hardware pin of the servo
     #endif
   private:
diff --git a/Marlin/boards.h b/Marlin/boards.h
index a8c980097d0..8a60f011d57 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/cardreader.cpp b/Marlin/cardreader.cpp
index fae6c1be690..877b72b9277 100644
--- a/Marlin/cardreader.cpp
+++ b/Marlin/cardreader.cpp
@@ -249,7 +249,7 @@ void CardReader::openFile(char* name, bool read, bool replace_current/*=true*/)
         if (!myDir.open(curDir, subdirname, O_READ)) {
           SERIAL_PROTOCOLPGM(MSG_SD_OPEN_FILE_FAIL);
           SERIAL_PROTOCOL(subdirname);
-          SERIAL_PROTOCOLLNPGM(".");
+          SERIAL_PROTOCOLCHAR('.');
           return;
         }
         else {
@@ -287,14 +287,14 @@ void CardReader::openFile(char* name, bool read, bool replace_current/*=true*/)
     else {
       SERIAL_PROTOCOLPGM(MSG_SD_OPEN_FILE_FAIL);
       SERIAL_PROTOCOL(fname);
-      SERIAL_PROTOCOLLNPGM(".");
+      SERIAL_PROTOCOLCHAR('.');
     }
   }
   else { //write
     if (!file.open(curDir, fname, O_CREAT | O_APPEND | O_WRITE | O_TRUNC)) {
       SERIAL_PROTOCOLPGM(MSG_SD_OPEN_FILE_FAIL);
       SERIAL_PROTOCOL(fname);
-      SERIAL_PROTOCOLLNPGM(".");
+      SERIAL_PROTOCOLCHAR('.');
     }
     else {
       saving = true;
@@ -330,7 +330,7 @@ void CardReader::removeFile(char* name) {
         if (!myDir.open(curDir, subdirname, O_READ)) {
           SERIAL_PROTOCOLPGM("open failed, File: ");
           SERIAL_PROTOCOL(subdirname);
-          SERIAL_PROTOCOLLNPGM(".");
+          SERIAL_PROTOCOLCHAR('.');
           return;
         }
         else {
@@ -360,7 +360,7 @@ void CardReader::removeFile(char* name) {
   else {
     SERIAL_PROTOCOLPGM("Deletion failed, File: ");
     SERIAL_PROTOCOL(fname);
-    SERIAL_PROTOCOLLNPGM(".");
+    SERIAL_PROTOCOLCHAR('.');
   }
 }
 
@@ -368,7 +368,7 @@ void CardReader::getStatus() {
   if (cardOK) {
     SERIAL_PROTOCOLPGM(MSG_SD_PRINTING_BYTE);
     SERIAL_PROTOCOL(sdpos);
-    SERIAL_PROTOCOLPGM("/");
+    SERIAL_PROTOCOLCHAR('/');
     SERIAL_PROTOCOLLN(filesize);
   }
   else {
diff --git a/Marlin/configurator/config/Configuration.h b/Marlin/configurator/config/Configuration.h
index 7868e8e815e..5b0960fdd42 100644
--- a/Marlin/configurator/config/Configuration.h
+++ b/Marlin/configurator/config/Configuration.h
@@ -412,7 +412,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 // #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
 // #define MESH_BED_LEVELING    // Enable mesh bed leveling
 
-#if defined(MESH_BED_LEVELING)
+#ifdef MESH_BED_LEVELING
   #define MESH_MIN_X 10
   #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
   #define MESH_MIN_Y 10
@@ -519,6 +519,20 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 
   #endif
 
+  // 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 separate 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. 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_ENDSTOP
+
 #endif // ENABLE_AUTO_BED_LEVELING
 
 
@@ -539,8 +553,10 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 
 // @section movement
 
-//// MOVEMENT SETTINGS
-#define NUM_AXIS 4 // The axis order in all axis related arrays is X, Y, Z, E
+/**
+ * MOVEMENT SETTINGS
+ */
+
 #define HOMING_FEEDRATE {50*60, 50*60, 4*60, 0}  // set the homing speeds (mm/min)
 
 // default settings
diff --git a/Marlin/configurator/config/Configuration_adv.h b/Marlin/configurator/config/Configuration_adv.h
index 21b30580c8f..8acdeb969cc 100644
--- a/Marlin/configurator/config/Configuration_adv.h
+++ b/Marlin/configurator/config/Configuration_adv.h
@@ -189,9 +189,9 @@
 // @section homing
 
 //homing hits the endstop, then retracts by this distance, before it tries to slowly bump again:
-#define X_HOME_RETRACT_MM 5
-#define Y_HOME_RETRACT_MM 5
-#define Z_HOME_RETRACT_MM 2
+#define X_HOME_BUMP_MM 5
+#define Y_HOME_BUMP_MM 5
+#define Z_HOME_BUMP_MM 2
 #define HOMING_BUMP_DIVISOR {10, 10, 20}  // Re-Bump Speed Divisor (Divides the Homing Feedrate)
 //#define QUICK_HOME  //if this is defined, if both x and y are to be homed, a diagonal move will be performed initially.
 
diff --git a/Marlin/dogm_lcd_implementation.h b/Marlin/dogm_lcd_implementation.h
index 63e99bd3aa9..c057e56425c 100644
--- a/Marlin/dogm_lcd_implementation.h
+++ b/Marlin/dogm_lcd_implementation.h
@@ -300,7 +300,7 @@ static void lcd_implementation_status_screen() {
   // Fan
   lcd_setFont(FONT_STATUSMENU);
   u8g.setPrintPos(104,27);
-  #if defined(FAN_PIN) && FAN_PIN > -1
+  #if HAS_FAN
     int per = ((fanSpeed + 1) * 100) / 256;
     if (per) {
 
diff --git a/Marlin/example_configurations/Felix/Configuration.h b/Marlin/example_configurations/Felix/Configuration.h
index 1cc50246a80..72d716989be 100644
--- a/Marlin/example_configurations/Felix/Configuration.h
+++ b/Marlin/example_configurations/Felix/Configuration.h
@@ -364,7 +364,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 // #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
 // #define MESH_BED_LEVELING    // Enable mesh bed leveling
 
-#if defined(MESH_BED_LEVELING)
+#ifdef MESH_BED_LEVELING
   #define MESH_MIN_X 10
   #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
   #define MESH_MIN_Y 10
@@ -469,6 +469,20 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 
   #endif
 
+  // 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 separate 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. 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_ENDSTOP
+
 #endif // ENABLE_AUTO_BED_LEVELING
 
 
@@ -485,8 +499,10 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
   //#define MANUAL_Z_HOME_POS 402 // For delta: Distance between nozzle and print surface after homing.
 #endif
 
-//// MOVEMENT SETTINGS
-#define NUM_AXIS 4 // The axis order in all axis related arrays is X, Y, Z, E
+/**
+ * MOVEMENT SETTINGS
+ */
+
 #define HOMING_FEEDRATE {50*60, 50*60, 4*60, 0}  // set the homing speeds (mm/min)
 
 // default settings
diff --git a/Marlin/example_configurations/Felix/Configuration_DUAL.h b/Marlin/example_configurations/Felix/Configuration_DUAL.h
index 956e2dc472f..e6683b4e811 100644
--- a/Marlin/example_configurations/Felix/Configuration_DUAL.h
+++ b/Marlin/example_configurations/Felix/Configuration_DUAL.h
@@ -364,7 +364,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 // #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
 // #define MESH_BED_LEVELING    // Enable mesh bed leveling
 
-#if defined(MESH_BED_LEVELING)
+#ifdef MESH_BED_LEVELING
   #define MESH_MIN_X 10
   #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
   #define MESH_MIN_Y 10
@@ -469,6 +469,20 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 
   #endif
 
+  // 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 separate 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. 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_ENDSTOP
+
 #endif // ENABLE_AUTO_BED_LEVELING
 
 
@@ -485,8 +499,10 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
   //#define MANUAL_Z_HOME_POS 402 // For delta: Distance between nozzle and print surface after homing.
 #endif
 
-//// MOVEMENT SETTINGS
-#define NUM_AXIS 4 // The axis order in all axis related arrays is X, Y, Z, E
+/**
+ * MOVEMENT SETTINGS
+ */
+
 #define HOMING_FEEDRATE {50*60, 50*60, 4*60, 0}  // set the homing speeds (mm/min)
 
 // default settings
diff --git a/Marlin/example_configurations/Felix/Configuration_adv.h b/Marlin/example_configurations/Felix/Configuration_adv.h
index 9bbd515caf9..d60eb529630 100644
--- a/Marlin/example_configurations/Felix/Configuration_adv.h
+++ b/Marlin/example_configurations/Felix/Configuration_adv.h
@@ -175,9 +175,9 @@
 #endif //DUAL_X_CARRIAGE
 
 //homing hits the endstop, then retracts by this distance, before it tries to slowly bump again:
-#define X_HOME_RETRACT_MM 5
-#define Y_HOME_RETRACT_MM 5
-#define Z_HOME_RETRACT_MM 3
+#define X_HOME_BUMP_MM 5
+#define Y_HOME_BUMP_MM 5
+#define Z_HOME_BUMP_MM 3
 #define HOMING_BUMP_DIVISOR {10, 10, 20}  // Re-Bump Speed Divisor (Divides the Homing Feedrate)
 //#define QUICK_HOME  //if this is defined, if both x and y are to be homed, a diagonal move will be performed initially.
 
diff --git a/Marlin/example_configurations/Hephestos/Configuration.h b/Marlin/example_configurations/Hephestos/Configuration.h
index 594295a3e1f..ac101be4d36 100644
--- a/Marlin/example_configurations/Hephestos/Configuration.h
+++ b/Marlin/example_configurations/Hephestos/Configuration.h
@@ -387,7 +387,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 // #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
 // #define MESH_BED_LEVELING    // Enable mesh bed leveling
 
-#if defined(MESH_BED_LEVELING)
+#ifdef MESH_BED_LEVELING
   #define MESH_MIN_X 10
   #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
   #define MESH_MIN_Y 10
@@ -492,6 +492,20 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 
   #endif
 
+  // 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 separate 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. 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_ENDSTOP
+
 #endif // ENABLE_AUTO_BED_LEVELING
 
 
@@ -508,8 +522,10 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
   //#define MANUAL_Z_HOME_POS 402 // For delta: Distance between nozzle and print surface after homing.
 #endif
 
-//// MOVEMENT SETTINGS
-#define NUM_AXIS 4 // The axis order in all axis related arrays is X, Y, Z, E
+/**
+ * MOVEMENT SETTINGS
+ */
+
 #define HOMING_FEEDRATE {2000, 2000, 150, 0}  // set the homing speeds (mm/min)
 
 // default settings
diff --git a/Marlin/example_configurations/Hephestos/Configuration_adv.h b/Marlin/example_configurations/Hephestos/Configuration_adv.h
index bac3b571a73..0529df9bebd 100644
--- a/Marlin/example_configurations/Hephestos/Configuration_adv.h
+++ b/Marlin/example_configurations/Hephestos/Configuration_adv.h
@@ -175,9 +175,9 @@
 #endif //DUAL_X_CARRIAGE
 
 //homing hits the endstop, then retracts by this distance, before it tries to slowly bump again:
-#define X_HOME_RETRACT_MM 5
-#define Y_HOME_RETRACT_MM 5
-#define Z_HOME_RETRACT_MM 2
+#define X_HOME_BUMP_MM 5
+#define Y_HOME_BUMP_MM 5
+#define Z_HOME_BUMP_MM 2
 #define HOMING_BUMP_DIVISOR {10, 10, 20}  // Re-Bump Speed Divisor (Divides the Homing Feedrate)
 //#define QUICK_HOME  //if this is defined, if both x and y are to be homed, a diagonal move will be performed initially.
 
diff --git a/Marlin/example_configurations/K8200/Configuration.h b/Marlin/example_configurations/K8200/Configuration.h
index c17e419c7bf..6d743e9379b 100644
--- a/Marlin/example_configurations/K8200/Configuration.h
+++ b/Marlin/example_configurations/K8200/Configuration.h
@@ -392,7 +392,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 // #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
 // #define MESH_BED_LEVELING    // Enable mesh bed leveling
 
-#if defined(MESH_BED_LEVELING)
+#ifdef MESH_BED_LEVELING
   #define MESH_MIN_X 10
   #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
   #define MESH_MIN_Y 10
@@ -497,6 +497,20 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 
   #endif
 
+  // 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 separate 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. 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_ENDSTOP
+
 #endif // ENABLE_AUTO_BED_LEVELING
 
 
@@ -513,8 +527,10 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
   //#define MANUAL_Z_HOME_POS 402 // For delta: Distance between nozzle and print surface after homing.
 #endif
 
-//// MOVEMENT SETTINGS
-#define NUM_AXIS 4 // The axis order in all axis related arrays is X, Y, Z, E
+/**
+ * MOVEMENT SETTINGS
+ */
+
 #define HOMING_FEEDRATE {50*60, 50*60, 4*60, 0}  // set the homing speeds (mm/min)
 
 // default settings
diff --git a/Marlin/example_configurations/K8200/Configuration_adv.h b/Marlin/example_configurations/K8200/Configuration_adv.h
index 9bbd515caf9..d60eb529630 100644
--- a/Marlin/example_configurations/K8200/Configuration_adv.h
+++ b/Marlin/example_configurations/K8200/Configuration_adv.h
@@ -175,9 +175,9 @@
 #endif //DUAL_X_CARRIAGE
 
 //homing hits the endstop, then retracts by this distance, before it tries to slowly bump again:
-#define X_HOME_RETRACT_MM 5
-#define Y_HOME_RETRACT_MM 5
-#define Z_HOME_RETRACT_MM 3
+#define X_HOME_BUMP_MM 5
+#define Y_HOME_BUMP_MM 5
+#define Z_HOME_BUMP_MM 3
 #define HOMING_BUMP_DIVISOR {10, 10, 20}  // Re-Bump Speed Divisor (Divides the Homing Feedrate)
 //#define QUICK_HOME  //if this is defined, if both x and y are to be homed, a diagonal move will be performed initially.
 
diff --git a/Marlin/example_configurations/SCARA/Configuration.h b/Marlin/example_configurations/SCARA/Configuration.h
index 96fef00cfe5..f126ab99bc1 100644
--- a/Marlin/example_configurations/SCARA/Configuration.h
+++ b/Marlin/example_configurations/SCARA/Configuration.h
@@ -416,7 +416,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 // #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
 // #define MESH_BED_LEVELING    // Enable mesh bed leveling
 
-#if defined(MESH_BED_LEVELING)
+#ifdef MESH_BED_LEVELING
   #define MESH_MIN_X 10
   #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
   #define MESH_MIN_Y 10
@@ -521,6 +521,20 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 
   #endif
 
+  // 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 separate 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. 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_ENDSTOP
+
 #endif // ENABLE_AUTO_BED_LEVELING
 
 
@@ -537,8 +551,10 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
   #define MANUAL_Z_HOME_POS 0.1  // Distance between nozzle and print surface after homing.
 #endif
 
-//// MOVEMENT SETTINGS
-#define NUM_AXIS 4 // The axis order in all axis related arrays is X, Y, Z, E
+/**
+ * MOVEMENT SETTINGS
+ */
+
 #define HOMING_FEEDRATE {40*60, 40*60, 10*60, 0}  // set the homing speeds (mm/min)
 
 // default settings
diff --git a/Marlin/example_configurations/SCARA/Configuration_adv.h b/Marlin/example_configurations/SCARA/Configuration_adv.h
index 7b00532e49e..5145b166853 100644
--- a/Marlin/example_configurations/SCARA/Configuration_adv.h
+++ b/Marlin/example_configurations/SCARA/Configuration_adv.h
@@ -175,9 +175,9 @@
 #endif //DUAL_X_CARRIAGE
 
 //homing hits the endstop, then retracts by this distance, before it tries to slowly bump again:
-#define X_HOME_RETRACT_MM 3
-#define Y_HOME_RETRACT_MM 3
-#define Z_HOME_RETRACT_MM 3
+#define X_HOME_BUMP_MM 3
+#define Y_HOME_BUMP_MM 3
+#define Z_HOME_BUMP_MM 3
 #define HOMING_BUMP_DIVISOR {10, 10, 20}  // Re-Bump Speed Divisor (Divides the Homing Feedrate)
 //#define QUICK_HOME  //if this is defined, if both x and y are to be homed, a diagonal move will be performed initially.
 
diff --git a/Marlin/example_configurations/WITBOX/Configuration.h b/Marlin/example_configurations/WITBOX/Configuration.h
index 190270e853f..265f73fce15 100644
--- a/Marlin/example_configurations/WITBOX/Configuration.h
+++ b/Marlin/example_configurations/WITBOX/Configuration.h
@@ -386,7 +386,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 // #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
 // #define MESH_BED_LEVELING    // Enable mesh bed leveling
 
-#if defined(MESH_BED_LEVELING)
+#ifdef MESH_BED_LEVELING
   #define MESH_MIN_X 10
   #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
   #define MESH_MIN_Y 10
@@ -491,6 +491,20 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 
   #endif
 
+  // 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 separate 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. 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_ENDSTOP
+
 #endif // ENABLE_AUTO_BED_LEVELING
 
 
@@ -507,8 +521,10 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
   //#define MANUAL_Z_HOME_POS 402 // For delta: Distance between nozzle and print surface after homing.
 #endif
 
-//// MOVEMENT SETTINGS
-#define NUM_AXIS 4 // The axis order in all axis related arrays is X, Y, Z, E
+/**
+ * MOVEMENT SETTINGS
+ */
+
 #define HOMING_FEEDRATE {120*60, 120*60, 7.2*60, 0}  // set the homing speeds (mm/min)
 
 // default settings
diff --git a/Marlin/example_configurations/WITBOX/Configuration_adv.h b/Marlin/example_configurations/WITBOX/Configuration_adv.h
index 965ecbf40b0..ab480dd6351 100644
--- a/Marlin/example_configurations/WITBOX/Configuration_adv.h
+++ b/Marlin/example_configurations/WITBOX/Configuration_adv.h
@@ -175,9 +175,9 @@
 #endif //DUAL_X_CARRIAGE
 
 //homing hits the endstop, then retracts by this distance, before it tries to slowly bump again:
-#define X_HOME_RETRACT_MM 5
-#define Y_HOME_RETRACT_MM 5
-#define Z_HOME_RETRACT_MM 2
+#define X_HOME_BUMP_MM 5
+#define Y_HOME_BUMP_MM 5
+#define Z_HOME_BUMP_MM 2
 #define HOMING_BUMP_DIVISOR {10, 10, 20}  // Re-Bump Speed Divisor (Divides the Homing Feedrate)
 //#define QUICK_HOME  //if this is defined, if both x and y are to be homed, a diagonal move will be performed initially.
 
diff --git a/Marlin/example_configurations/delta/generic/Configuration.h b/Marlin/example_configurations/delta/generic/Configuration.h
index 4aeb5d55f47..3218d2d9d75 100644
--- a/Marlin/example_configurations/delta/generic/Configuration.h
+++ b/Marlin/example_configurations/delta/generic/Configuration.h
@@ -414,7 +414,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 // #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
 // #define MESH_BED_LEVELING    // Enable mesh bed leveling
 
-#if defined(MESH_BED_LEVELING)
+#ifdef MESH_BED_LEVELING
   #define MESH_MIN_X 10
   #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
   #define MESH_MIN_Y 10
@@ -507,10 +507,10 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
     #define Z_PROBE_ALLEN_KEY_DEPLOY_Y DELTA_PRINTABLE_RADIUS
     #define Z_PROBE_ALLEN_KEY_DEPLOY_Z 100
     
-    #define Z_PROBE_ALLEN_KEY_RETRACT_X     -64
-    #define Z_PROBE_ALLEN_KEY_RETRACT_Y     56
-    #define Z_PROBE_ALLEN_KEY_RETRACT_Z     23
-    #define Z_PROBE_ALLEN_KEY_RETRACT_DEPTH 20
+    #define Z_PROBE_ALLEN_KEY_STOW_X     -64
+    #define Z_PROBE_ALLEN_KEY_STOW_Y     56
+    #define Z_PROBE_ALLEN_KEY_STOW_Z     23
+    #define Z_PROBE_ALLEN_KEY_STOW_DEPTH 20
   #endif
   
   //If defined, the Probe servo will be turned on only during movement and then turned off to avoid jerk
@@ -537,6 +537,20 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 
   #endif
 
+  // 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 separate 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. 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_ENDSTOP
+
 #endif // ENABLE_AUTO_BED_LEVELING
 
 
@@ -552,8 +566,10 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
   #define MANUAL_Z_HOME_POS 250 // For delta: Distance between nozzle and print surface after homing.
 #endif
 
-//// MOVEMENT SETTINGS
-#define NUM_AXIS 4 // The axis order in all axis related arrays is X, Y, Z, E
+/**
+ * MOVEMENT SETTINGS
+ */
+
 
 // delta homing speeds must be the same on xyz
 #define HOMING_FEEDRATE {200*60, 200*60, 200*60, 0}  // set the homing speeds (mm/min)
diff --git a/Marlin/example_configurations/delta/generic/Configuration_adv.h b/Marlin/example_configurations/delta/generic/Configuration_adv.h
index abecacec227..501cd814d22 100644
--- a/Marlin/example_configurations/delta/generic/Configuration_adv.h
+++ b/Marlin/example_configurations/delta/generic/Configuration_adv.h
@@ -175,9 +175,9 @@
 #endif //DUAL_X_CARRIAGE
 
 //homing hits the endstop, then retracts by this distance, before it tries to slowly bump again:
-#define X_HOME_RETRACT_MM 5
-#define Y_HOME_RETRACT_MM 5
-#define Z_HOME_RETRACT_MM 5 // deltas need the same for all three axis
+#define X_HOME_BUMP_MM 5
+#define Y_HOME_BUMP_MM 5
+#define Z_HOME_BUMP_MM 5 // deltas need the same for all three axis
 #define HOMING_BUMP_DIVISOR {10, 10, 20}  // Re-Bump Speed Divisor (Divides the Homing Feedrate)
 //#define QUICK_HOME  //if this is defined, if both x and y are to be homed, a diagonal move will be performed initially.
 
diff --git a/Marlin/example_configurations/delta/kossel_mini/Configuration.h b/Marlin/example_configurations/delta/kossel_mini/Configuration.h
index edb7ff5261a..489dca6d58a 100644
--- a/Marlin/example_configurations/delta/kossel_mini/Configuration.h
+++ b/Marlin/example_configurations/delta/kossel_mini/Configuration.h
@@ -414,7 +414,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 // #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
 // #define MESH_BED_LEVELING    // Enable mesh bed leveling
 
-#if defined(MESH_BED_LEVELING)
+#ifdef MESH_BED_LEVELING
   #define MESH_MIN_X 10
   #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
   #define MESH_MIN_Y 10
@@ -511,10 +511,10 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
     #define Z_PROBE_ALLEN_KEY_DEPLOY_Y DELTA_PRINTABLE_RADIUS
     #define Z_PROBE_ALLEN_KEY_DEPLOY_Z 100
     
-    #define Z_PROBE_ALLEN_KEY_RETRACT_X     -64
-    #define Z_PROBE_ALLEN_KEY_RETRACT_Y     56
-    #define Z_PROBE_ALLEN_KEY_RETRACT_Z     23
-    #define Z_PROBE_ALLEN_KEY_RETRACT_DEPTH 20
+    #define Z_PROBE_ALLEN_KEY_STOW_X     -64
+    #define Z_PROBE_ALLEN_KEY_STOW_Y     56
+    #define Z_PROBE_ALLEN_KEY_STOW_Z     23
+    #define Z_PROBE_ALLEN_KEY_STOW_DEPTH 20
   #endif
   
   //If defined, the Probe servo will be turned on only during movement and then turned off to avoid jerk
@@ -541,6 +541,20 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 
   #endif
 
+  // 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 separate 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. 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_ENDSTOP
+
 #endif // ENABLE_AUTO_BED_LEVELING
 
 
@@ -556,8 +570,10 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
   #define MANUAL_Z_HOME_POS 250 // For delta: Distance between nozzle and print surface after homing.
 #endif
 
-//// MOVEMENT SETTINGS
-#define NUM_AXIS 4 // The axis order in all axis related arrays is X, Y, Z, E
+/**
+ * MOVEMENT SETTINGS
+ */
+
 
 // delta homing speeds must be the same on xyz
 #define HOMING_FEEDRATE {200*60, 200*60, 200*60, 0}  // set the homing speeds (mm/min)
diff --git a/Marlin/example_configurations/delta/kossel_mini/Configuration_adv.h b/Marlin/example_configurations/delta/kossel_mini/Configuration_adv.h
index b255000bce9..c402064bfeb 100644
--- a/Marlin/example_configurations/delta/kossel_mini/Configuration_adv.h
+++ b/Marlin/example_configurations/delta/kossel_mini/Configuration_adv.h
@@ -175,9 +175,9 @@
 #endif //DUAL_X_CARRIAGE
 
 //homing hits the endstop, then retracts by this distance, before it tries to slowly bump again:
-#define X_HOME_RETRACT_MM 5
-#define Y_HOME_RETRACT_MM 5
-#define Z_HOME_RETRACT_MM 5 // deltas need the same for all three axis
+#define X_HOME_BUMP_MM 5
+#define Y_HOME_BUMP_MM 5
+#define Z_HOME_BUMP_MM 5 // deltas need the same for all three axis
 #define HOMING_BUMP_DIVISOR {10, 10, 20}  // Re-Bump Speed Divisor (Divides the Homing Feedrate)
 //#define QUICK_HOME  //if this is defined, if both x and y are to be homed, a diagonal move will be performed initially.
 
diff --git a/Marlin/example_configurations/makibox/Configuration.h b/Marlin/example_configurations/makibox/Configuration.h
index a3e8fd08857..45350960f8c 100644
--- a/Marlin/example_configurations/makibox/Configuration.h
+++ b/Marlin/example_configurations/makibox/Configuration.h
@@ -384,7 +384,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 // #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
 // #define MESH_BED_LEVELING    // Enable mesh bed leveling
 
-#if defined(MESH_BED_LEVELING)
+#ifdef MESH_BED_LEVELING
   #define MESH_MIN_X 10
   #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
   #define MESH_MIN_Y 10
@@ -489,6 +489,20 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 
   #endif
 
+  // 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 separate 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. 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_ENDSTOP
+
 #endif // ENABLE_AUTO_BED_LEVELING
 
 
@@ -505,8 +519,10 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
   //#define MANUAL_Z_HOME_POS 402 // For delta: Distance between nozzle and print surface after homing.
 #endif
 
-//// MOVEMENT SETTINGS
-#define NUM_AXIS 4 // The axis order in all axis related arrays is X, Y, Z, E
+/**
+ * MOVEMENT SETTINGS
+ */
+
 #define HOMING_FEEDRATE {1500, 1500, 120, 0}  // set the homing speeds (mm/min)   ***** MakiBox A6 *****
 
 // default settings
diff --git a/Marlin/example_configurations/makibox/Configuration_adv.h b/Marlin/example_configurations/makibox/Configuration_adv.h
index 5e0e6ef4d82..3db90efcff8 100644
--- a/Marlin/example_configurations/makibox/Configuration_adv.h
+++ b/Marlin/example_configurations/makibox/Configuration_adv.h
@@ -175,9 +175,9 @@
 #endif //DUAL_X_CARRIAGE
 
 //homing hits the endstop, then retracts by this distance, before it tries to slowly bump again:
-#define X_HOME_RETRACT_MM 5
-#define Y_HOME_RETRACT_MM 5
-#define Z_HOME_RETRACT_MM 2
+#define X_HOME_BUMP_MM 5
+#define Y_HOME_BUMP_MM 5
+#define Z_HOME_BUMP_MM 2
 #define HOMING_BUMP_DIVISOR {10, 10, 20}  // Re-Bump Speed Divisor (Divides the Homing Feedrate)
 //#define QUICK_HOME  //if this is defined, if both x and y are to be homed, a diagonal move will be performed initially.
 
diff --git a/Marlin/example_configurations/tvrrug/Round2/Configuration.h b/Marlin/example_configurations/tvrrug/Round2/Configuration.h
index 7ffff335340..2c98a310fb4 100644
--- a/Marlin/example_configurations/tvrrug/Round2/Configuration.h
+++ b/Marlin/example_configurations/tvrrug/Round2/Configuration.h
@@ -386,7 +386,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 // #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
 // #define MESH_BED_LEVELING    // Enable mesh bed leveling
 
-#if defined(MESH_BED_LEVELING)
+#ifdef MESH_BED_LEVELING
   #define MESH_MIN_X 10
   #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
   #define MESH_MIN_Y 10
@@ -491,6 +491,20 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 
   #endif
 
+  // 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 separate 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. 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_ENDSTOP
+
 #endif // ENABLE_AUTO_BED_LEVELING
 
 
@@ -507,8 +521,10 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
   //#define MANUAL_Z_HOME_POS 402 // For delta: Distance between nozzle and print surface after homing.
 #endif
 
-//// MOVEMENT SETTINGS
-#define NUM_AXIS 4 // The axis order in all axis related arrays is X, Y, Z, E
+/**
+ * MOVEMENT SETTINGS
+ */
+
 #define HOMING_FEEDRATE {50*60, 50*60, 4*60, 0}  // set the homing speeds (mm/min)
 
 // default settings
diff --git a/Marlin/example_configurations/tvrrug/Round2/Configuration_adv.h b/Marlin/example_configurations/tvrrug/Round2/Configuration_adv.h
index bbf7dc00535..2ecf75951ac 100644
--- a/Marlin/example_configurations/tvrrug/Round2/Configuration_adv.h
+++ b/Marlin/example_configurations/tvrrug/Round2/Configuration_adv.h
@@ -175,9 +175,9 @@
 #endif //DUAL_X_CARRIAGE
 
 //homing hits the endstop, then retracts by this distance, before it tries to slowly bump again:
-#define X_HOME_RETRACT_MM 5
-#define Y_HOME_RETRACT_MM 5
-#define Z_HOME_RETRACT_MM 1
+#define X_HOME_BUMP_MM 5
+#define Y_HOME_BUMP_MM 5
+#define Z_HOME_BUMP_MM 1
 #define HOMING_BUMP_DIVISOR {10, 10, 20}  // Re-Bump Speed Divisor (Divides the Homing Feedrate)
 //#define QUICK_HOME  //if this is defined, if both x and y are to be homed, a diagonal move will be performed initially.
 
diff --git a/Marlin/fastio.h b/Marlin/fastio.h
index 3087e87f2a3..0e3b34d2551 100644
--- a/Marlin/fastio.h
+++ b/Marlin/fastio.h
@@ -91,7 +91,7 @@
 	added as necessary or if I feel like it- not a comprehensive list!
 */
 
-#if defined (__AVR_ATmega168__) || defined (__AVR_ATmega328__) || defined (__AVR_ATmega328P__)
+#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega328__) || defined(__AVR_ATmega328P__)
 // UART
 #define	RXD					DIO0
 #define	TXD					DIO1
@@ -426,7 +426,7 @@ pins
 #define PD7_PWM			NULL
 #endif	/*	_AVR_ATmega{168,328,328P}__ */
 
-#if defined (__AVR_ATmega644__) || defined (__AVR_ATmega644P__) || defined (__AVR_ATmega644PA__) || defined (__AVR_ATmega1284P__)
+#if defined(__AVR_ATmega644__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644PA__) || defined(__AVR_ATmega1284P__)
 // UART
 #define	RXD					DIO8
 #define	TXD					DIO9
@@ -929,7 +929,7 @@ pins
 #define PD7_PWM			OCR2A
 #endif	/*	_AVR_ATmega{644,644P,644PA}__ */
 
-#if defined (__AVR_ATmega1280__) || defined (__AVR_ATmega2560__)
+#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
 // UART
 #define	RXD					DIO0
 #define	TXD					DIO1
@@ -2024,7 +2024,7 @@ pins
 
 #endif
 
-#if defined (__AVR_AT90USB1287__) || defined (__AVR_AT90USB1286__) || defined (__AVR_AT90USB646__) || defined(__AVR_AT90USB647__)
+#if defined(__AVR_AT90USB1287__) || defined(__AVR_AT90USB1286__) || defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB647__)
 // SPI
 #define	SCK					DIO9
 #define	MISO				DIO11
@@ -3322,7 +3322,7 @@ Teensy   28 29 30 31 32 33 34 35 20 21 22 23 24 25 26 27 10 11 12 13 14 15 16 17
 #endif // __AVR_AT90usbxxx__
 
 
-#if defined (__AVR_ATmega1281__) || defined (__AVR_ATmega2561__)
+#if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
 // UART
 #define	RXD					DIO0
 #define	TXD					DIO1
diff --git a/Marlin/language.h b/Marlin/language.h
index 10ef445d837..3fe7afd2ede 100644
--- a/Marlin/language.h
+++ b/Marlin/language.h
@@ -36,18 +36,19 @@
   #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"
 
 #if MB(ULTIMAKER)|| MB(ULTIMAKER_OLD)|| MB(ULTIMAIN_2)
-  #undef FIRMWARE_URL
   #define MACHINE_NAME "Ultimaker"
   #define FIRMWARE_URL "http://firmware.ultimaker.com"
 #elif MB(RUMBA)
   #define MACHINE_NAME "Rumba"
 #elif MB(3DRAG)
   #define MACHINE_NAME "3Drag"
-  #undef FIRMWARE_URL
   #define FIRMWARE_URL "http://3dprint.elettronicain.it/"
 #elif MB(K8200)
   #define MACHINE_NAME "K8200"
@@ -55,23 +56,40 @@
   #define MACHINE_NAME "Makibox"
 #elif MB(SAV_MKI)
   #define MACHINE_NAME "SAV MkI"
-  #undef FIRMWARE_URL
   #define FIRMWARE_URL "https://github.com/fmalpartida/Marlin/tree/SAV-MkI-config"
 #elif MB(WITBOX)
   #define MACHINE_NAME "WITBOX"
-  #undef FIRMWARE_URL
   #define FIRMWARE_URL "http://www.bq.com/gb/downloads-witbox.html"
 #elif MB(HEPHESTOS)
   #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"
+#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"
+  #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
@@ -122,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)"
@@ -138,6 +156,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"
diff --git a/Marlin/mesh_bed_leveling.h b/Marlin/mesh_bed_leveling.h
index b6c4ed5b007..bf7275e5c34 100644
--- a/Marlin/mesh_bed_leveling.h
+++ b/Marlin/mesh_bed_leveling.h
@@ -1,6 +1,6 @@
 #include "Marlin.h"
 
-#if defined(MESH_BED_LEVELING)
+#ifdef MESH_BED_LEVELING
 
   #define MESH_X_DIST ((MESH_MAX_X - MESH_MIN_X)/(MESH_NUM_X_POINTS - 1))
   #define MESH_Y_DIST ((MESH_MAX_Y - MESH_MIN_Y)/(MESH_NUM_Y_POINTS - 1))
diff --git a/Marlin/pins.h b/Marlin/pins.h
index 3c75a73badc..e5af0af1797 100644
--- a/Marlin/pins.h
+++ b/Marlin/pins.h
@@ -187,6 +187,10 @@
   #define Z_MIN_PIN          -1
 #endif
 
+#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
+
 #ifdef DISABLE_XMAX_ENDSTOP
   #undef X_MAX_PIN
   #define X_MAX_PIN          -1
@@ -216,8 +220,11 @@
   #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, \
-                        HEATER_BED_PIN, FAN_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) \
                        }
diff --git a/Marlin/pins_AZTEEG_X3_PRO.h b/Marlin/pins_AZTEEG_X3_PRO.h
index 0cdc7160107..d76874a09ac 100644
--- a/Marlin/pins_AZTEEG_X3_PRO.h
+++ b/Marlin/pins_AZTEEG_X3_PRO.h
@@ -34,7 +34,12 @@
   #define Z_MAX_PIN          18
  #endif
 //
-
+ #ifdef Z_PROBE_ENDSTOP
+//#undef Z_MIN_PIN
+//#define Z_MIN_PIN        15
+  #define Z_PROBE_PIN      19
+ #endif
+//
 #define E2_STEP_PIN        23
 #define E2_DIR_PIN         25
 #define E2_ENABLE_PIN      40
diff --git a/Marlin/pins_RAMPS_13.h b/Marlin/pins_RAMPS_13.h
index 71287f68325..2708117d04d 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
@@ -61,7 +62,12 @@
   #define FILWIDTH_PIN        5
 #endif
 
-#if defined(FILAMENT_RUNOUT_SENSOR)
+#ifdef 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
+
+#ifdef 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
 #endif
diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index d98ef63d4df..dcfbe674cfe 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -58,7 +58,7 @@
 #include "ultralcd.h"
 #include "language.h"
 
-#if defined(MESH_BED_LEVELING)
+#ifdef MESH_BED_LEVELING
   #include "mesh_bed_leveling.h"
 #endif  // MESH_BED_LEVELING
 
@@ -67,7 +67,7 @@
 //===========================================================================
 
 unsigned long minsegmenttime;
-float max_feedrate[NUM_AXIS]; // set the max speeds
+float max_feedrate[NUM_AXIS]; // Max speeds in mm per minute
 float axis_steps_per_unit[NUM_AXIS];
 unsigned long max_acceleration_units_per_sq_second[NUM_AXIS]; // Use M201 to override by software
 float minimumfeedrate;
@@ -427,7 +427,7 @@ void check_axes_activity() {
     disable_e3();
   }
 
-  #if defined(FAN_PIN) && FAN_PIN > -1 // HAS_FAN
+  #if HAS_FAN
     #ifdef FAN_KICKSTART_TIME
       static unsigned long fan_kick_end;
       if (tail_fan_speed) {
@@ -447,17 +447,17 @@ void check_axes_activity() {
     #else
       analogWrite(FAN_PIN, tail_fan_speed);
     #endif //!FAN_SOFT_PWM
-  #endif //FAN_PIN > -1
+  #endif // HAS_FAN
 
   #ifdef AUTOTEMP
     getHighESpeed();
   #endif
 
   #ifdef BARICUDA
-    #if defined(HEATER_1_PIN) && HEATER_1_PIN > -1 // HAS_HEATER_1
+    #if HAS_HEATER_1
       analogWrite(HEATER_1_PIN,tail_valve_pressure);
     #endif
-    #if defined(HEATER_2_PIN) && HEATER_2_PIN > -1 // HAS_HEATER_2
+    #if HAS_HEATER_2
       analogWrite(HEATER_2_PIN,tail_e_to_p_pressure);
     #endif
   #endif
@@ -614,7 +614,7 @@ float junction_deviation = 0.1;
         #if EXTRUDERS > 1
           case 1:
             enable_e1();
-            g_uc_extruder_last_move[1] = BLOCK_BUFFER_SIZE*2;
+            g_uc_extruder_last_move[1] = BLOCK_BUFFER_SIZE * 2;
             if (g_uc_extruder_last_move[0] == 0) disable_e0();
             #if EXTRUDERS > 2
               if (g_uc_extruder_last_move[2] == 0) disable_e2();
@@ -626,7 +626,7 @@ float junction_deviation = 0.1;
           #if EXTRUDERS > 2
             case 2:
               enable_e2();
-              g_uc_extruder_last_move[2] = BLOCK_BUFFER_SIZE*2;
+              g_uc_extruder_last_move[2] = BLOCK_BUFFER_SIZE * 2;
               if (g_uc_extruder_last_move[0] == 0) disable_e0();
               if (g_uc_extruder_last_move[1] == 0) disable_e1();
               #if EXTRUDERS > 3
@@ -636,7 +636,7 @@ float junction_deviation = 0.1;
             #if EXTRUDERS > 3
               case 3:
                 enable_e3();
-                g_uc_extruder_last_move[3] = BLOCK_BUFFER_SIZE*2;
+                g_uc_extruder_last_move[3] = BLOCK_BUFFER_SIZE * 2;
                 if (g_uc_extruder_last_move[0] == 0) disable_e0();
                 if (g_uc_extruder_last_move[1] == 0) disable_e1();
                 if (g_uc_extruder_last_move[2] == 0) disable_e2();
diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index a7dba4659ec..ba4a4e9671d 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; // 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;
@@ -85,33 +86,37 @@ static volatile bool endstop_z_hit = false;
   int motor_current_setting[3] = DEFAULT_PWM_MOTOR_CURRENT;
 #endif
 
-#if defined(X_MIN_PIN) && X_MIN_PIN >= 0
+#if HAS_X_MIN
   static bool old_x_min_endstop = false;
 #endif
-#if defined(X_MAX_PIN) && X_MAX_PIN >= 0
+#if HAS_X_MAX
   static bool old_x_max_endstop = false;
 #endif
-#if defined(Y_MIN_PIN) && Y_MIN_PIN >= 0
+#if HAS_Y_MIN
   static bool old_y_min_endstop = false;
 #endif
-#if defined(Y_MAX_PIN) && Y_MAX_PIN >= 0
+#if HAS_Y_MAX
   static bool old_y_max_endstop = false;
 #endif
-#if defined(Z_MIN_PIN) && Z_MIN_PIN >= 0
+#if HAS_Z_MIN
   static bool old_z_min_endstop = false;
 #endif
-#if defined(Z_MAX_PIN) && Z_MAX_PIN >= 0
+#if HAS_Z_MAX
   static bool old_z_max_endstop = false;
 #endif
 #ifdef Z_DUAL_ENDSTOPS
-  #if defined(Z2_MIN_PIN) && Z2_MIN_PIN >= 0
+  #if HAS_Z2_MIN
     static bool old_z2_min_endstop = false;
   #endif
-  #if defined(Z2_MAX_PIN) && Z2_MAX_PIN >= 0
+  #if HAS_Z2_MAX
     static bool old_z2_max_endstop = false;
   #endif
 #endif
 
+#ifdef Z_PROBE_ENDSTOP // No need to check for valid pin, SanityCheck.h already does this.
+static bool old_z_probe_endstop = false;
+#endif
+
 static bool check_endstops = true;
 
 volatile long count_position[NUM_AXIS] = { 0 };
@@ -254,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; // #ifdef endstop_z_probe_hit = to save space if needed.
 }
 
 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) { // #ifdef || endstop_z_probe_hit to save space if needed.
     SERIAL_ECHO_START;
     SERIAL_ECHOPGM(MSG_ENDSTOPS_HIT);
     if (endstop_x_hit) {
@@ -273,6 +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();
@@ -472,7 +483,7 @@ ISR(TIMER1_COMPA_vect) {
               if ((current_block->active_extruder == 0 && X_HOME_DIR == -1) || (current_block->active_extruder != 0 && X2_HOME_DIR == -1))
             #endif          
               {
-                #if defined(X_MIN_PIN) && X_MIN_PIN >= 0
+                #if HAS_X_MIN
                   UPDATE_ENDSTOP(x, X, min, MIN);
                 #endif
               }
@@ -483,7 +494,7 @@ ISR(TIMER1_COMPA_vect) {
               if ((current_block->active_extruder == 0 && X_HOME_DIR == 1) || (current_block->active_extruder != 0 && X2_HOME_DIR == 1))
             #endif
               {
-                #if defined(X_MAX_PIN) && X_MAX_PIN >= 0
+                #if HAS_X_MAX
                   UPDATE_ENDSTOP(x, X, max, MAX);
                 #endif
               }
@@ -498,12 +509,12 @@ ISR(TIMER1_COMPA_vect) {
           if (TEST(out_bits, Y_AXIS))   // -direction
       #endif
           { // -direction
-            #if defined(Y_MIN_PIN) && Y_MIN_PIN >= 0
+            #if HAS_Y_MIN
               UPDATE_ENDSTOP(y, Y, min, MIN);
             #endif
           }
           else { // +direction
-            #if defined(Y_MAX_PIN) && Y_MAX_PIN >= 0
+            #if HAS_Y_MAX
               UPDATE_ENDSTOP(y, Y, max, MAX);
             #endif
           }
@@ -519,13 +530,13 @@ ISR(TIMER1_COMPA_vect) {
 
       if (check_endstops) {
 
-        #if defined(Z_MIN_PIN) && Z_MIN_PIN >= 0
+        #if HAS_Z_MIN
 
           #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
+                  #if HAS_Z2_MIN
                     READ(Z2_MIN_PIN) != Z2_MIN_ENDSTOP_INVERTING
                   #else
                     z_min_endstop
@@ -551,6 +562,19 @@ ISR(TIMER1_COMPA_vect) {
 
         #endif // Z_MIN_PIN
 
+        #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)
+          {
+        	  endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
+        	  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;
+        #endif
+        
       } // check_endstops
 
     }
@@ -561,13 +585,13 @@ ISR(TIMER1_COMPA_vect) {
 
       if (check_endstops) {
 
-        #if defined(Z_MAX_PIN) && Z_MAX_PIN >= 0
+        #if HAS_Z_MAX
 
           #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
+                  #if HAS_Z2_MAX
                     READ(Z2_MAX_PIN) != Z2_MAX_ENDSTOP_INVERTING
                   #else
                     z_max_endstop
@@ -596,6 +620,18 @@ ISR(TIMER1_COMPA_vect) {
           #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);
+          if(z_probe_endstop && old_z_probe_endstop)
+          {
+        	  endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
+        	  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;
+        #endif
 
       } // check_endstops
 
@@ -679,7 +715,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) {
@@ -835,133 +871,140 @@ void st_init() {
   #endif
   
   // Initialize Dir Pins
-  #if defined(X_DIR_PIN) && X_DIR_PIN >= 0
+  #if HAS_X_DIR
     X_DIR_INIT;
   #endif
-  #if defined(X2_DIR_PIN) && X2_DIR_PIN >= 0
+  #if HAS_X2_DIR
     X2_DIR_INIT;
   #endif
-  #if defined(Y_DIR_PIN) && Y_DIR_PIN >= 0
+  #if HAS_Y_DIR
     Y_DIR_INIT;
-    #if defined(Y_DUAL_STEPPER_DRIVERS) && defined(Y2_DIR_PIN) && Y2_DIR_PIN >= 0
+    #if defined(Y_DUAL_STEPPER_DRIVERS) && HAS_Y2_DIR
       Y2_DIR_INIT;
     #endif
   #endif
-  #if defined(Z_DIR_PIN) && Z_DIR_PIN >= 0
+  #if HAS_Z_DIR
     Z_DIR_INIT;
-    #if defined(Z_DUAL_STEPPER_DRIVERS) && defined(Z2_DIR_PIN) && Z2_DIR_PIN >= 0
+    #if defined(Z_DUAL_STEPPER_DRIVERS) && HAS_Z2_DIR
       Z2_DIR_INIT;
     #endif
   #endif
-  #if defined(E0_DIR_PIN) && E0_DIR_PIN >= 0
+  #if HAS_E0_DIR
     E0_DIR_INIT;
   #endif
-  #if defined(E1_DIR_PIN) && E1_DIR_PIN >= 0
+  #if HAS_E1_DIR
     E1_DIR_INIT;
   #endif
-  #if defined(E2_DIR_PIN) && E2_DIR_PIN >= 0
+  #if HAS_E2_DIR
     E2_DIR_INIT;
   #endif
-  #if defined(E3_DIR_PIN) && E3_DIR_PIN >= 0
+  #if HAS_E3_DIR
     E3_DIR_INIT;
   #endif
 
   //Initialize Enable Pins - steppers default to disabled.
 
-  #if defined(X_ENABLE_PIN) && X_ENABLE_PIN >= 0
+  #if HAS_X_ENABLE
     X_ENABLE_INIT;
     if (!X_ENABLE_ON) X_ENABLE_WRITE(HIGH);
   #endif
-  #if defined(X2_ENABLE_PIN) && X2_ENABLE_PIN >= 0
+  #if HAS_X2_ENABLE
     X2_ENABLE_INIT;
     if (!X_ENABLE_ON) X2_ENABLE_WRITE(HIGH);
   #endif
-  #if defined(Y_ENABLE_PIN) && Y_ENABLE_PIN >= 0
+  #if HAS_Y_ENABLE
     Y_ENABLE_INIT;
     if (!Y_ENABLE_ON) Y_ENABLE_WRITE(HIGH);
 	
-	#if defined(Y_DUAL_STEPPER_DRIVERS) && defined(Y2_ENABLE_PIN) && Y2_ENABLE_PIN >= 0
+	#if defined(Y_DUAL_STEPPER_DRIVERS) && HAS_Y2_ENABLE
 	  Y2_ENABLE_INIT;
 	  if (!Y_ENABLE_ON) Y2_ENABLE_WRITE(HIGH);
 	#endif
   #endif
-  #if defined(Z_ENABLE_PIN) && Z_ENABLE_PIN >= 0
+  #if HAS_Z_ENABLE
     Z_ENABLE_INIT;
     if (!Z_ENABLE_ON) Z_ENABLE_WRITE(HIGH);
 
-    #if defined(Z_DUAL_STEPPER_DRIVERS) && defined(Z2_ENABLE_PIN) && Z2_ENABLE_PIN >= 0
+    #if defined(Z_DUAL_STEPPER_DRIVERS) && HAS_Z2_ENABLE
       Z2_ENABLE_INIT;
       if (!Z_ENABLE_ON) Z2_ENABLE_WRITE(HIGH);
     #endif
   #endif
-  #if defined(E0_ENABLE_PIN) && E0_ENABLE_PIN >= 0
+  #if HAS_E0_ENABLE
     E0_ENABLE_INIT;
     if (!E_ENABLE_ON) E0_ENABLE_WRITE(HIGH);
   #endif
-  #if defined(E1_ENABLE_PIN) && E1_ENABLE_PIN >= 0
+  #if HAS_E1_ENABLE
     E1_ENABLE_INIT;
     if (!E_ENABLE_ON) E1_ENABLE_WRITE(HIGH);
   #endif
-  #if defined(E2_ENABLE_PIN) && E2_ENABLE_PIN >= 0
+  #if HAS_E2_ENABLE
     E2_ENABLE_INIT;
     if (!E_ENABLE_ON) E2_ENABLE_WRITE(HIGH);
   #endif
-  #if defined(E3_ENABLE_PIN) && E3_ENABLE_PIN >= 0
+  #if HAS_E3_ENABLE
     E3_ENABLE_INIT;
     if (!E_ENABLE_ON) E3_ENABLE_WRITE(HIGH);
   #endif
 
   //endstops and pullups
 
-  #if defined(X_MIN_PIN) && X_MIN_PIN >= 0
+  #if HAS_X_MIN
     SET_INPUT(X_MIN_PIN);
     #ifdef ENDSTOPPULLUP_XMIN
       WRITE(X_MIN_PIN,HIGH);
     #endif
   #endif
 
-  #if defined(Y_MIN_PIN) && Y_MIN_PIN >= 0
+  #if HAS_Y_MIN
     SET_INPUT(Y_MIN_PIN);
     #ifdef ENDSTOPPULLUP_YMIN
       WRITE(Y_MIN_PIN,HIGH);
     #endif
   #endif
 
-  #if defined(Z_MIN_PIN) && Z_MIN_PIN >= 0
+  #if HAS_Z_MIN
     SET_INPUT(Z_MIN_PIN);
     #ifdef ENDSTOPPULLUP_ZMIN
       WRITE(Z_MIN_PIN,HIGH);
     #endif
   #endif
 
-  #if defined(X_MAX_PIN) && X_MAX_PIN >= 0
+  #if HAS_X_MAX
     SET_INPUT(X_MAX_PIN);
     #ifdef ENDSTOPPULLUP_XMAX
       WRITE(X_MAX_PIN,HIGH);
     #endif
   #endif
 
-  #if defined(Y_MAX_PIN) && Y_MAX_PIN >= 0
+  #if HAS_Y_MAX
     SET_INPUT(Y_MAX_PIN);
     #ifdef ENDSTOPPULLUP_YMAX
       WRITE(Y_MAX_PIN,HIGH);
     #endif
   #endif
 
-  #if defined(Z_MAX_PIN) && Z_MAX_PIN >= 0
+  #if HAS_Z_MAX
     SET_INPUT(Z_MAX_PIN);
     #ifdef ENDSTOPPULLUP_ZMAX
       WRITE(Z_MAX_PIN,HIGH);
     #endif
   #endif
 
-  #if defined(Z2_MAX_PIN) && Z2_MAX_PIN >= 0
+  #if HAS_Z2_MAX
     SET_INPUT(Z2_MAX_PIN);
     #ifdef ENDSTOPPULLUP_ZMAX
       WRITE(Z2_MAX_PIN,HIGH);
     #endif
   #endif  
   
+#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);
+  #endif
+#endif
+
   #define AXIS_INIT(axis, AXIS, PIN) \
     AXIS ##_STEP_INIT; \
     AXIS ##_STEP_WRITE(INVERT_## PIN ##_STEP_PIN); \
@@ -970,36 +1013,36 @@ void st_init() {
   #define E_AXIS_INIT(NUM) AXIS_INIT(e## NUM, E## NUM, E)
 
   // Initialize Step Pins
-  #if defined(X_STEP_PIN) && X_STEP_PIN >= 0
+  #if HAS_X_STEP
     AXIS_INIT(x, X, X);
   #endif
-  #if defined(X2_STEP_PIN) && X2_STEP_PIN >= 0
+  #if HAS_X2_STEP
     AXIS_INIT(x, X2, X);
   #endif
-  #if defined(Y_STEP_PIN) && Y_STEP_PIN >= 0
-    #if defined(Y_DUAL_STEPPER_DRIVERS) && defined(Y2_STEP_PIN) && Y2_STEP_PIN >= 0
+  #if HAS_Y_STEP
+    #if defined(Y_DUAL_STEPPER_DRIVERS) && HAS_Y2_STEP
       Y2_STEP_INIT;
       Y2_STEP_WRITE(INVERT_Y_STEP_PIN);
     #endif
     AXIS_INIT(y, Y, Y);
   #endif
-  #if defined(Z_STEP_PIN) && Z_STEP_PIN >= 0
-    #if defined(Z_DUAL_STEPPER_DRIVERS) && defined(Z2_STEP_PIN) && Z2_STEP_PIN >= 0
+  #if HAS_Z_STEP
+    #if defined(Z_DUAL_STEPPER_DRIVERS) && HAS_Z2_STEP
       Z2_STEP_INIT;
       Z2_STEP_WRITE(INVERT_Z_STEP_PIN);
     #endif
     AXIS_INIT(z, Z, Z);
   #endif
-  #if defined(E0_STEP_PIN) && E0_STEP_PIN >= 0
+  #if HAS_E0_STEP
     E_AXIS_INIT(0);
   #endif
-  #if defined(E1_STEP_PIN) && E1_STEP_PIN >= 0
+  #if HAS_E1_STEP
     E_AXIS_INIT(1);
   #endif
-  #if defined(E2_STEP_PIN) && E2_STEP_PIN >= 0
+  #if HAS_E2_STEP
     E_AXIS_INIT(2);
   #endif
-  #if defined(E3_STEP_PIN) && E3_STEP_PIN >= 0
+  #if HAS_E3_STEP
     E_AXIS_INIT(3);
   #endif
 
@@ -1084,13 +1127,7 @@ long st_get_position(uint8_t axis) {
 
 void finishAndDisableSteppers() {
   st_synchronize();
-  disable_x();
-  disable_y();
-  disable_z();
-  disable_e0();
-  disable_e1();
-  disable_e2();
-  disable_e3();
+  disable_all_steppers();
 }
 
 void quickStop() {
@@ -1220,12 +1257,12 @@ void digipot_current(uint8_t driver, int current) {
 }
 
 void microstep_init() {
-  #if defined(E1_MS1_PIN) && E1_MS1_PIN >= 0
+  #if HAS_MICROSTEPS_E1
     pinMode(E1_MS1_PIN,OUTPUT);
-    pinMode(E1_MS2_PIN,OUTPUT); 
+    pinMode(E1_MS2_PIN,OUTPUT);
   #endif
 
-  #if defined(X_MS1_PIN) && X_MS1_PIN >= 0
+  #if HAS_MICROSTEPS
     pinMode(X_MS1_PIN,OUTPUT);
     pinMode(X_MS2_PIN,OUTPUT);  
     pinMode(Y_MS1_PIN,OUTPUT);
@@ -1246,7 +1283,7 @@ void microstep_ms(uint8_t driver, int8_t ms1, int8_t ms2) {
     case 1: digitalWrite(Y_MS1_PIN, ms1); break;
     case 2: digitalWrite(Z_MS1_PIN, ms1); break;
     case 3: digitalWrite(E0_MS1_PIN, ms1); break;
-    #if defined(E1_MS1_PIN) && E1_MS1_PIN >= 0
+    #if HAS_MICROSTEPS_E1
       case 4: digitalWrite(E1_MS1_PIN, ms1); break;
     #endif
   }
@@ -1285,7 +1322,7 @@ void microstep_readings() {
   SERIAL_PROTOCOLPGM("E0: ");
   SERIAL_PROTOCOL(digitalRead(E0_MS1_PIN));
   SERIAL_PROTOCOLLN(digitalRead(E0_MS2_PIN));
-  #if defined(E1_MS1_PIN) && E1_MS1_PIN >= 0
+  #if HAS_MICROSTEPS_E1
     SERIAL_PROTOCOLPGM("E1: ");
     SERIAL_PROTOCOL(digitalRead(E1_MS1_PIN));
     SERIAL_PROTOCOLLN(digitalRead(E1_MS2_PIN));
diff --git a/Marlin/temperature.cpp b/Marlin/temperature.cpp
index b59ff29df00..28e1afb7cef 100644
--- a/Marlin/temperature.cpp
+++ b/Marlin/temperature.cpp
@@ -1,5 +1,5 @@
 /*
-  temperature.c - temperature control
+  temperature.cpp - temperature control
   Part of Marlin
   
  Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
@@ -16,18 +16,7 @@
  
  You should have received a copy of the GNU General Public License
  along with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-/*
- This firmware is a mashup between Sprinter and grbl.
-  (https://github.com/kliment/Sprinter)
-  (https://github.com/simen/grbl/tree)
- 
- It has preliminary support for Matthew Roberts advance algorithm 
-    http://reprap.org/pipermail/reprap-dev/2011-May/003323.html
-
- */
-
+*/
 
 #include "Marlin.h"
 #include "ultralcd.h"
@@ -87,14 +76,14 @@ unsigned char soft_pwm_bed;
 #define HAS_HEATER_THERMAL_PROTECTION (defined(THERMAL_RUNAWAY_PROTECTION_PERIOD) && THERMAL_RUNAWAY_PROTECTION_PERIOD > 0)
 #define HAS_BED_THERMAL_PROTECTION (defined(THERMAL_RUNAWAY_PROTECTION_BED_PERIOD) && THERMAL_RUNAWAY_PROTECTION_BED_PERIOD > 0 && TEMP_SENSOR_BED != 0)
 #if HAS_HEATER_THERMAL_PROTECTION || HAS_BED_THERMAL_PROTECTION
-  static bool thermal_runaway = false;
-  void thermal_runaway_protection(int *state, unsigned long *timer, float temperature, float target_temperature, int heater_id, int period_seconds, int hysteresis_degc);
+  enum TRState { TRReset, TRInactive, TRFirstHeating, TRStable, TRRunaway };
+  void thermal_runaway_protection(TRState *state, unsigned long *timer, float temperature, float target_temperature, int heater_id, int period_seconds, int hysteresis_degc);
   #if HAS_HEATER_THERMAL_PROTECTION
-    static int thermal_runaway_state_machine[4]; // = {0,0,0,0};
+    static TRState thermal_runaway_state_machine[4] = { TRReset, TRReset, TRReset, TRReset };
     static unsigned long thermal_runaway_timer[4]; // = {0,0,0,0};
   #endif
   #if HAS_BED_THERMAL_PROTECTION
-    static int thermal_runaway_bed_state_machine;
+    static TRState thermal_runaway_bed_state_machine = TRReset;
     static unsigned long thermal_runaway_bed_timer;
   #endif
 #endif
@@ -238,7 +227,7 @@ void PID_autotune(float temp, int extruder, int ncycles)
     soft_pwm[extruder] = bias = d = PID_MAX / 2;
 
   // PID Tuning loop
-  for(;;) {
+  for (;;) {
 
     unsigned long ms = millis();
 
@@ -609,7 +598,7 @@ void manage_heater() {
   // Loop through all extruders
   for (int e = 0; e < EXTRUDERS; e++) {
 
-    #if defined (THERMAL_RUNAWAY_PROTECTION_PERIOD) && THERMAL_RUNAWAY_PROTECTION_PERIOD > 0
+    #if HAS_HEATER_THERMAL_PROTECTION
       thermal_runaway_protection(&thermal_runaway_state_machine[e], &thermal_runaway_timer[e], current_temperature[e], target_temperature[e], e, THERMAL_RUNAWAY_PROTECTION_PERIOD, THERMAL_RUNAWAY_PROTECTION_HYSTERESIS);
     #endif
 
@@ -637,7 +626,7 @@ void manage_heater() {
         disable_heater();
         _temp_error(0, PSTR(MSG_EXTRUDER_SWITCHED_OFF), PSTR(MSG_ERR_REDUNDANT_TEMP));
       }
-    #endif //TEMP_SENSOR_1_AS_REDUNDANT
+    #endif // TEMP_SENSOR_1_AS_REDUNDANT
 
   } // Extruders Loop
 
@@ -656,7 +645,7 @@ void manage_heater() {
   #if TEMP_SENSOR_BED != 0
   
     #if HAS_BED_THERMAL_PROTECTION
-      thermal_runaway_protection(&thermal_runaway_bed_state_machine, &thermal_runaway_bed_timer, current_temperature_bed, target_temperature_bed, 9, THERMAL_RUNAWAY_PROTECTION_BED_PERIOD, THERMAL_RUNAWAY_PROTECTION_BED_HYSTERESIS);
+      thermal_runaway_protection(&thermal_runaway_bed_state_machine, &thermal_runaway_bed_timer, current_temperature_bed, target_temperature_bed, -1, THERMAL_RUNAWAY_PROTECTION_BED_PERIOD, THERMAL_RUNAWAY_PROTECTION_BED_HYSTERESIS);
     #endif
 
     #ifdef PIDTEMPBED
@@ -1014,69 +1003,72 @@ void setWatch() {
 }
 
 #if HAS_HEATER_THERMAL_PROTECTION || HAS_BED_THERMAL_PROTECTION
-void thermal_runaway_protection(int *state, unsigned long *timer, float temperature, float target_temperature, int heater_id, int period_seconds, int hysteresis_degc)
-{
-/*
-      SERIAL_ECHO_START;
-      SERIAL_ECHO("Thermal Thermal Runaway Running. Heater ID:");
-      SERIAL_ECHO(heater_id);
-      SERIAL_ECHO(" ;  State:");
-      SERIAL_ECHO(*state);
-      SERIAL_ECHO(" ;  Timer:");
-      SERIAL_ECHO(*timer);
-      SERIAL_ECHO(" ;  Temperature:");
-      SERIAL_ECHO(temperature);
-      SERIAL_ECHO(" ;  Target Temp:");
-      SERIAL_ECHO(target_temperature);
-      SERIAL_ECHOLN("");    
-*/
-  if ((target_temperature == 0) || thermal_runaway)
-  {
-    *state = 0;
-    *timer = 0;
-    return;
-  }
-  switch (*state)
-  {
-    case 0: // "Heater Inactive" state
-      if (target_temperature > 0) *state = 1;
-      break;
-    case 1: // "First Heating" state
-      if (temperature >= target_temperature) *state = 2;
-      break;
-    case 2: // "Temperature Stable" state
-    {
-      unsigned long ms = millis();
-      if (temperature >= (target_temperature - hysteresis_degc))
-      {
-        *timer = ms;
-      } 
-      else if ( (ms - *timer) > ((unsigned long) period_seconds) * 1000)
-      {
+
+  void thermal_runaway_protection(TRState *state, unsigned long *timer, float temperature, float target_temperature, int heater_id, int period_seconds, int hysteresis_degc) {
+
+    static float tr_target_temperature[EXTRUDERS+1] = { 0.0 };
+
+    /*
+        SERIAL_ECHO_START;
+        SERIAL_ECHOPGM("Thermal Thermal Runaway Running. Heater ID: ");
+        if (heater_id < 0) SERIAL_ECHOPGM("bed"); else SERIAL_ECHOPGM(heater_id);
+        SERIAL_ECHOPGM(" ;  State:");
+        SERIAL_ECHOPGM(*state);
+        SERIAL_ECHOPGM(" ;  Timer:");
+        SERIAL_ECHOPGM(*timer);
+        SERIAL_ECHOPGM(" ;  Temperature:");
+        SERIAL_ECHOPGM(temperature);
+        SERIAL_ECHOPGM(" ;  Target Temp:");
+        SERIAL_ECHOPGM(target_temperature);
+        SERIAL_EOL;
+    */
+
+    int heater_index = heater_id >= 0 ? heater_id : EXTRUDERS;
+
+    // If the target temperature changes, restart
+    if (tr_target_temperature[heater_index] != target_temperature)
+      *state = TRReset;
+
+    switch (*state) {
+      case TRReset:
+        *timer = 0;
+        *state = TRInactive;
+        break;
+      // Inactive state waits for a target temperature to be set
+      case TRInactive:
+        if (target_temperature > 0) {
+          tr_target_temperature[heater_index] = target_temperature;
+          *state = TRFirstHeating;
+        }
+        break;
+      // When first heating, wait for the temperature to be reached then go to Stable state
+      case TRFirstHeating:
+        if (temperature >= tr_target_temperature[heater_index]) *state = TRStable;
+        break;
+      // While the temperature is stable watch for a bad temperature
+      case TRStable:
+        // If the temperature is over the target (-hysteresis) restart the timer
+        if (temperature >= tr_target_temperature[heater_index] - hysteresis_degc)
+          *timer = millis();
+          // If the timer goes too long without a reset, trigger shutdown
+        else if (millis() > *timer + period_seconds * 1000UL)
+          *state = TRRunaway;
+        break;
+      case TRRunaway:
         SERIAL_ERROR_START;
         SERIAL_ERRORLNPGM(MSG_THERMAL_RUNAWAY_STOP);
-        SERIAL_ERRORLN((int)heater_id);
-        LCD_ALERTMESSAGEPGM(MSG_THERMAL_RUNAWAY); // translatable
-        thermal_runaway = true;
-        while(1)
-        {
-          disable_heater();
-          disable_x();
-          disable_y();
-          disable_z();
-          disable_e0();
-          disable_e1();
-          disable_e2();
-          disable_e3();
+        if (heater_id < 0) SERIAL_ERRORLNPGM("bed"); else SERIAL_ERRORLN(heater_id);
+        LCD_ALERTMESSAGEPGM(MSG_THERMAL_RUNAWAY);
+        disable_heater();
+        disable_all_steppers();
+        for (;;) {
           manage_heater();
           lcd_update();
         }
-      }
-    } break;
+    }
   }
-}
-#endif //THERMAL_RUNAWAY_PROTECTION_PERIOD
 
+#endif // HAS_HEATER_THERMAL_PROTECTION || HAS_BED_THERMAL_PROTECTION
 
 void disable_heater() {
   for (int i=0; i<EXTRUDERS; i++) setTargetHotend(0, i);
@@ -1559,7 +1551,7 @@ ISR(TIMER0_COMPB_vect) {
       #else
         #define GE2 >=
       #endif
-      if (current_temperature_raw[2] GE2 (maxttemp_raw[2]) max_temp_error(2);
+      if (current_temperature_raw[2] GE2 maxttemp_raw[2]) max_temp_error(2);
       if (minttemp_raw[2] GE2 current_temperature_raw[2]) min_temp_error(2);
     #endif // TEMP_SENSOR_2
 
diff --git a/Marlin/temperature.h b/Marlin/temperature.h
index 79146a35565..847c41b009a 100644
--- a/Marlin/temperature.h
+++ b/Marlin/temperature.h
@@ -18,8 +18,8 @@
   along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
 */
 
-#ifndef temperature_h
-#define temperature_h 
+#ifndef TEMPERATURE_H
+#define TEMPERATURE_H 
 
 #include "Marlin.h"
 #include "planner.h"
@@ -53,7 +53,7 @@ extern float current_temperature_bed;
   extern float redundant_temperature;
 #endif
 
-#if defined(CONTROLLERFAN_PIN) && CONTROLLERFAN_PIN > -1
+#if HAS_CONTROLLERFAN
   extern unsigned char soft_pwm_bed;
 #endif
 
@@ -72,11 +72,11 @@ extern float current_temperature_bed;
   float unscalePID_d(float d);
 
 #endif
+
 #ifdef PIDTEMPBED
   extern float bedKp,bedKi,bedKd;
 #endif
   
-  
 #ifdef BABYSTEPPING
   extern volatile int babystepsTodo[3];
 #endif
@@ -105,40 +105,27 @@ FORCE_INLINE bool isHeatingBed() { return target_temperature_bed > current_tempe
 FORCE_INLINE bool isCoolingHotend(uint8_t extruder) { return target_temperature[extruder] < current_temperature[extruder]; }
 FORCE_INLINE bool isCoolingBed() { return target_temperature_bed < current_temperature_bed; }
 
-#define degHotend0() degHotend(0)
-#define degTargetHotend0() degTargetHotend(0)
-#define setTargetHotend0(_celsius) setTargetHotend((_celsius), 0)
-#define isHeatingHotend0() isHeatingHotend(0)
-#define isCoolingHotend0() isCoolingHotend(0)
+#define HOTEND_ROUTINES(NR) \
+  FORCE_INLINE float degHotend##NR() { return degHotend(NR); } \
+  FORCE_INLINE float degTargetHotend##NR() { return degTargetHotend(NR); } \
+  FORCE_INLINE void setTargetHotend##NR(const float c) { setTargetHotend(c, NR); } \
+  FORCE_INLINE bool isHeatingHotend##NR() { return isHeatingHotend(NR); } \
+  FORCE_INLINE bool isCoolingHotend##NR() { return isCoolingHotend(NR); }
+HOTEND_ROUTINES(0);
 #if EXTRUDERS > 1
-  #define degHotend1() degHotend(1)
-  #define degTargetHotend1() degTargetHotend(1)
-  #define setTargetHotend1(_celsius) setTargetHotend((_celsius), 1)
-  #define isHeatingHotend1() isHeatingHotend(1)
-  #define isCoolingHotend1() isCoolingHotend(1)
+  HOTEND_ROUTINES(1);
 #else
-  #define setTargetHotend1(_celsius) do{}while(0)
+  #define setTargetHotend1(c) do{}while(0)
 #endif
 #if EXTRUDERS > 2
-  #define degHotend2() degHotend(2)
-  #define degTargetHotend2() degTargetHotend(2)
-  #define setTargetHotend2(_celsius) setTargetHotend((_celsius), 2)
-  #define isHeatingHotend2() isHeatingHotend(2)
-  #define isCoolingHotend2() isCoolingHotend(2)
+  HOTEND_ROUTINES(2);
 #else
-  #define setTargetHotend2(_celsius) do{}while(0)
+  #define setTargetHotend2(c) do{}while(0)
 #endif
 #if EXTRUDERS > 3
-  #define degHotend3() degHotend(3)
-  #define degTargetHotend3() degTargetHotend(3)
-  #define setTargetHotend3(_celsius) setTargetHotend((_celsius), 3)
-  #define isHeatingHotend3() isHeatingHotend(3)
-  #define isCoolingHotend3() isCoolingHotend(3)
+  HOTEND_ROUTINES(3);
 #else
-  #define setTargetHotend3(_celsius) do{}while(0)
-#endif
-#if EXTRUDERS > 4
-  #error Invalid number of extruders
+  #define setTargetHotend3(c) do{}while(0)
 #endif
 
 int getHeaterPower(int heater);
@@ -161,5 +148,4 @@ FORCE_INLINE void autotempShutdown() {
   #endif
 }
 
-
-#endif
+#endif // TEMPERATURE_H
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index e6da28e0f17..42a957dcab5 100644
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -262,8 +262,7 @@ static void lcd_goto_menu(menuFunc_t menu, const uint32_t encoder=0, const bool
 }
 
 /* Main status screen. It's up to the implementation specific part to show what is needed. As this is very display dependent */
-static void lcd_status_screen()
-{
+static void lcd_status_screen() {
 	encoderRateMultiplierEnabled = false;
 
   #ifdef LCD_PROGRESS_BAR
@@ -296,15 +295,7 @@ static void lcd_status_screen()
     #endif
   #endif //LCD_PROGRESS_BAR
 
-  if (lcd_status_update_delay)
-    lcd_status_update_delay--;
-  else
-    lcdDrawUpdate = 1;
-
-  if (lcdDrawUpdate) {
     lcd_implementation_status_screen();
-    lcd_status_update_delay = 10;   /* redraw the main screen every second. This is easier then trying keep track of all things that change on the screen */
-  }
 
 #ifdef ULTIPANEL
 
@@ -1298,7 +1289,7 @@ void lcd_update() {
       }
     }
   #endif//CARDINSERTED
-
+  
   uint32_t ms = millis();
   if (ms > lcd_next_update_millis) {
 
@@ -1349,27 +1340,36 @@ void lcd_update() {
             } // encoderRateMultiplierEnabled
           #endif //ENCODER_RATE_MULTIPLIER
 
-          lcdDrawUpdate = 1;
           encoderPosition += (encoderDiff * encoderMultiplier) / ENCODER_PULSES_PER_STEP;
           encoderDiff = 0;
         }
         timeoutToStatus = ms + LCD_TIMEOUT_TO_STATUS;
+        lcdDrawUpdate = 1;
       }
-
     #endif //ULTIPANEL
 
+    if (currentMenu == lcd_status_screen) {
+      if (!lcd_status_update_delay) {
+        lcdDrawUpdate = 1;
+        lcd_status_update_delay = 10;   /* redraw the main screen every second. This is easier then trying keep track of all things that change on the screen */
+      }
+      else {
+        lcd_status_update_delay--;
+      }
+    }
     #ifdef DOGLCD  // Changes due to different driver architecture of the DOGM display
-      blink++;     // Variable for fan animation and alive dot
-      u8g.firstPage();
-      do {
-        lcd_setFont(FONT_MENU);
-        u8g.setPrintPos(125, 0);
-        if (blink % 2) u8g.setColorIndex(1); else u8g.setColorIndex(0); // Set color for the alive dot
-        u8g.drawPixel(127, 63); // draw alive dot
-        u8g.setColorIndex(1); // black on white
-        (*currentMenu)();
-        if (!lcdDrawUpdate) break; // Terminate display update, when nothing new to draw. This must be done before the last dogm.next()
-      } while( u8g.nextPage() );
+      if (lcdDrawUpdate) {
+        blink++;     // Variable for fan animation and alive dot
+        u8g.firstPage();
+        do {
+          lcd_setFont(FONT_MENU);
+          u8g.setPrintPos(125, 0);
+          if (blink % 2) u8g.setColorIndex(1); else u8g.setColorIndex(0); // Set color for the alive dot
+          u8g.drawPixel(127, 63); // draw alive dot
+          u8g.setColorIndex(1); // black on white
+          (*currentMenu)();
+        } while( u8g.nextPage() );
+      }
     #else
       (*currentMenu)();
     #endif
@@ -1789,7 +1789,7 @@ char *ftostr52(const float &x) {
   return conv;
 }
 
-#if defined(MANUAL_BED_LEVELING)
+#ifdef MANUAL_BED_LEVELING
 static int _lcd_level_bed_position;
 static void _lcd_level_bed()
 {
@@ -1849,8 +1849,7 @@ static void _lcd_level_bed_homing()
     lcd_goto_menu(_lcd_level_bed);
   }
 }
-static void lcd_level_bed()
-{
+static void lcd_level_bed() {
   axis_known_position[X_AXIS] = false;
   axis_known_position[Y_AXIS] = false;
   axis_known_position[Z_AXIS] = false;
diff --git a/Marlin/ultralcd.h b/Marlin/ultralcd.h
index a89c2620647..fe4f5976802 100644
--- a/Marlin/ultralcd.h
+++ b/Marlin/ultralcd.h
@@ -64,14 +64,14 @@
 
     #define LCD_CLICKED (buttons&EN_C)
     #ifdef REPRAPWORLD_KEYPAD
-  	  #define EN_REPRAPWORLD_KEYPAD_F3 BIT(BLEN_REPRAPWORLD_KEYPAD_F3)
-  	  #define EN_REPRAPWORLD_KEYPAD_F2 BIT(BLEN_REPRAPWORLD_KEYPAD_F2)
-  	  #define EN_REPRAPWORLD_KEYPAD_F1 BIT(BLEN_REPRAPWORLD_KEYPAD_F1)
-  	  #define EN_REPRAPWORLD_KEYPAD_UP BIT(BLEN_REPRAPWORLD_KEYPAD_UP)
-  	  #define EN_REPRAPWORLD_KEYPAD_RIGHT BIT(BLEN_REPRAPWORLD_KEYPAD_RIGHT)
-  	  #define EN_REPRAPWORLD_KEYPAD_MIDDLE BIT(BLEN_REPRAPWORLD_KEYPAD_MIDDLE)
-  	  #define EN_REPRAPWORLD_KEYPAD_DOWN BIT(BLEN_REPRAPWORLD_KEYPAD_DOWN)
-  	  #define EN_REPRAPWORLD_KEYPAD_LEFT BIT(BLEN_REPRAPWORLD_KEYPAD_LEFT)
+  	  #define EN_REPRAPWORLD_KEYPAD_F3 (BIT(BLEN_REPRAPWORLD_KEYPAD_F3))
+  	  #define EN_REPRAPWORLD_KEYPAD_F2 (BIT(BLEN_REPRAPWORLD_KEYPAD_F2))
+  	  #define EN_REPRAPWORLD_KEYPAD_F1 (BIT(BLEN_REPRAPWORLD_KEYPAD_F1))
+  	  #define EN_REPRAPWORLD_KEYPAD_UP (BIT(BLEN_REPRAPWORLD_KEYPAD_UP))
+  	  #define EN_REPRAPWORLD_KEYPAD_RIGHT (BIT(BLEN_REPRAPWORLD_KEYPAD_RIGHT))
+  	  #define EN_REPRAPWORLD_KEYPAD_MIDDLE (BIT(BLEN_REPRAPWORLD_KEYPAD_MIDDLE))
+  	  #define EN_REPRAPWORLD_KEYPAD_DOWN (BIT(BLEN_REPRAPWORLD_KEYPAD_DOWN))
+  	  #define EN_REPRAPWORLD_KEYPAD_LEFT (BIT(BLEN_REPRAPWORLD_KEYPAD_LEFT))
 
   	  #define LCD_CLICKED ((buttons&EN_C) || (buttons_reprapworld_keypad&EN_REPRAPWORLD_KEYPAD_F1))
   	  #define REPRAPWORLD_KEYPAD_MOVE_Z_UP (buttons_reprapworld_keypad&EN_REPRAPWORLD_KEYPAD_F2)
diff --git a/Marlin/vector_3.cpp b/Marlin/vector_3.cpp
index 243f0838f00..9eb3465fbf3 100644
--- a/Marlin/vector_3.cpp
+++ b/Marlin/vector_3.cpp
@@ -125,9 +125,9 @@ void matrix_3x3::debug(const char title[]) {
   int count = 0;
   for(int i=0; i<3; i++) {
     for(int j=0; j<3; j++) {
-      if (matrix[count] >= 0.0) SERIAL_PROTOCOLPGM("+");
+      if (matrix[count] >= 0.0) SERIAL_PROTOCOLCHAR('+');
       SERIAL_PROTOCOL_F(matrix[count], 6);
-      SERIAL_PROTOCOLPGM(" ");
+      SERIAL_PROTOCOLCHAR(' ');
       count++;
     }
     SERIAL_EOL;
diff --git a/README.md b/README.md
index 2ecb9d8e8a6..d2ff347d6f2 100644
--- a/README.md
+++ b/README.md
@@ -18,8 +18,8 @@
 ## Quick Information
 
 This is a firmware for reprap single-processor electronics setups.
-It also works on the Ultimaker PCB. It supports printing from SD card+Folders, and look-ahead trajectory planning.
-This firmware is a mashup between [Sprinter](https://github.com/kliment/Sprinter), [grbl](https://github.com/simen/grbl) and many original parts.
+It also works on the Ultimaker PCB. It supports printing from SD card+Folders and look-ahead trajectory planning.
+This firmware is a mashup between [Sprinter](https://github.com/kliment/Sprinter), [grbl](https://github.com/simen/grbl), and many original parts.
 
 ## Current Status: Bug Fixing
 
@@ -31,18 +31,18 @@ We are actively looking for testers. So please try the current development versi
 
 ## Contact
 
-__IRC:__ #marlin-firmware @freenode ([WebChat Client](https://webchat.freenode.net/?channels=marlin-firmware)
+__Google Hangout:__ <a href="https://plus.google.com/hangouts/_/g2wp5duzb2y6ahikg6tmwao3kua" target="_blank">Hagnout</a>
 
 ## Credits
 
 The current Marlin dev team consists of:
 
- - Erik van der Zalm ([@ErikZalm](https://github.com/ErikZalm))
- - [@daid](https://github.com/daid)
+ - Scott Lahteine [@thinkyhead]
+ - 
 
 Sprinters lead developers are Kliment and caru.
-Grbls lead developer is Simen Svale Skogsrud.
-Sonney Jeon (Chamnit) improved some parts of grbl
+Grbl's lead developer is Simen Svale Skogsrud.
+Sonney Jeon (Chamnit) improved some parts of grbl.
 A fork by bkubicek for the Ultimaker was merged.
 
 More features have been added by: