diff --git a/Marlin/src/HAL/HAL.h b/Marlin/src/HAL/HAL.h
index 1e0de556f..80a968069 100644
--- a/Marlin/src/HAL/HAL.h
+++ b/Marlin/src/HAL/HAL.h
@@ -31,22 +31,21 @@
 
 #include "SPI.h"
 
+#define CPU_32_BIT
+
 #ifdef __AVR__
+  #undef CPU_32_BIT
   #include "HAL_AVR/HAL_AVR.h"
 #elif defined(ARDUINO_ARCH_SAM)
-  #define CPU_32_BIT
   #include "HAL_DUE/HAL_Due.h"
   #include "math_32bit.h"
 #elif defined(__MK64FX512__) || defined(__MK66FX1M0__)
-  #define CPU_32_BIT
   #include "HAL_TEENSY35_36/HAL_Teensy.h"
   #include "math_32bit.h"
 #elif defined(TARGET_LPC1768)
-  #define CPU_32_BIT
   #include "math_32bit.h"
   #include "HAL_LPC1768/HAL.h"
 #elif defined(__STM32F1__) || defined(TARGET_STM32F1)
-  #define CPU_32_BIT
   #include "math_32bit.h"
   #include "HAL_STM32F1/HAL_Stm32f1.h"
 #else
diff --git a/Marlin/src/HAL/HAL_AVR/HAL_AVR.h b/Marlin/src/HAL/HAL_AVR/HAL_AVR.h
index 2a7ef0674..6c6b9be0b 100644
--- a/Marlin/src/HAL/HAL_AVR/HAL_AVR.h
+++ b/Marlin/src/HAL/HAL_AVR/HAL_AVR.h
@@ -35,7 +35,7 @@
 
 #include <stdint.h>
 
-#include "Arduino.h"
+#include <Arduino.h>
 
 #include <util/delay.h>
 #include <avr/eeprom.h>
diff --git a/Marlin/src/HAL/HAL_DUE/HAL_Due.h b/Marlin/src/HAL/HAL_DUE/HAL_Due.h
index 3c7993b99..c8f416067 100644
--- a/Marlin/src/HAL/HAL_DUE/HAL_Due.h
+++ b/Marlin/src/HAL/HAL_DUE/HAL_Due.h
@@ -31,7 +31,7 @@
 
 #include <stdint.h>
 
-#include "Arduino.h"
+#include <Arduino.h>
 
 #include "fastio_Due.h"
 #include "watchdog_Due.h"
diff --git a/Marlin/src/HAL/HAL_LPC1768/HAL.h b/Marlin/src/HAL/HAL_LPC1768/HAL.h
index 907b550c3..cf54cb1d7 100644
--- a/Marlin/src/HAL/HAL_LPC1768/HAL.h
+++ b/Marlin/src/HAL/HAL_LPC1768/HAL.h
@@ -35,6 +35,10 @@
 
 #include <stdint.h>
 #include <stdarg.h>
+
+#undef min
+#undef max
+
 #include <algorithm>
 
 void _printf (const  char *format, ...);
@@ -52,7 +56,8 @@ extern "C" volatile uint32_t _millis;
 #define B01 1
 #define B10 2
 
-#include "arduino.h"
+#include "include/arduino.h"
+
 #include "pinmapping.h"
 #include "fastio.h"
 #include "watchdog.h"
diff --git a/Marlin/src/HAL/HAL_LPC1768/HAL_LCD_I2C_routines.c b/Marlin/src/HAL/HAL_LPC1768/HAL_LCD_I2C_routines.c
index e43032b09..80bf5fc63 100644
--- a/Marlin/src/HAL/HAL_LPC1768/HAL_LCD_I2C_routines.c
+++ b/Marlin/src/HAL/HAL_LPC1768/HAL_LCD_I2C_routines.c
@@ -23,161 +23,150 @@
 // adapted from  I2C/master/master.c example
 //   https://www-users.cs.york.ac.uk/~pcc/MCP/HAPR-Course-web/CMSIS/examples/html/master_8c_source.html
 
+#ifdef TARGET_LPC1768
 
+#ifdef __cplusplus
+  extern "C" {
+#endif
 
-#if defined(TARGET_LPC1768)
+#include <lpc17xx_i2c.h>
+#include <lpc17xx_pinsel.h>
+#include <lpc17xx_libcfg_default.h>
 
-  #ifdef __cplusplus
-    extern "C" {
-  #endif
+//////////////////////////////////////////////////////////////////////////////////////
 
-  #include <lpc17xx_i2c.h>
-  #include <lpc17xx_pinsel.h>
-  #include <lpc17xx_libcfg_default.h>
+// These two routines are exact copies of the lpc17xx_i2c.c routines.  Couldn't link to
+// to the lpc17xx_i2c.c routines so had to copy them into this file & rename them.
 
-  //////////////////////////////////////////////////////////////////////////////////////
+static uint32_t _I2C_Start (LPC_I2C_TypeDef *I2Cx) {
+  // Reset STA, STO, SI
+  I2Cx->I2CONCLR = I2C_I2CONCLR_SIC|I2C_I2CONCLR_STOC|I2C_I2CONCLR_STAC;
 
-  // These two routines are exact copies of the lpc17xx_i2c.c routines.  Couldn't link to
-  // to the lpc17xx_i2c.c routines so had to copy them into this file & rename them.
+  // Enter to Master Transmitter mode
+  I2Cx->I2CONSET = I2C_I2CONSET_STA;
 
-  static uint32_t _I2C_Start (LPC_I2C_TypeDef *I2Cx)
-  {
-    // Reset STA, STO, SI
-    I2Cx->I2CONCLR = I2C_I2CONCLR_SIC|I2C_I2CONCLR_STOC|I2C_I2CONCLR_STAC;
+  // Wait for complete
+  while (!(I2Cx->I2CONSET & I2C_I2CONSET_SI));
+  I2Cx->I2CONCLR = I2C_I2CONCLR_STAC;
+  return (I2Cx->I2STAT & I2C_STAT_CODE_BITMASK);
+}
 
-    // Enter to Master Transmitter mode
-    I2Cx->I2CONSET = I2C_I2CONSET_STA;
-
-    // Wait for complete
-    while (!(I2Cx->I2CONSET & I2C_I2CONSET_SI));
+static void _I2C_Stop (LPC_I2C_TypeDef *I2Cx) {
+  /* Make sure start bit is not active */
+  if (I2Cx->I2CONSET & I2C_I2CONSET_STA)
     I2Cx->I2CONCLR = I2C_I2CONCLR_STAC;
-    return (I2Cx->I2STAT & I2C_STAT_CODE_BITMASK);
-  }
 
-  static void _I2C_Stop (LPC_I2C_TypeDef *I2Cx)
-  {
+  I2Cx->I2CONSET = I2C_I2CONSET_STO|I2C_I2CONSET_AA;
+  I2Cx->I2CONCLR = I2C_I2CONCLR_SIC;
+}
 
-    /* Make sure start bit is not active */
-    if (I2Cx->I2CONSET & I2C_I2CONSET_STA)
-    {
-      I2Cx->I2CONCLR = I2C_I2CONCLR_STAC;
-    }
+//////////////////////////////////////////////////////////////////////////////////////
 
-    I2Cx->I2CONSET = I2C_I2CONSET_STO|I2C_I2CONSET_AA;
+#define U8G_I2C_OPT_FAST 16  // from u8g.h
 
-    I2Cx->I2CONCLR = I2C_I2CONCLR_SIC;
-  }
+#define USEDI2CDEV_M            1
 
+#define I2CDEV_S_ADDR   0x78  // from SSD1306  //actual address is 0x3C - shift left 1 with LSB set to 0 to indicate write
 
-  //////////////////////////////////////////////////////////////////////////////////////
+#define BUFFER_SIZE                     0x1  // only do single byte transfers with LCDs
 
-  #define U8G_I2C_OPT_FAST 16  // from u8g.h
+#if (USEDI2CDEV_M == 0)
+  #define I2CDEV_M LPC_I2C0
+#elif (USEDI2CDEV_M == 1)
+  #define I2CDEV_M LPC_I2C1
+#elif (USEDI2CDEV_M == 2)
+  #define I2CDEV_M LPC_I2C2
+#else
+  #error "Master I2C device not defined!"
+#endif
 
-  #define USEDI2CDEV_M            1
+PINSEL_CFG_Type PinCfg;
+I2C_M_SETUP_Type transferMCfg;
 
-  #define I2CDEV_S_ADDR   0x78  // from SSD1306  //actual address is 0x3C - shift left 1 with LSB set to 0 to indicate write
+#define I2C_status (LPC_I2C1->I2STAT & I2C_STAT_CODE_BITMASK)
 
-  #define BUFFER_SIZE                     0x1  // only do single byte transfers with LCDs
+uint8_t u8g_i2c_start(uint8_t sla) {  // send slave address and write bit
+  // Sometimes TX data ACK or NAK status is returned.  That mean the start state didn't
+  // happen which means only the value of the slave address was send.  Keep looping until
+  // the slave address and write bit are actually sent.
+  do{
+    _I2C_Stop(I2CDEV_M); // output stop state on I2C bus
+    _I2C_Start(I2CDEV_M); // output start state on I2C bus
+    while ((I2C_status != I2C_I2STAT_M_TX_START)
+        && (I2C_status != I2C_I2STAT_M_TX_RESTART)
+        && (I2C_status != I2C_I2STAT_M_TX_DAT_ACK)
+        && (I2C_status != I2C_I2STAT_M_TX_DAT_NACK));  //wait for start to be asserted
 
-  #if (USEDI2CDEV_M == 0)
-    #define I2CDEV_M LPC_I2C0
-  #elif (USEDI2CDEV_M == 1)
-    #define I2CDEV_M LPC_I2C1
-  #elif (USEDI2CDEV_M == 2)
-    #define I2CDEV_M LPC_I2C2
-  #else
-    #error "Master I2C device not defined!"
-  #endif
-
-
-  PINSEL_CFG_Type PinCfg;
-  I2C_M_SETUP_Type transferMCfg;
-
-  #define I2C_status (LPC_I2C1->I2STAT & I2C_STAT_CODE_BITMASK)
-
-
-  uint8_t u8g_i2c_start(uint8_t sla) {  // send slave address and write bit
-    // Sometimes TX data ACK or NAK status is returned.  That mean the start state didn't
-    // happen which means only the value of the slave address was send.  Keep looping until
-    // the slave address and write bit are actually sent.
-    do{
-      _I2C_Stop(I2CDEV_M); // output stop state on I2C bus
-      _I2C_Start(I2CDEV_M); // output start state on I2C bus
-      while ((I2C_status != I2C_I2STAT_M_TX_START)
-          && (I2C_status != I2C_I2STAT_M_TX_RESTART)
-          && (I2C_status != I2C_I2STAT_M_TX_DAT_ACK)
-          && (I2C_status != I2C_I2STAT_M_TX_DAT_NACK));  //wait for start to be asserted
-
-      LPC_I2C1->I2CONCLR = I2C_I2CONCLR_STAC; // clear start state before tansmitting slave address
-      LPC_I2C1->I2DAT = I2CDEV_S_ADDR & I2C_I2DAT_BITMASK; // transmit slave address & write bit
-      LPC_I2C1->I2CONSET = I2C_I2CONSET_AA;
-      LPC_I2C1->I2CONCLR = I2C_I2CONCLR_SIC;
-      while ((I2C_status != I2C_I2STAT_M_TX_SLAW_ACK)
-          && (I2C_status != I2C_I2STAT_M_TX_SLAW_NACK)
-          && (I2C_status != I2C_I2STAT_M_TX_DAT_ACK)
-          && (I2C_status != I2C_I2STAT_M_TX_DAT_NACK));  //wait for slaw to finish
-    }while ( (I2C_status == I2C_I2STAT_M_TX_DAT_ACK) ||  (I2C_status == I2C_I2STAT_M_TX_DAT_NACK));
-    return 1;
-  }
-
-
-  void u8g_i2c_init(uint8_t clock_option) {
-
-    /*
-      * Init I2C pin connect
-    */
-    PinCfg.OpenDrain = 0;
-    PinCfg.Pinmode = 0;
-    #if ((USEDI2CDEV_M == 0))
-      PinCfg.Funcnum = 1;
-      PinCfg.Pinnum = 27;
-      PinCfg.Portnum = 0;
-      PINSEL_ConfigPin(&PinCfg); // SDA0 / D57  AUX-1
-      PinCfg.Pinnum = 28;
-      PINSEL_ConfigPin(&PinCfg); // SCL0 / D58  AUX-1
-    #endif
-    #if ((USEDI2CDEV_M == 1))
-      PinCfg.Funcnum = 3;
-      PinCfg.Pinnum = 0;
-      PinCfg.Portnum = 0;
-      PINSEL_ConfigPin(&PinCfg);  // SDA1 / D20 SCA
-      PinCfg.Pinnum = 1;
-      PINSEL_ConfigPin(&PinCfg);  // SCL1 / D21 SCL
-    #endif
-    #if ((USEDI2CDEV_M == 2))
-      PinCfg.Funcnum = 2;
-      PinCfg.Pinnum = 10;
-      PinCfg.Portnum = 0;
-      PINSEL_ConfigPin(&PinCfg); // SDA2 / D38  X_ENABLE_PIN
-      PinCfg.Pinnum = 11;
-      PINSEL_ConfigPin(&PinCfg); // SCL2 / D55  X_DIR_PIN
-    #endif
-    // Initialize I2C peripheral
-    I2C_Init(I2CDEV_M, (clock_option & U8G_I2C_OPT_FAST) ? 400000: 100000);  // LCD data rates
-
-    /* Enable Master I2C operation */
-    I2C_Cmd(I2CDEV_M, I2C_MASTER_MODE, ENABLE);
-
-    u8g_i2c_start(0); // send slave address and write bit
-  }
-
-  volatile extern uint32_t _millis;
-  uint8_t u8g_i2c_send_byte(uint8_t data) {
-    #define I2C_TIMEOUT 3
-    LPC_I2C1->I2DAT = data & I2C_I2DAT_BITMASK; // transmit data
+    LPC_I2C1->I2CONCLR = I2C_I2CONCLR_STAC; // clear start state before tansmitting slave address
+    LPC_I2C1->I2DAT = I2CDEV_S_ADDR & I2C_I2DAT_BITMASK; // transmit slave address & write bit
     LPC_I2C1->I2CONSET = I2C_I2CONSET_AA;
     LPC_I2C1->I2CONCLR = I2C_I2CONCLR_SIC;
-    uint32_t timeout = _millis + I2C_TIMEOUT;
-    while ((I2C_status != I2C_I2STAT_M_TX_DAT_ACK) && (I2C_status != I2C_I2STAT_M_TX_DAT_NACK) && (timeout > _millis));  // wait for xmit to finish
-    // had hangs with SH1106 so added time out - have seen temporary screen corruption when this happens
-    return 1;
-  }
+    while ((I2C_status != I2C_I2STAT_M_TX_SLAW_ACK)
+        && (I2C_status != I2C_I2STAT_M_TX_SLAW_NACK)
+        && (I2C_status != I2C_I2STAT_M_TX_DAT_ACK)
+        && (I2C_status != I2C_I2STAT_M_TX_DAT_NACK));  //wait for slaw to finish
+  }while ( (I2C_status == I2C_I2STAT_M_TX_DAT_ACK) ||  (I2C_status == I2C_I2STAT_M_TX_DAT_NACK));
+  return 1;
+}
 
-  void u8g_i2c_stop(void) {
-  }
+void u8g_i2c_init(uint8_t clock_option) {
 
-
-  #ifdef __cplusplus
-    }
+  /**
+   * Init I2C pin connect
+   */
+  PinCfg.OpenDrain = 0;
+  PinCfg.Pinmode = 0;
+  #if ((USEDI2CDEV_M == 0))
+    PinCfg.Funcnum = 1;
+    PinCfg.Pinnum = 27;
+    PinCfg.Portnum = 0;
+    PINSEL_ConfigPin(&PinCfg); // SDA0 / D57  AUX-1
+    PinCfg.Pinnum = 28;
+    PINSEL_ConfigPin(&PinCfg); // SCL0 / D58  AUX-1
   #endif
+  #if ((USEDI2CDEV_M == 1))
+    PinCfg.Funcnum = 3;
+    PinCfg.Pinnum = 0;
+    PinCfg.Portnum = 0;
+    PINSEL_ConfigPin(&PinCfg);  // SDA1 / D20 SCA
+    PinCfg.Pinnum = 1;
+    PINSEL_ConfigPin(&PinCfg);  // SCL1 / D21 SCL
+  #endif
+  #if ((USEDI2CDEV_M == 2))
+    PinCfg.Funcnum = 2;
+    PinCfg.Pinnum = 10;
+    PinCfg.Portnum = 0;
+    PINSEL_ConfigPin(&PinCfg); // SDA2 / D38  X_ENABLE_PIN
+    PinCfg.Pinnum = 11;
+    PINSEL_ConfigPin(&PinCfg); // SCL2 / D55  X_DIR_PIN
+  #endif
+  // Initialize I2C peripheral
+  I2C_Init(I2CDEV_M, (clock_option & U8G_I2C_OPT_FAST) ? 400000: 100000);  // LCD data rates
+
+  // Enable Master I2C operation
+  I2C_Cmd(I2CDEV_M, I2C_MASTER_MODE, ENABLE);
+
+  u8g_i2c_start(0); // send slave address and write bit
+}
+
+volatile extern uint32_t _millis;
+uint8_t u8g_i2c_send_byte(uint8_t data) {
+  #define I2C_TIMEOUT 3
+  LPC_I2C1->I2DAT = data & I2C_I2DAT_BITMASK; // transmit data
+  LPC_I2C1->I2CONSET = I2C_I2CONSET_AA;
+  LPC_I2C1->I2CONCLR = I2C_I2CONCLR_SIC;
+  uint32_t timeout = _millis + I2C_TIMEOUT;
+  while ((I2C_status != I2C_I2STAT_M_TX_DAT_ACK) && (I2C_status != I2C_I2STAT_M_TX_DAT_NACK) && (timeout > _millis));  // wait for xmit to finish
+  // had hangs with SH1106 so added time out - have seen temporary screen corruption when this happens
+  return 1;
+}
+
+void u8g_i2c_stop(void) {
+}
+
+
+#ifdef __cplusplus
+  }
 #endif
+
+#endif // TARGET_LPC1768
diff --git a/Marlin/src/HAL/HAL_LPC1768/LPC1768_PWM.h b/Marlin/src/HAL/HAL_LPC1768/LPC1768_PWM.h
index 177a05308..2f6ce39ec 100644
--- a/Marlin/src/HAL/HAL_LPC1768/LPC1768_PWM.h
+++ b/Marlin/src/HAL/HAL_LPC1768/LPC1768_PWM.h
@@ -60,7 +60,10 @@
  * See the end of this file for details on the hardware/firmware interaction
  */
 
-#include "fastio.h"
+#ifndef _LPC1768_PWM_H_
+#define _LPC1768_PWM_H_
+
+#include "pinmapping.h"
 
 #define LPC_PWM1_MR0 19999  // base repetition rate minus one count - 20mS
 #define LPC_PWM1_PR 24      // prescaler value - prescaler divide by 24 + 1  -  1 MHz output
@@ -73,3 +76,5 @@ bool LPC1768_PWM_attach_pin(pin_t pin, uint32_t min=1, uint32_t max=(LPC_PWM1_MR
 bool LPC1768_PWM_write(pin_t pin, uint32_t value);
 bool LPC1768_PWM_detach_pin(pin_t pin);
 bool useable_hardware_PWM(pin_t pin);
+
+#endif // _LPC1768_PWM_H_
diff --git a/Marlin/src/HAL/HAL_LPC1768/WInterrupts.cpp b/Marlin/src/HAL/HAL_LPC1768/WInterrupts.cpp
index 8c47092b2..1c73db03c 100644
--- a/Marlin/src/HAL/HAL_LPC1768/WInterrupts.cpp
+++ b/Marlin/src/HAL/HAL_LPC1768/WInterrupts.cpp
@@ -19,7 +19,7 @@
 #ifdef TARGET_LPC1768
 
 #include "../../inc/MarlinConfig.h"
-#include "arduino.h"
+#include "include/arduino.h"
 #include "pinmapping.h"
 //#include "HAL_timers.h"
 #include "fastio.h"
diff --git a/Marlin/src/HAL/HAL_LPC1768/arduino.cpp b/Marlin/src/HAL/HAL_LPC1768/arduino.cpp
index 4832c57f2..4b31a54bd 100644
--- a/Marlin/src/HAL/HAL_LPC1768/arduino.cpp
+++ b/Marlin/src/HAL/HAL_LPC1768/arduino.cpp
@@ -22,11 +22,11 @@
 
 #ifdef TARGET_LPC1768
 
-#include "../../inc/MarlinConfig.h"
 #include "LPC1768_PWM.h"
-
 #include <lpc17xx_pinsel.h>
 
+#include "../../inc/MarlinConfig.h"
+
 // Interrupts
 void cli(void) { __disable_irq(); } // Disable
 void sei(void) { __enable_irq(); }  // Enable
@@ -72,8 +72,7 @@ extern "C" void delay(const int msec) {
 // IO functions
 // As defined by Arduino INPUT(0x0), OUPUT(0x1), INPUT_PULLUP(0x2)
 void pinMode(pin_t pin, uint8_t mode) {
-  if (!VALID_PIN(pin))
-    return;
+  if (!VALID_PIN(pin)) return;
 
   PINSEL_CFG_Type config = { LPC1768_PIN_PORT(pin),
                              LPC1768_PIN_PIN(pin),
@@ -100,8 +99,7 @@ void pinMode(pin_t pin, uint8_t mode) {
 }
 
 void digitalWrite(pin_t pin, uint8_t pin_status) {
-  if (!VALID_PIN(pin))
-    return;
+  if (!VALID_PIN(pin)) return;
 
   if (pin_status)
     LPC_GPIO(LPC1768_PIN_PORT(pin))->FIOSET = LPC_PIN(LPC1768_PIN_PIN(pin));
@@ -120,20 +118,18 @@ void digitalWrite(pin_t pin, uint8_t pin_status) {
 }
 
 bool digitalRead(pin_t pin) {
-  if (!VALID_PIN(pin)) {
-    return false;
-  }
+  if (!VALID_PIN(pin)) return false;
+
   return LPC_GPIO(LPC1768_PIN_PORT(pin))->FIOPIN & LPC_PIN(LPC1768_PIN_PIN(pin)) ? 1 : 0;
 }
 
 void analogWrite(pin_t pin, int pwm_value) {  // 1 - 254: pwm_value, 0: LOW, 255: HIGH
+  if (!VALID_PIN(pin)) return;
+
   #define MR0_MARGIN 200       // if channel value too close to MR0 the system locks up
 
   static bool out_of_PWM_slots = false;
 
-  if (!VALID_PIN(pin))
-    return;
-
   uint value = MAX(MIN(pwm_value, 255), 0);
   if (value == 0 || value == 255) {  // treat as digital pin
     LPC1768_PWM_detach_pin(pin);    // turn off PWM
diff --git a/Marlin/src/HAL/HAL_LPC1768/pinmapping.h b/Marlin/src/HAL/HAL_LPC1768/pinmapping.h
index 76efe3c0d..c9ce96eef 100644
--- a/Marlin/src/HAL/HAL_LPC1768/pinmapping.h
+++ b/Marlin/src/HAL/HAL_LPC1768/pinmapping.h
@@ -22,8 +22,11 @@
 
 #ifndef __HAL_PINMAPPING_H__
 #define __HAL_PINMAPPING_H__
+
 #include "../../core/macros.h"
 
+#include <stdint.h>
+
 typedef int16_t pin_t;
 
 #define PORT_0  000
diff --git a/Marlin/src/HAL/HAL_STM32F1/HAL_Stm32f1.h b/Marlin/src/HAL/HAL_STM32F1/HAL_Stm32f1.h
index 3d6184227..354b5eff0 100644
--- a/Marlin/src/HAL/HAL_STM32F1/HAL_Stm32f1.h
+++ b/Marlin/src/HAL/HAL_STM32F1/HAL_Stm32f1.h
@@ -40,7 +40,7 @@
 
 #include <stdint.h>
 
-#include "Arduino.h"
+#include <Arduino.h>
 
 // --------------------------------------------------------------------------
 // Undefine DEBUG_ settings
diff --git a/Marlin/src/HAL/HAL_TEENSY35_36/HAL_Teensy.h b/Marlin/src/HAL/HAL_TEENSY35_36/HAL_Teensy.h
index 0ac8244e0..f4de615a6 100644
--- a/Marlin/src/HAL/HAL_TEENSY35_36/HAL_Teensy.h
+++ b/Marlin/src/HAL/HAL_TEENSY35_36/HAL_Teensy.h
@@ -30,7 +30,11 @@
 // Includes
 // --------------------------------------------------------------------------
 
-#include "Arduino.h"
+#include <Arduino.h>
+
+// Redefine sq macro defined by teensy3/wiring.h
+#undef sq
+#define sq(x) ((x)*(x))
 
 #include "fastio_Teensy.h"
 #include "watchdog_Teensy.h"
diff --git a/Marlin/src/HAL/SPI.h b/Marlin/src/HAL/SPI.h
index 78e867a1f..632c50b9a 100644
--- a/Marlin/src/HAL/SPI.h
+++ b/Marlin/src/HAL/SPI.h
@@ -28,12 +28,8 @@
 #ifndef _SPI_H_
 #define _SPI_H_
 
-//#include "../inc/MarlinConfig.h"
-
 #include <stdint.h>
 
-#ifndef SPI_FULL_SPEED
-
 /**
  * SPI speed where 0 <= index <= 6
  *
@@ -73,6 +69,4 @@ void spiRead(uint8_t* buf, uint16_t nbyte);
 /** Write token and then write from 512 byte buffer to SPI (for SD card) */
 void spiSendBlock(uint8_t token, const uint8_t* buf);
 
-#endif // SPI_FULL_SPEED
-
 #endif // _SPI_H_
diff --git a/Marlin/src/Marlin.cpp b/Marlin/src/Marlin.cpp
index 3f98c565b..0045b93a4 100644
--- a/Marlin/src/Marlin.cpp
+++ b/Marlin/src/Marlin.cpp
@@ -75,7 +75,7 @@
 #endif
 
 #if HAS_SERVOS
-  #include "HAL/servo.h"
+  #include "module/servo.h"
 #endif
 
 #if HAS_DIGIPOTSS
@@ -247,35 +247,6 @@ void setup_powerhold() {
   #endif
 }
 
-#if HAS_SERVOS
-
-  HAL_SERVO_LIB servo[NUM_SERVOS];
-
-  void servo_init() {
-    #if NUM_SERVOS >= 1 && HAS_SERVO_0
-      servo[0].attach(SERVO0_PIN);
-      servo[0].detach(); // Just set up the pin. We don't have a position yet. Don't move to a random position.
-    #endif
-    #if NUM_SERVOS >= 2 && HAS_SERVO_1
-      servo[1].attach(SERVO1_PIN);
-      servo[1].detach();
-    #endif
-    #if NUM_SERVOS >= 3 && HAS_SERVO_2
-      servo[2].attach(SERVO2_PIN);
-      servo[2].detach();
-    #endif
-    #if NUM_SERVOS >= 4 && HAS_SERVO_3
-      servo[3].attach(SERVO3_PIN);
-      servo[3].detach();
-    #endif
-
-    #if HAS_Z_SERVO_ENDSTOP
-      servo_probe_init();
-    #endif
-  }
-
-#endif // HAS_SERVOS
-
 /**
  * Stepper Reset (RigidBoard, et.al.)
  */
@@ -745,6 +716,10 @@ void setup() {
     servo_init();
   #endif
 
+  #if HAS_Z_SERVO_ENDSTOP
+    servo_probe_init();
+  #endif
+
   #if HAS_PHOTOGRAPH
     OUT_WRITE(PHOTOGRAPH_PIN, LOW);
   #endif
diff --git a/Marlin/src/Marlin.h b/Marlin/src/Marlin.h
index 79dba4fcf..a1b6741b9 100644
--- a/Marlin/src/Marlin.h
+++ b/Marlin/src/Marlin.h
@@ -22,16 +22,16 @@
 #ifndef __MARLIN_H__
 #define __MARLIN_H__
 
-#include <math.h>
-#include <stdio.h>
-#include <stdlib.h>
-
 #include "inc/MarlinConfig.h"
 
 #ifdef DEBUG_GCODE_PARSER
   #include "gcode/parser.h"
 #endif
 
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+
 void stop();
 
 void idle(
@@ -186,16 +186,6 @@ extern volatile bool wait_for_heatup;
 // Inactivity shutdown timer
 extern millis_t max_inactive_time, stepper_inactive_time;
 
-#if HAS_SERVOS
-  #include "HAL/servo.h"
-  extern HAL_SERVO_LIB servo[NUM_SERVOS];
-  #define MOVE_SERVO(I, P) servo[I].move(P)
-  #if HAS_Z_SERVO_ENDSTOP
-    #define DEPLOY_Z_SERVO() MOVE_SERVO(Z_ENDSTOP_SERVO_NR, z_servo_angle[0])
-    #define STOW_Z_SERVO() MOVE_SERVO(Z_ENDSTOP_SERVO_NR, z_servo_angle[1])
-  #endif
-#endif
-
 #if FAN_COUNT > 0
   extern int16_t fanSpeeds[FAN_COUNT];
   #if ENABLED(EXTRA_FAN_SPEED)
@@ -209,6 +199,11 @@ extern millis_t max_inactive_time, stepper_inactive_time;
 #endif
 
 #if ENABLED(ADVANCED_PAUSE_FEATURE)
+  enum AdvancedPauseMenuResponse {
+    ADVANCED_PAUSE_RESPONSE_WAIT_FOR,
+    ADVANCED_PAUSE_RESPONSE_EXTRUDE_MORE,
+    ADVANCED_PAUSE_RESPONSE_RESUME_PRINT
+  };
   extern AdvancedPauseMenuResponse advanced_pause_menu_response;
 #endif
 
diff --git a/Marlin/src/core/enum.h b/Marlin/src/core/enum.h
index f70e0ac1e..4f5d53fb1 100644
--- a/Marlin/src/core/enum.h
+++ b/Marlin/src/core/enum.h
@@ -23,8 +23,6 @@
 #ifndef __ENUM_H__
 #define __ENUM_H__
 
-#include "../inc/MarlinConfig.h"
-
 /**
  * Axis indices as enumerated constants
  *
@@ -69,46 +67,6 @@ typedef enum {
   TEMPUNIT_F
 } TempUnit;
 
-#if ENABLED(EMERGENCY_PARSER)
-  enum e_parser_state {
-    state_RESET,
-    state_N,
-    state_M,
-    state_M1,
-    state_M10,
-    state_M108,
-    state_M11,
-    state_M112,
-    state_M4,
-    state_M41,
-    state_M410,
-    state_IGNORE // to '\n'
-  };
-#endif
-
-#if ENABLED(ADVANCED_PAUSE_FEATURE)
-  enum AdvancedPauseMenuResponse {
-    ADVANCED_PAUSE_RESPONSE_WAIT_FOR,
-    ADVANCED_PAUSE_RESPONSE_EXTRUDE_MORE,
-    ADVANCED_PAUSE_RESPONSE_RESUME_PRINT
-  };
-
-  #if ENABLED(ULTIPANEL)
-    enum AdvancedPauseMessage {
-      ADVANCED_PAUSE_MESSAGE_INIT,
-      ADVANCED_PAUSE_MESSAGE_UNLOAD,
-      ADVANCED_PAUSE_MESSAGE_INSERT,
-      ADVANCED_PAUSE_MESSAGE_LOAD,
-      ADVANCED_PAUSE_MESSAGE_EXTRUDE,
-      ADVANCED_PAUSE_MESSAGE_OPTION,
-      ADVANCED_PAUSE_MESSAGE_RESUME,
-      ADVANCED_PAUSE_MESSAGE_STATUS,
-      ADVANCED_PAUSE_MESSAGE_CLICK_TO_HEAT_NOZZLE,
-      ADVANCED_PAUSE_MESSAGE_WAIT_FOR_NOZZLES_TO_HEAT
-    };
-  #endif
-#endif
-
 /**
  * SD Card
  */
@@ -125,15 +83,4 @@ enum LCDViewAction {
   LCDVIEW_CALL_NO_REDRAW
 };
 
-/**
- * Dual X Carriage modes. A Dual Nozzle can also do duplication.
- */
-#if ENABLED(DUAL_X_CARRIAGE) || ENABLED(DUAL_NOZZLE_DUPLICATION_MODE)
-  enum DualXMode {
-    DXC_FULL_CONTROL_MODE,  // DUAL_X_CARRIAGE only
-    DXC_AUTO_PARK_MODE,     // DUAL_X_CARRIAGE only
-    DXC_DUPLICATION_MODE
-  };
-#endif
-
 #endif // __ENUM_H__
diff --git a/Marlin/src/core/macros.h b/Marlin/src/core/macros.h
index 1265ddbbc..6342c1643 100644
--- a/Marlin/src/core/macros.h
+++ b/Marlin/src/core/macros.h
@@ -43,7 +43,6 @@
 #define _O2          __attribute__((optimize("O2")))
 #define _O3          __attribute__((optimize("O3")))
 
-
 // Clock speed factors
 #define CYCLES_PER_MICROSECOND (F_CPU / 1000000L) // 16 or 20
 #define INT0_PRESCALER 8
diff --git a/Marlin/src/core/serial.h b/Marlin/src/core/serial.h
index 887402a06..eeeb6155a 100644
--- a/Marlin/src/core/serial.h
+++ b/Marlin/src/core/serial.h
@@ -44,6 +44,23 @@ enum DebugFlags {
   DEBUG_ALL           = 0xFF
 };
 
+#if ENABLED(EMERGENCY_PARSER)
+  enum e_parser_state {
+    state_RESET,
+    state_N,
+    state_M,
+    state_M1,
+    state_M10,
+    state_M108,
+    state_M11,
+    state_M112,
+    state_M4,
+    state_M41,
+    state_M410,
+    state_IGNORE // to '\n'
+  };
+#endif
+
 //todo: HAL: breaks encapsulation
 // For AVR only, define a serial interface based on configuration
 #ifdef __AVR__
diff --git a/Marlin/src/feature/leds/blinkm.h b/Marlin/src/feature/leds/blinkm.h
index 5c7b1307a..d4b5b8d2e 100644
--- a/Marlin/src/feature/leds/blinkm.h
+++ b/Marlin/src/feature/leds/blinkm.h
@@ -28,7 +28,7 @@
 #ifndef __BLINKM_H__
 #define __BLINKM_H__
 
-#include "Arduino.h"
+#include <Arduino.h>
 #include <Wire.h>
 
 void blinkm_set_led_color(const uint8_t r, const uint8_t g, const uint8_t b);
diff --git a/Marlin/src/feature/leds/pca9632.h b/Marlin/src/feature/leds/pca9632.h
index b3ddfcd6e..0a8b48d3c 100644
--- a/Marlin/src/feature/leds/pca9632.h
+++ b/Marlin/src/feature/leds/pca9632.h
@@ -28,7 +28,7 @@
 #ifndef __PCA9632_H__
 #define __PCA9632_H__
 
-#include "Arduino.h"
+#include <Arduino.h>
 #include <Wire.h>
 
 void pca9632_set_led_color(const byte r, const byte g, const byte b);
diff --git a/Marlin/src/gcode/config/M43.cpp b/Marlin/src/gcode/config/M43.cpp
index 9b7fc4753..b600e9545 100644
--- a/Marlin/src/gcode/config/M43.cpp
+++ b/Marlin/src/gcode/config/M43.cpp
@@ -31,6 +31,7 @@
 
 #if HAS_Z_SERVO_ENDSTOP
   #include "../../module/probe.h"
+  #include "../../module/servo.h"
 #endif
 
 inline void toggle_pins() {
diff --git a/Marlin/src/gcode/control/M280.cpp b/Marlin/src/gcode/control/M280.cpp
index 9d0f5c4e7..1cfb6c89f 100644
--- a/Marlin/src/gcode/control/M280.cpp
+++ b/Marlin/src/gcode/control/M280.cpp
@@ -25,7 +25,7 @@
 #if HAS_SERVOS
 
 #include "../gcode.h"
-#include "../../Marlin.h" // for servo[]
+#include "../../module/servo.h"
 
 /**
  * M280: Get or set servo position. P<index> [S<angle>]
diff --git a/Marlin/src/inc/MarlinConfig.h b/Marlin/src/inc/MarlinConfig.h
index 5b2d5a5d8..c52c319ec 100644
--- a/Marlin/src/inc/MarlinConfig.h
+++ b/Marlin/src/inc/MarlinConfig.h
@@ -26,7 +26,6 @@
 #include "../core/boards.h"
 #include "../core/macros.h"
 #include "Version.h"
-#include "../HAL/SPI.h"
 #include "../../Configuration.h"
 #include "Conditionals_LCD.h"
 #include "../../Configuration_adv.h"
diff --git a/Marlin/src/inc/Version.h b/Marlin/src/inc/Version.h
index 3731b476f..6a08df0fb 100644
--- a/Marlin/src/inc/Version.h
+++ b/Marlin/src/inc/Version.h
@@ -27,7 +27,7 @@
 
 /**
  * This file is the standard Marlin version identifier file, all fields can be
- * overriden by the ones defined on _Version.h by using the Configuration.h
+ * overriden by the ones defined in _Version.h by using the Configuration.h
  * directive USE_AUTOMATIC_VERSIONING.
  */
 
@@ -53,7 +53,7 @@
    * here we define this default string as the date where the latest release
    * version was tagged.
    */
-  #define STRING_DISTRIBUTION_DATE "2017-10-19 12:00"
+  #define STRING_DISTRIBUTION_DATE "2017-11-19 12:00"
 
   /**
    * Required minimum Configuration.h and Configuration_adv.h file versions.
diff --git a/Marlin/src/lcd/dogm/u8g_dev_ssd1306_sh1106_128x64_I2C.cpp b/Marlin/src/lcd/dogm/u8g_dev_ssd1306_sh1106_128x64_I2C.cpp
index c17f3e5be..040d19541 100644
--- a/Marlin/src/lcd/dogm/u8g_dev_ssd1306_sh1106_128x64_I2C.cpp
+++ b/Marlin/src/lcd/dogm/u8g_dev_ssd1306_sh1106_128x64_I2C.cpp
@@ -69,6 +69,10 @@
  * beginning.
  */
 
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(DOGLCD)
+
 #include <U8glib.h>
 #include "HAL_LCD_com_defines.h"
 
@@ -308,3 +312,4 @@ uint8_t u8g_WriteEscSeqP_2_wire(u8g_t *u8g, u8g_dev_t *dev, const uint8_t *esc_s
   return 1;
 }
 
+#endif // DOGLCD
diff --git a/Marlin/src/lcd/dogm/u8g_dev_st7565_64128n_HAL.cpp b/Marlin/src/lcd/dogm/u8g_dev_st7565_64128n_HAL.cpp
index 0bd87ae20..891db0077 100644
--- a/Marlin/src/lcd/dogm/u8g_dev_st7565_64128n_HAL.cpp
+++ b/Marlin/src/lcd/dogm/u8g_dev_st7565_64128n_HAL.cpp
@@ -57,6 +57,10 @@
 
 */
 
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(DOGLCD)
+
 #include <U8glib.h>
 
 #include "HAL_LCD_com_defines.h"
@@ -226,4 +230,6 @@ u8g_dev_t u8g_dev_st7565_64128n_HAL_2x_sw_spi = { u8g_dev_st7565_64128n_HAL_2x_f
 
 
 U8G_PB_DEV(u8g_dev_st7565_64128n_HAL_hw_spi, WIDTH, HEIGHT, PAGE_HEIGHT, u8g_dev_st7565_64128n_HAL_fn, U8G_COM_HAL_HW_SPI_FN);
-u8g_dev_t u8g_dev_st7565_64128n_HAL_2x_hw_spi = { u8g_dev_st7565_64128n_HAL_2x_fn, &u8g_dev_st7565_64128n_HAL_2x_pb, U8G_COM_HAL_HW_SPI_FN };
\ No newline at end of file
+u8g_dev_t u8g_dev_st7565_64128n_HAL_2x_hw_spi = { u8g_dev_st7565_64128n_HAL_2x_fn, &u8g_dev_st7565_64128n_HAL_2x_pb, U8G_COM_HAL_HW_SPI_FN };
+
+#endif // DOGLCD
diff --git a/Marlin/src/lcd/dogm/u8g_dev_st7920_128x64_HAL.cpp b/Marlin/src/lcd/dogm/u8g_dev_st7920_128x64_HAL.cpp
index c5efe5158..baafc2c43 100644
--- a/Marlin/src/lcd/dogm/u8g_dev_st7920_128x64_HAL.cpp
+++ b/Marlin/src/lcd/dogm/u8g_dev_st7920_128x64_HAL.cpp
@@ -56,6 +56,10 @@
 
 */
 
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(DOGLCD)
+
 #include <U8glib.h>
 
 #include "HAL_LCD_com_defines.h"
@@ -200,3 +204,5 @@ u8g_dev_t u8g_dev_st7920_128x64_HAL_4x_hw_spi = { u8g_dev_st7920_128x64_HAL_4x_f
   // for the ST7920 for HAL systems no matter what is selected in ultralcd_impl_DOGM.h.
   u8g_dev_t u8g_dev_st7920_128x64_rrd_sw_spi = { u8g_dev_st7920_128x64_HAL_4x_fn, &u8g_dev_st7920_128x64_HAL_4x_pb, U8G_COM_ST7920_HAL_SW_SPI };
 #endif
+
+#endif // DOGLCD
diff --git a/Marlin/src/lcd/dogm/ultralcd_st7920_u8glib_rrd_AVR.cpp b/Marlin/src/lcd/dogm/ultralcd_st7920_u8glib_rrd_AVR.cpp
index 66a21d133..967a9edde 100644
--- a/Marlin/src/lcd/dogm/ultralcd_st7920_u8glib_rrd_AVR.cpp
+++ b/Marlin/src/lcd/dogm/ultralcd_st7920_u8glib_rrd_AVR.cpp
@@ -23,9 +23,13 @@
 // NOTE - the HAL version of the rrd device uses a generic ST7920 device.  See the
 // file u8g_dev_st7920_128x64_HAL.cpp for the HAL version.
 
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(DOGLCD)
+
 #ifndef U8G_HAL_LINKS
 
-#include "../../Marlin.h"
+//#include "../../Marlin.h"
 
 //#if ENABLED(U8GLIB_ST7920)
 //#if ( ENABLED(SHARED_SPI) || !ENABLED(SHARED_SPI) && (defined(LCD_PINS_D4) &&  LCD_PINS_D4 >= 0) &&  (defined(LCD_PINS_ENABLE) &&  LCD_PINS_ENABLE >= 0))
@@ -97,18 +101,15 @@
   #define U8G_DELAY() u8g_10MicroDelay()
 #endif
 
-
-
 static void ST7920_WRITE_BYTE(uint8_t val) {
   for (uint8_t i = 0; i < 8; i++) {
     WRITE(ST7920_DAT_PIN, val & 0x80);
     WRITE(ST7920_CLK_PIN, HIGH);
     WRITE(ST7920_CLK_PIN, LOW);
-    val = val << 1;
+    val <<= 1;
   }
 }
 
-
 #define ST7920_SET_CMD()         { ST7920_WRITE_BYTE(0xF8); U8G_DELAY(); }
 #define ST7920_SET_DAT()         { ST7920_WRITE_BYTE(0xFA); U8G_DELAY(); }
 #define ST7920_WRITE_NIBBLES(a)     { ST7920_WRITE_BYTE((uint8_t)((a)&0xF0u)); ST7920_WRITE_BYTE((uint8_t)((a)<<4u)); U8G_DELAY(); }
@@ -118,17 +119,13 @@ static void ST7920_WRITE_BYTE(uint8_t val) {
 #define ST7920_CS()              { WRITE(ST7920_CS_PIN,1); U8G_DELAY(); }
 #define ST7920_NCS()             { WRITE(ST7920_CS_PIN,0); }
 
-
-
 uint8_t u8g_dev_rrd_st7920_128x64_fn(u8g_t *u8g, u8g_dev_t *dev, uint8_t msg, void *arg) {
   uint8_t i, y;
   switch (msg) {
     case U8G_DEV_MSG_INIT: {
       OUT_WRITE(ST7920_CS_PIN, LOW);
-
-
-        OUT_WRITE(ST7920_DAT_PIN, LOW);
-        OUT_WRITE(ST7920_CLK_PIN, LOW);
+      OUT_WRITE(ST7920_DAT_PIN, LOW);
+      OUT_WRITE(ST7920_CLK_PIN, LOW);
 
       ST7920_CS();
       u8g_Delay(120);                 //initial delay for boot up
@@ -197,4 +194,7 @@ u8g_dev_t u8g_dev_st7920_128x64_rrd_sw_spi = {u8g_dev_rrd_st7920_128x64_fn, &u8g
 
 //#endif //( ENABLED(SHARED_SPI) || !ENABLED(SHARED_SPI) && (defined(LCD_PINS_D4) &&  LCD_PINS_D4 >= 0) &&  (defined(LCD_PINS_ENABLE) &&  LCD_PINS_ENABLE >= 0))
 //#endif // U8GLIB_ST7920
-#endif // AVR
+
+#endif // U8G_HAL_LINKS
+
+#endif // DOGLCD
diff --git a/Marlin/src/lcd/ultralcd.h b/Marlin/src/lcd/ultralcd.h
index e0b7b6f11..0301cf4f5 100644
--- a/Marlin/src/lcd/ultralcd.h
+++ b/Marlin/src/lcd/ultralcd.h
@@ -108,6 +108,18 @@
     void lcd_completion_feedback(const bool good=true);
 
     #if ENABLED(ADVANCED_PAUSE_FEATURE)
+      enum AdvancedPauseMessage {
+        ADVANCED_PAUSE_MESSAGE_INIT,
+        ADVANCED_PAUSE_MESSAGE_UNLOAD,
+        ADVANCED_PAUSE_MESSAGE_INSERT,
+        ADVANCED_PAUSE_MESSAGE_LOAD,
+        ADVANCED_PAUSE_MESSAGE_EXTRUDE,
+        ADVANCED_PAUSE_MESSAGE_OPTION,
+        ADVANCED_PAUSE_MESSAGE_RESUME,
+        ADVANCED_PAUSE_MESSAGE_STATUS,
+        ADVANCED_PAUSE_MESSAGE_CLICK_TO_HEAT_NOZZLE,
+        ADVANCED_PAUSE_MESSAGE_WAIT_FOR_NOZZLES_TO_HEAT
+      };
       void lcd_advanced_pause_show_message(const AdvancedPauseMessage message);
     #endif
 
diff --git a/Marlin/src/module/motion.h b/Marlin/src/module/motion.h
index 7edbd29b3..0c88af4c6 100644
--- a/Marlin/src/module/motion.h
+++ b/Marlin/src/module/motion.h
@@ -292,6 +292,12 @@ void homeaxis(const AxisEnum axis);
  */
 #if ENABLED(DUAL_X_CARRIAGE)
 
+  enum DualXMode {
+    DXC_FULL_CONTROL_MODE,  // DUAL_X_CARRIAGE only
+    DXC_AUTO_PARK_MODE,     // DUAL_X_CARRIAGE only
+    DXC_DUPLICATION_MODE
+  };
+
   extern DualXMode dual_x_carriage_mode;
   extern float inactive_extruder_x_pos,           // used in mode 0 & 1
                raised_parked_position[XYZE],      // used in mode 1
@@ -304,7 +310,13 @@ void homeaxis(const AxisEnum axis);
 
   FORCE_INLINE int x_home_dir(const uint8_t extruder) { return extruder ? X2_HOME_DIR : X_HOME_DIR; }
 
-#endif // DUAL_X_CARRIAGE
+#elif ENABLED(DUAL_NOZZLE_DUPLICATION_MODE)
+
+  enum DualXMode {
+    DXC_DUPLICATION_MODE = 2
+  };
+
+#endif
 
 #if HAS_WORKSPACE_OFFSET || ENABLED(DUAL_X_CARRIAGE)
   void update_software_endstops(const AxisEnum axis);
diff --git a/Marlin/src/module/probe.cpp b/Marlin/src/module/probe.cpp
index 43ab0f9d9..127c6b003 100644
--- a/Marlin/src/module/probe.cpp
+++ b/Marlin/src/module/probe.cpp
@@ -53,6 +53,7 @@
 float zprobe_zoffset; // Initialized by settings.load()
 
 #if HAS_Z_SERVO_ENDSTOP
+  #include "../module/servo.h"
   const int z_servo_angle[2] = Z_SERVO_ANGLES;
 #endif
 
diff --git a/Marlin/src/module/probe.h b/Marlin/src/module/probe.h
index 012d3d801..be7a3390a 100644
--- a/Marlin/src/module/probe.h
+++ b/Marlin/src/module/probe.h
@@ -29,11 +29,10 @@
 
 #include "../inc/MarlinConfig.h"
 
-bool set_probe_deployed(const bool deploy);
-float probe_pt(const float &rx, const float &ry, const bool, const uint8_t, const bool printable=true);
-
 #if HAS_BED_PROBE
   extern float zprobe_zoffset;
+  bool set_probe_deployed(const bool deploy);
+  float probe_pt(const float &rx, const float &ry, const bool, const uint8_t, const bool printable=true);
   #define DEPLOY_PROBE() set_probe_deployed(true)
   #define STOW_PROBE() set_probe_deployed(false)
 #else
diff --git a/Marlin/src/module/servo.cpp b/Marlin/src/module/servo.cpp
new file mode 100644
index 000000000..a1f9633f7
--- /dev/null
+++ b/Marlin/src/module/servo.cpp
@@ -0,0 +1,54 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * module/servo.cpp
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if HAS_SERVOS
+
+#include "servo.h"
+
+HAL_SERVO_LIB servo[NUM_SERVOS];
+
+void servo_init() {
+  #if NUM_SERVOS >= 1 && HAS_SERVO_0
+    servo[0].attach(SERVO0_PIN);
+    servo[0].detach(); // Just set up the pin. We don't have a position yet. Don't move to a random position.
+  #endif
+  #if NUM_SERVOS >= 2 && HAS_SERVO_1
+    servo[1].attach(SERVO1_PIN);
+    servo[1].detach();
+  #endif
+  #if NUM_SERVOS >= 3 && HAS_SERVO_2
+    servo[2].attach(SERVO2_PIN);
+    servo[2].detach();
+  #endif
+  #if NUM_SERVOS >= 4 && HAS_SERVO_3
+    servo[3].attach(SERVO3_PIN);
+    servo[3].detach();
+  #endif
+}
+
+#endif // HAS_SERVOS
diff --git a/Marlin/src/module/servo.h b/Marlin/src/module/servo.h
new file mode 100644
index 000000000..cb5080e2b
--- /dev/null
+++ b/Marlin/src/module/servo.h
@@ -0,0 +1,44 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * module/servo.h
+ */
+
+#ifndef _SERVO_H_
+#define _SERVO_H_
+
+#include "../HAL/servo.h"
+
+extern HAL_SERVO_LIB servo[NUM_SERVOS];
+extern void servo_init();
+
+#define MOVE_SERVO(I, P) servo[I].move(P)
+
+#include "../inc/MarlinConfig.h"
+
+#if HAS_Z_SERVO_ENDSTOP
+  #define DEPLOY_Z_SERVO() MOVE_SERVO(Z_ENDSTOP_SERVO_NR, z_servo_angle[0])
+  #define STOW_Z_SERVO() MOVE_SERVO(Z_ENDSTOP_SERVO_NR, z_servo_angle[1])
+#endif
+
+#endif // _SERVO_H_
diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp
index 98f2b326a..4793f7687 100644
--- a/Marlin/src/module/stepper.cpp
+++ b/Marlin/src/module/stepper.cpp
@@ -54,12 +54,12 @@
 #include "planner.h"
 #include "motion.h"
 
-#include "../Marlin.h"
 #include "../module/temperature.h"
 #include "../lcd/ultralcd.h"
 #include "../core/language.h"
 #include "../gcode/queue.h"
 #include "../sd/cardreader.h"
+#include "../Marlin.h"
 
 #if MB(ALLIGATOR)
   #include "../feature/dac/dac_dac084s085.h"
diff --git a/Marlin/src/module/tool_change.cpp b/Marlin/src/module/tool_change.cpp
index 10210653e..cb74c5913 100644
--- a/Marlin/src/module/tool_change.cpp
+++ b/Marlin/src/module/tool_change.cpp
@@ -34,6 +34,10 @@
   #include "../gcode/gcode.h" // for dwell()
 #endif
 
+#if ENABLED(SWITCHING_EXTRUDER) || ENABLED(SWITCHING_NOZZLE)
+  #include "../module/servo.h"
+#endif
+
 #if ENABLED(EXT_SOLENOID) && !ENABLED(PARKING_EXTRUDER)
   #include "../feature/solenoid.h"
 #endif