diff --git a/Documentation/Compilation.md b/Documentation/Compilation.md
index 56b4f356ca7d00f5d8b0184deb5766f87ccb8d73..93c23a627d398367c5d5322a61a29d215f030e80 100644
--- a/Documentation/Compilation.md
+++ b/Documentation/Compilation.md
@@ -3,14 +3,14 @@
   1. Install the latest non-beta arduino software IDE/toolset: http://www.arduino.cc/en/Main/Software
   2. Download the Marlin firmware
     - [Latest developement version](https://github.com/MarlinFirmware/Marlin/tree/Development)
-    - [Stable version](https://github.com/MarlinFirmware/Marlin/tree/Development)
+    - [Stable version]()
   3. In both cases use the "Download Zip" button on the right.
   4. Some boards require special files and/or libraries from the ArduinoAddons directory. Take a look at the dedicated [README](/ArduinoAddons/README.md) for details.
   5. Start the arduino IDE.
-  6. Select Tools -> Board -> Arduino Mega 2560    or your microcontroller
+  6. Select Tools -> Board -> Arduino Mega 2560 or your microcontroller
   7. Select the correct serial port in Tools ->Serial Port
   8. Open Marlin.pde or .ino
   9. Click the Verify/Compile button
   10. Click the Upload button. If all goes well the firmware is uploading
 
-That's ok.  Enjoy Silky Smooth Printing.
\ No newline at end of file
+That's ok.  Enjoy Silky Smooth Printing.
diff --git a/Documentation/GCodes.md b/Documentation/GCodes.md
index 57b95cdeea3b83ad07dcb7e334440aa5f446d130..c05be6198baf11f12bc2d9d62c1b2484b5106ac1 100644
--- a/Documentation/GCodes.md
+++ b/Documentation/GCodes.md
@@ -1,108 +1,176 @@
-# Implemented G Codes
+## Implemented G Codes
 
-## G Codes
+### Movement G and M Codes
+```
+G0   -> G1
+G1   - Coordinated Movement X Y Z E
+G2   - CW ARC
+G3   - CCW ARC
+G4   - Dwell S[seconds] or P[milliseconds]
+G92  - Set the "current position" to coordinates: X<xpos> Y<ypos> Z<zpos> E<epos>
+M0   - Wait for user, with optional prompt message (requires LCD controller)
+       M0 Click When Ready ; show "Click When Ready" until the button is pressed.
+M1   - Same as M0
+M400 - Finish all moves
+M999 - Restart after being stopped by error
+```
+### SD Card M Codes
+```
+M20  - List SD card
+M21  - Init SD card
+M22  - Release SD card
+M23  - Select SD file (M23 filename.g)
+M24  - Start/resume SD print
+M25  - Pause SD print
+M26  - Set SD position in bytes (M26 S12345)
+M27  - Report SD print status
+M28  - Start SD write (M28 filename.g)
+M29  - Stop SD write
+M30  - Delete file from SD (M30 filename.g)
+M31  - Output time since last M109 or SD card start to serial
+M32  - Select file and start SD print (Can be used _while_ printing from SD card files):
+       syntax "M32 /path/filename#", or "M32 S<startpos bytes> !filename#"
+       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
+M540 - Enable/Disable "Stop SD Print on Endstop Hit" (req. ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
+       M540 S<0|1>
+M928 - Start SD logging (M928 filename.g) - ended by M29
+```
+### Hardware Control
+```
+M42  - Change pin status via gcode: P<pin> S<value>. If P is omitted the onboard LED pin will be used.
+       M42 P9 S1 ; set PIN 9 to 1. 
+M80  - Turn on Power Supply
+M81  - Turn off Power Supply
+M84  - Disable steppers until next move,
+       or use S<seconds> to specify an inactivity timeout, after which the steppers will be disabled.  S0 to disable the timeout.
+M85  - Set inactivity shutdown timer with parameter S<seconds>. Disable with "M85" or "M85 S0".
+M112 - Emergency stop. Requires hardware reset!!
+M226 - Wait for a pin to be in some state: P<pin number> S<pin state>
+```
+### Temperature M Codes
+```
+M104 - Set extruder target temp
+M105 - Read current temp
+M106 - Fan on
+M107 - Fan off
+M109 - Sxxx Wait for extruder current temp to reach target temp. Waits only when heating
+       Rxxx Wait for extruder current temp to reach target temp. Waits when heating and cooling
+       IF AUTOTEMP is enabled, S<mintemp> B<maxtemp> F<factor>. Exit autotemp by any M109 without F
+M140 - Set bed target temp
+M190 - Sxxx Wait for bed current temp to reach target temp. Waits only when heating
+       Rxxx Wait for bed current temp to reach target temp. Waits when heating and cooling
+M301 - Set PID parameters P I and D
+M302 - Allow cold extrudes, or set the minimum extrude S<temperature>.
+M303 - PID relay autotune S<temperature> sets the target temperature. (default target temperature = 150C)
+M304 - Set bed PID parameters P I and D
+```
+### Message M Codes
+```
+M114 - Output current position to serial port
+M115 - Capabilities string
+M117 - Display message text on the LCD
+M119 - Output Endstop status to serial port
+```
+### Endstops M Codes
+```
+M120 - Enable endstop detection
+M121 - Disable endstop detection
+```
+### Special Features M Codes
+```
+M126 - Solenoid Air Valve Open (BariCUDA support by jmil)
+M127 - Solenoid Air Valve Closed (BariCUDA vent to atmospheric pressure by jmil)
+M128 - EtoP Open (BariCUDA EtoP = electricity to air pressure transducer by jmil)
+M129 - EtoP Closed (BariCUDA EtoP = electricity to air pressure transducer by jmil)
+M150 - Set BlinkM Color: R<red> U<green> B<blue> via I2C. Range: 0-255
+M240 - Trigger a camera to take a photograph. (Add to your per-layer GCode.)
+M250 - Set LCD contrast: C<0-63>
+M280 - Set servo position absolute. P<servo index> S<angle or microseconds>
+M300 - Play beep sound S<frequency Hz> P<duration ms>
+M380 - Activate solenoid on active extruder
+M381 - Disable all solenoids
+M600 - Pause for filament change X<xpos> Y<ypos> Z<lift> E<initial retract> L<later retract for removal>
+```
+### Units and Measures G and M Codes
+```
+G90  - Use Absolute Coordinates
+G91  - Use Relative Coordinates
+M82  - Set E codes absolute (default)
+M83  - Set E codes relative while in Absolute Coordinates (G90) mode
+M92  - Set axis_steps_per_unit - same syntax as G92
+M200 - Set filament diameter and set E axis units to mm^3 (use S0 to set back to mm).: D<mm>
+M201 - Set max acceleration in units/s^2 for print moves (M201 X1000 Y1000)
+M202 - Set max acceleration in units/s^2 for travel moves (M202 X1000 Y1000) Unused in Marlin!!
+M203 - Set maximum feedrate that your machine can sustain (M203 X200 Y200 Z300 E10000) in mm/sec
+M204 - Set default acceleration: P for Printing moves, R for Retract only (no X, Y, Z) moves and T for Travel (non printing) moves (ex. M204 P800 T3000 R9000) in mm/sec^2
+M205 - Advanced settings: minimum travel speed S=while printing T=travel only, B=minimum segment time X=maximum xy jerk, Z=maximum Z jerk, E=maximum E jerk
+M206 - Set additional homing offset
+M218 - Set hotend offset (in mm): T<extruder_number> X<offset_on_X> Y<offset_on_Y>
+M220 - Set speed factor override percentage: S<factor in percent>
+M221 - Set extrude factor override percentage: S<factor in percent>
+```
+### Firmware Retraction G and M Codes
+```
+G10  - Retract filament according to settings of M207
+G11  - Retract recover filament according to settings of M208
+M207 - Set retract length S[positive mm] F[feedrate mm/min] Z[additional zlift/hop], stays in mm regardless of M200 setting
+M208 - Set recover=unretract length S[positive mm surplus to the M207 S*] F[feedrate mm/s]
+M209 - S<1=true/0=false> enable automatic retract detect if the slicer did not support G10/11: every normal extrude-only move will be classified as retract depending on the direction
+```
+### Z Probe G and M Codes
+```
+G28  - Home all Axis
+G29  - Detailed Z-Probe, probes the bed at 3 or more points. The printer must be homed with G28 before G29.
+G30  - Single Z Probe, probes bed at current XY location.
+G31  - Dock Z Probe sled (if enabled)
+G32  - Undock Z Probe sled (if enabled)
+M48  - Measure Z_Probe repeatability: P<points> X<xpos> Y<ypos> V<verbosity> E=engage L<legs of travel>
+       As with G29, the E flag causes the probe to stow after each probe.
+M401 - Lower Z-probe (if present)
+M402 - Raise Z-probe (if present)
+```
+### Filament Diameter M Codes
+```
+M404 - Set (or display) Nominal Filament Diameter in mm: [N<diameter>] (e.g., 3mm or 1.75mm)
+M405 - Turn on Filament Sensor extrusion control: [D<distance>] to set distance (in cm) from sensor to extruder
+M406 - Turn off Filament Sensor extrusion control
+M407 - Display measured Filament Diameter
+```
+### EEPROM Settings M Codes
+```
+M500 - Store parameters in EEPROM
+M501 - Read parameters from EEPROM (if you need reset them after you changed them temporarily).
+M502 - Revert to the default "factory settings". You still need to store them in EEPROM afterwards if you want to.
+M503 - Print the current settings (from memory not from EEPROM). Use S0 to leave off headings.
+```
+### Delta M Codes
+```
+M665 - Set Delta configurations: L<diagonal rod> R<delta radius> S<segments/s>
+M666 - Set Delta endstop adjustment: X<x-adjustment> Y<y-adjustment> Z<z-adjustment>
+M605 - Set dual x-carriage movement mode: S<mode> [ X<duplication x-offset> R<duplication temp offset> ]
+```
+### Stepper Driver M Codes
+```
+M17  - Enable/Power all stepper motors
+M18  - Disable all stepper motors. (same as M84)
+M907 - Set digital trimpot motor current using axis codes.
+M908 - Control digital trimpot directly.
+M350 - Set microstepping mode.
+M351 - Toggle MS1 MS2 pins directly.
+```
+### SCARA M-Codes
+__May change to suit future G-code standards__
+```
+M360 - SCARA calibration: Move to cal-position ThetaA (0 deg calibration)
+M361 - SCARA calibration: Move to cal-position ThetaB (90 deg calibration - steps per degree)
+M362 - SCARA calibration: Move to cal-position PsiA (0 deg calibration)
+M363 - SCARA calibration: Move to cal-position PsiB (90 deg calibration - steps per degree)
+M364 - SCARA calibration: Move to cal-position PSIC (90 deg to Theta calibration position)
+M365 - SCARA calibration: Scaling factor, X, Y, Z axis
+```
 
-*  G0  -> G1
-*  G1  - Coordinated Movement X Y Z E
-*  G2  - CW ARC
-*  G3  - CCW ARC
-*  G4  - Dwell S[seconds] or P[milliseconds]
-*  G10 - retract filament according to settings of M207
-*  G11 - retract recover filament according to settings of M208
-*  G28 - Home all Axis
-*  G29 - Detailed Z-Probe, probes the bed at 3 points.  You must be at the home position for this to work correctly.
-*  G30 - Single Z Probe, probes bed at current XY location.
-*  G31 - Dock Z Probe sled (if enabled)
-*  G32 - Undock Z Probe sled (if enabled)
-*  G90 - Use Absolute Coordinates
-*  G91 - Use Relative Coordinates
-*  G92 - Set current position to cordinates given
-
-## M Codes
-*  M0   - Unconditional stop - Wait for user to press a button on the LCD (Only if ULTRA_LCD is enabled)
-*  M1   - Same as M0
-*  M17  - Enable/Power all stepper motors
-*  M18  - Disable all stepper motors; same as M84
-*  M20  - List SD card
-*  M21  - Init SD card
-*  M22  - Release SD card
-*  M23  - Select SD file (M23 filename.g)
-*  M24  - Start/resume SD print
-*  M25  - Pause SD print
-*  M26  - Set SD position in bytes (M26 S12345)
-*  M27  - Report SD print status
-*  M28  - Start SD write (M28 filename.g)
-*  M29  - Stop SD write
-*  M30  - Delete file from SD (M30 filename.g)
-*  M31  - Output time since last M109 or SD card start to serial
-*  M32  - Select file and start SD print (Can be used when printing from SD card)
-*  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.
-*  M80  - Turn on Power Supply
-*  M81  - Turn off Power Supply
-*  M82  - Set E codes absolute (default)
-*  M83  - Set E codes relative while in Absolute Coordinates (G90) mode
-*  M84  - Disable steppers until next move, or use S[seconds] to specify an inactivity timeout, after which the steppers will be disabled.  S0 to disable the timeout.
-*  M85  - Set inactivity shutdown timer with parameter S[seconds]. To disable set zero (default)
-*  M92  - Set axis_steps_per_unit - same syntax as G92
-*  M104 - Set extruder target temp
-*  M105 - Read current temp
-*  M106 - Fan on
-*  M107 - Fan off
-*  M109 - Sxxx Wait for extruder current temp to reach target temp. Waits only when heating
-*         Rxxx Wait for extruder current temp to reach target temp. Waits when heating and cooling
-*  M112 - Emergency stop
-*  M114 - Output current position to serial port
-*  M115 - Capabilities string
-*  M117 - display message
-*  M119 - Output Endstop status to serial port
-*  M126 - Solenoid Air Valve Open (BariCUDA support by jmil)
-*  M127 - Solenoid Air Valve Closed (BariCUDA vent to atmospheric pressure by jmil)
-*  M128 - EtoP Open (BariCUDA EtoP = electricity to air pressure transducer by jmil)
-*  M129 - EtoP Closed (BariCUDA EtoP = electricity to air pressure transducer by jmil)
-*  M140 - Set bed target temp
-*  M190 - Sxxx Wait for bed current temp to reach target temp. Waits only when heating
-*         Rxxx Wait for bed current temp to reach target temp. Waits when heating and cooling
-*  M200 - D[millimeters]- set filament diameter and set E axis units to cubic millimeters (use S0 to set back to millimeters).
-*  M201 - Set max acceleration in units/s^2 for print moves (M201 X1000 Y1000)
-*  M202 - Set max acceleration in units/s^2 for travel moves (M202 X1000 Y1000) Unused in Marlin!!
-*  M203 - Set maximum feedrate that your machine can sustain (M203 X200 Y200 Z300 E10000) in mm/sec
-*  M204 - Set default acceleration: P for Printing moves, R for Retract only (no X, Y, Z) moves and T for Travel (non printing) moves (ex. M204 P800 T3000 R9000) in mm/sec^2
-*  M205 - advanced settings:  minimum travel speed S=while printing T=travel only,  B=minimum segment time X= maximum xy jerk, Z=maximum Z jerk, E=maximum E jerk
-*  M206 - set additional homing offset
-*  M207 - set retract length S[positive mm] F[feedrate mm/min] Z[additional zlift/hop], stays in mm regardless of M200 setting
-*  M208 - set recover=unretract length S[positive mm surplus to the M207 S*] F[feedrate mm/min]
-*  M209 - S[1=true/0=false] enable automatic retract detect if the slicer did not support G10/11: every normal extrude-only move will be classified as retract depending on the direction.
-*  M218 - set hotend offset (in mm): T[extruder_number] X[offset_on_X] Y[offset_on_Y]
-*  M220 - S[factor in percent] - set speed factor override percentage
-*  M221 - S[factor in percent] - set extrude factor override percentage
-*  M240 - Trigger a camera to take a photograph
-*  M280 - Position an RC Servo P[index] S[angle/microseconds], ommit S to report back current angle
-*  M300 - Play beep sound S[frequency Hz] P[duration ms]
-*  M301 - Set PID parameters P I and D
-*  M302 - Allow cold extrudes
-*  M303 - PID relay autotune S[temperature] sets the target temperature. (default target temperature = 150C)
-*  M304 - Set bed PID parameters P I and D
-*  M350 - Set microstepping mode.
-*  M351 - Toggle MS1 MS2 pins directly.
-*  M400 - Finish all moves
-*  M401 - Lower z-probe if present
-*  M402 - Raise z-probe if present
-*  M404 - N[dia in mm] Enter the nominal filament width (3mm, 1.75mm) or will display nominal filament width without parameters
-*  M405 - Turn on Filament Sensor extrusion control.  Optional D[delay in cm] to set delay in centimeters between sensor and extruder
-*  M406 - Turn off Filament Sensor extrusion control
-*  M407 - Displays measured filament diameter
-*  M500 - stores paramters in EEPROM
-*  M501 - reads parameters from EEPROM (if you need reset them after you changed them temporarily).
-*  M502 - reverts to the default "factory settings".  You still need to store them in EEPROM afterwards if you want to.
-*  M503 - print the current settings (from memory not from EEPROM)
-*  M540 - Use S[0|1] to enable or disable the stop SD card print on endstop hit (requires ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
-*  M600 - Pause for filament change X[pos] Y[pos] Z[relative lift] E[initial retract] L[later retract distance for removal]
-*  M907 - Set digital trimpot motor current using axis codes.
-*  M908 - Control digital trimpot directly.
-*  M928 - Start SD logging (M928 filename.g) - ended by M29
-*  M999 - Restart after being stopped by error
-
-# Comments
+## Comments
 
 Comments start at a `;` (semicolon) and end with the end of the line:
 
@@ -122,4 +190,4 @@ If you need to use a literal `;` somewhere (for example within `M117`), you can
 
     M117 backslash: \\;and a comment
 
-Please note that hosts should strip any comments before sending GCODE to the printer in order to save bandwidth.
\ No newline at end of file
+Please note that hosts should strip any comments before sending GCODE to the printer in order to save bandwidth.
diff --git a/Documentation/LCDLanguageFont.md b/Documentation/LCDLanguageFont.md
index 235cc1f3b70e0bf18505c18d04ee77840e8ef64a..befc526db546851ebfa882e2806ae0cd967433d4 100644
--- a/Documentation/LCDLanguageFont.md
+++ b/Documentation/LCDLanguageFont.md
@@ -47,11 +47,9 @@ We have two different technologies for the displays:
   * nl    Dutch
   * ca    Catalan
   * eu    Basque-Euskera
+  * cn    Chinese
+  * jp    Japanese (Katakana)
 
-  and recently on [Thingiverse](http://www.thingiverse.com/) a new port to
-  * jp    [Japanese](http://www.thingiverse.com/thing:664397)
-
- appeared.
 
 ## The Problem
   All of this languages, except the English, normally use extended symbol sets, not contained in US-ASCII.
@@ -133,6 +131,9 @@ We have two different technologies for the displays:
 
   You'll find all translatable strings in 'language_en.h'. Please don't translate any strings from 'language.h', this may break the serial protocol.
 
+  For information about fonts see: Marlin\fonts\README.fonts
+
+
 ## User Instructions
    Define your hardware and the wanted language in 'Configuration.h'.
    To find out what charset your hardware is, define language 'test' and compile. In the menu you will see two lines from the upper half of the charset.
diff --git a/Marlin/Conditionals.h b/Marlin/Conditionals.h
index 5d3213e0e35864524ed707e2a7e80da6e9f570f3..fa574083d82fb8449dbabd2b2f59d2dd5059a910 100644
--- a/Marlin/Conditionals.h
+++ b/Marlin/Conditionals.h
@@ -130,7 +130,6 @@
      #define NEWPANEL
   #endif
 
-
   #ifdef ULTIPANEL
     #define NEWPANEL  //enable this if you have a click-encoder panel
     #define SDSUPPORT
@@ -144,16 +143,45 @@
     #endif
   #else //no panel but just LCD
     #ifdef ULTRA_LCD
-    #ifdef DOGLCD // Change number of lines to match the 128x64 graphics display
-      #define LCD_WIDTH 22
-      #define LCD_HEIGHT 5
-    #else
-      #define LCD_WIDTH 16
-      #define LCD_HEIGHT 2
-    #endif
+      #ifdef DOGLCD // Change number of lines to match the 128x64 graphics display
+        #define LCD_WIDTH 22
+        #define LCD_HEIGHT 5
+      #else
+        #define LCD_WIDTH 16
+        #define LCD_HEIGHT 2
+      #endif
     #endif
   #endif
 
+  #ifdef DOGLCD
+    /* Custom characters defined in font font_6x10_marlin_symbols */
+    // \x00 intentionally skipped to avoid problems in strings
+    #define LCD_STR_REFRESH     "\x01"
+    #define LCD_STR_FOLDER      "\x02"
+    #define LCD_STR_ARROW_RIGHT "\x03"
+    #define LCD_STR_UPLEVEL     "\x04"
+    #define LCD_STR_CLOCK       "\x05"
+    #define LCD_STR_FEEDRATE    "\x06"
+    #define LCD_STR_BEDTEMP     "\x07"
+    #define LCD_STR_THERMOMETER "\x08"
+    #define LCD_STR_DEGREE      "\x09"
+
+    #define LCD_STR_SPECIAL_MAX '\x09'
+    // Maximum here is 0x1f because 0x20 is ' ' (space) and the normal charsets begin.
+    // Better stay below 0x10 because DISPLAY_CHARSET_HD44780_WESTERN begins here.
+  #else
+    /* Custom characters defined in the first 8 characters of the LCD */
+    #define LCD_STR_BEDTEMP     "\x00"  // this will have 'unexpected' results when used in a string!
+    #define LCD_STR_DEGREE      "\x01"
+    #define LCD_STR_THERMOMETER "\x02"
+    #define LCD_STR_UPLEVEL     "\x03"
+    #define LCD_STR_REFRESH     "\x04"
+    #define LCD_STR_FOLDER      "\x05"
+    #define LCD_STR_FEEDRATE    "\x06"
+    #define LCD_STR_CLOCK       "\x07"
+    #define LCD_STR_ARROW_RIGHT ">"  /* from the default character set */
+  #endif
+
   /**
    * Default LCD contrast for dogm-like LCD displays
    */
@@ -161,6 +189,13 @@
     #define DEFAULT_LCD_CONTRAST 32
   #endif
 
+  #ifdef DOGLCD
+    #define HAS_LCD_CONTRAST
+    #ifdef U8GLIB_ST7920
+      #undef HAS_LCD_CONTRAST
+    #endif
+  #endif
+
 #else // CONFIGURATION_LCD
 
   #define CONDITIONALS_H
diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index 63ce9aee8b97e426f612051b2b9d2c3a385549a6..63dcee53546cd938ab56e032c3c885d55562cc56 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -41,7 +41,6 @@ Here are some standard links for getting your machine calibrated:
 // startup. Implementation of an idea by Prof Braino to inform user that any changes made to this
 // build by the user have been successfully uploaded into firmware.
 #define STRING_VERSION "1.0.3 dev"
-#define STRING_URL "reprap.org"
 #define STRING_VERSION_CONFIG_H __DATE__ " " __TIME__ // build date and time
 #define STRING_CONFIG_H_AUTHOR "(none, default config)" // Who made the changes.
 #define STRING_SPLASH_LINE1 "v" STRING_VERSION // will be shown during bootup in line 1
@@ -68,8 +67,9 @@ Here are some standard links for getting your machine calibrated:
   #define MOTHERBOARD BOARD_RAMPS_13_EFB
 #endif
 
-// Define this to set a custom name for your generic Mendel,
-// #define CUSTOM_MENDEL_NAME "This Mendel"
+// Optional custom name for your RepStrap or other custom machine
+// Displayed in the LCD "Ready" message
+// #define CUSTOM_MACHINE_NAME "3D Printer"
 
 // Define this to set a unique identifier for this printer, (Used by some programs to differentiate between machines)
 // You can use an online service to generate a random UUID. (eg http://www.uuidgenerator.net/version4)
@@ -363,6 +363,7 @@ const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic
 #define E_ENABLE_ON 0 // For all extruders
 
 // Disables axis when it's not being used.
+// WARNING: When motors turn off there is a chance of losing position accuracy!
 #define DISABLE_X false
 #define DISABLE_Y false
 #define DISABLE_Z false
@@ -420,7 +421,7 @@ const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic
 //#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined.
 
 //===========================================================================
-//============================ Manual Bed Leveling ==========================
+//============================ Mesh Bed Leveling ============================
 //===========================================================================
 
 // #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
@@ -640,7 +641,7 @@ const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic
 // @section lcd
 
 // Define your display language below. Replace (en) with your language code and uncomment.
-// en, pl, fr, de, es, ru, bg, it, pt, pt-br, fi, an, nl, ca, eu, kana, kana_utf8, test
+// en, pl, fr, de, es, ru, bg, it, pt, pt-br, fi, an, nl, ca, eu, kana, kana_utf8, cn, test
 // See also language.h
 #define LANGUAGE_INCLUDE GENERATE_LANGUAGE_INCLUDE(en)
 
@@ -660,10 +661,9 @@ const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic
 //#define ENCODER_STEPS_PER_MENU_ITEM 5 // Set according to ENCODER_PULSES_PER_STEP or your liking
 //#define ULTIMAKERCONTROLLER //as available from the Ultimaker online store.
 //#define ULTIPANEL  //the UltiPanel as on Thingiverse
-//#define LCD_FEEDBACK_FREQUENCY_HZ 1000	// this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
 //#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click
-                                               // 0 to disable buzzer feedback  
-
+//#define LCD_FEEDBACK_FREQUENCY_HZ 1000         // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
+                                                 // 0 to disable buzzer feedback. Test with M300 S<frequency Hz> P<duration ms>
 // PanelOne from T3P3 (via RAMPS 1.4 AUX2/AUX3)
 // http://reprap.org/wiki/PanelOne
 //#define PANEL_ONE
@@ -791,13 +791,13 @@ const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic
 // Uncomment below to enable
 //#define FILAMENT_SENSOR
 
-#define FILAMENT_SENSOR_EXTRUDER_NUM	0  //The number of the extruder that has the filament sensor (0,1,2)
-#define MEASUREMENT_DELAY_CM			14  //measurement delay in cm.  This is the distance from filament sensor to middle of barrel
+#define FILAMENT_SENSOR_EXTRUDER_NUM 0   //The number of the extruder that has the filament sensor (0,1,2)
+#define MEASUREMENT_DELAY_CM        14   //measurement delay in cm.  This is the distance from filament sensor to middle of barrel
 
-#define DEFAULT_NOMINAL_FILAMENT_DIA  3.0  //Enter the diameter (in mm) of the filament generally used (3.0 mm or 1.75 mm) - this is then used in the slicer software.  Used for sensor reading validation
-#define MEASURED_UPPER_LIMIT          3.30  //upper limit factor used for sensor reading validation in mm
-#define MEASURED_LOWER_LIMIT          1.90  //lower limit factor for sensor reading validation in mm
-#define MAX_MEASUREMENT_DELAY			20  //delay buffer size in bytes (1 byte = 1cm)- limits maximum measurement delay allowable (must be larger than MEASUREMENT_DELAY_CM  and lower number saves RAM)
+#define DEFAULT_NOMINAL_FILAMENT_DIA 3.0 //Enter the diameter (in mm) of the filament generally used (3.0 mm or 1.75 mm) - this is then used in the slicer software.  Used for sensor reading validation
+#define MEASURED_UPPER_LIMIT         3.3 //upper limit factor used for sensor reading validation in mm
+#define MEASURED_LOWER_LIMIT         1.9 //lower limit factor for sensor reading validation in mm
+#define MAX_MEASUREMENT_DELAY       20   //delay buffer size in bytes (1 byte = 1cm)- limits maximum measurement delay allowable (must be larger than MEASUREMENT_DELAY_CM  and lower number saves RAM)
 
 //defines used in the code
 #define DEFAULT_MEASURED_FILAMENT_DIA  DEFAULT_NOMINAL_FILAMENT_DIA  //set measured to nominal initially
diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h
index 6478eb923e90956a3fa862d1c1225ac2324830e5..b79035ab69517969eeb85368d30d7d691f3eed74 100644
--- a/Marlin/Configuration_adv.h
+++ b/Marlin/Configuration_adv.h
@@ -195,6 +195,9 @@
 #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.
 
+// When G28 is called, this option will make Y home before X
+// #define HOME_Y_BEFORE_X
+
 // @section machine
 
 #define AXIS_RELATIVE_MODES {false, false, false, false}
@@ -258,7 +261,6 @@
 #define ENCODER_RATE_MULTIPLIER         // If defined, certain menu edit operations automatically multiply the steps when the encoder is moved quickly
 #define ENCODER_10X_STEPS_PER_SEC 75    // If the encoder steps per sec exceeds this value, multiply steps moved x10 to quickly advance the value
 #define ENCODER_100X_STEPS_PER_SEC 160  // If the encoder steps per sec exceeds this value, multiply steps moved x100 to really quickly advance the value
-//#define ENCODER_RATE_MULTIPLIER_DEBUG  // If defined, output the encoder steps per second value
 
 //#define CHDK 4        //Pin for triggering CHDK to take a picture see how to use it here http://captain-slow.dk/2014/03/09/3d-printing-timelapses/
 #define CHDK_DELAY 50 //How long in ms the pin should stay HIGH before going LOW again
@@ -355,7 +357,7 @@ const unsigned int dropsegments=5; //everything with less than this number of st
 //#define HEATERS_PARALLEL
 
 //===========================================================================
-//=============================Buffers           ============================
+//================================= Buffers =================================
 //===========================================================================
 
 // @section hidden
diff --git a/Marlin/Makefile b/Marlin/Makefile
index 56e5dda73403ccebcbf6a1c1f8c48fd86b36da4b..e23c2a19b423a175f45acd404409e429c56c691f 100644
--- a/Marlin/Makefile
+++ b/Marlin/Makefile
@@ -267,8 +267,8 @@ endif
 CXXSRC = WMath.cpp WString.cpp Print.cpp Marlin_main.cpp	\
 	MarlinSerial.cpp Sd2Card.cpp SdBaseFile.cpp SdFatUtil.cpp	\
 	SdFile.cpp SdVolume.cpp motion_control.cpp planner.cpp		\
-	stepper.cpp temperature.cpp cardreader.cpp ConfigurationStore.cpp \
-	watchdog.cpp SPI.cpp Servo.cpp Tone.cpp ultralcd.cpp digipot_mcp4451.cpp \
+	stepper.cpp temperature.cpp cardreader.cpp configuration_store.cpp \
+	watchdog.cpp SPI.cpp servo.cpp Tone.cpp ultralcd.cpp digipot_mcp4451.cpp \
 	vector_3.cpp qr_solve.cpp
 ifeq ($(LIQUID_TWI2), 0)
 CXXSRC += LiquidCrystal.cpp
diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h
index 9cec9dd37a1628c6ed7b376e712ca8127793e489..97978c45f7a1f0ab2f7bf67cb29e571897f06be0 100644
--- a/Marlin/Marlin.h
+++ b/Marlin/Marlin.h
@@ -31,6 +31,10 @@
 #define TEST(n,b) (((n)&BIT(b))!=0)
 #define RADIANS(d) ((d)*M_PI/180.0)
 #define DEGREES(r) ((d)*180.0/M_PI)
+#define NOLESS(v,n) do{ if (v < n) v = n; }while(0)
+#define NOMORE(v,n) do{ if (v > n) v = n; }while(0)
+
+typedef unsigned long millis_t;
 
 // Arduino < 1.0.0 does not define this, so we need to do it ourselves
 #ifndef analogInputToDigitalPin
@@ -219,18 +223,30 @@ void Stop();
   void filrunout();
 #endif
 
+/**
+ * Debug flags - not yet widely applied
+ */
+enum DebugFlags {
+  DEBUG_ECHO          = BIT(0),
+  DEBUG_INFO          = BIT(1),
+  DEBUG_ERRORS        = BIT(2),
+  DEBUG_DRYRUN        = BIT(3),
+  DEBUG_COMMUNICATION = BIT(4)
+};
+extern uint8_t marlin_debug_flags;
+
 extern bool Running;
 inline bool IsRunning() { return  Running; }
 inline bool IsStopped() { return !Running; }
 
-bool enquecommand(const char *cmd); //put a single ASCII command at the end of the current buffer or return false when it is full
-void enquecommands_P(const char *cmd); //put one or many ASCII commands at the end of the current buffer, read from flash
+bool enqueuecommand(const char *cmd); //put a single ASCII command at the end of the current buffer or return false when it is full
+void enqueuecommands_P(const char *cmd); //put one or many ASCII commands at the end of the current buffer, read from flash
 
 void prepare_arc_move(char isclockwise);
 void clamp_to_software_endstops(float target[3]);
 
-extern unsigned long previous_millis_cmd;
-inline void refresh_cmd_timeout() { previous_millis_cmd = millis(); }
+extern millis_t previous_cmd_ms;
+inline void refresh_cmd_timeout() { previous_cmd_ms = millis(); }
 
 #ifdef FAST_PWM_FAN
   void setPwmFrequency(uint8_t pin, int val);
@@ -243,7 +259,7 @@ inline void refresh_cmd_timeout() { previous_millis_cmd = millis(); }
 
 extern float homing_feedrate[];
 extern bool axis_relative_modes[];
-extern int feedmultiply;
+extern int feedrate_multiplier;
 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.
@@ -273,6 +289,10 @@ extern bool axis_known_position[3];
   extern float zprobe_zoffset;
 #endif
 
+#ifdef PREVENT_DANGEROUS_EXTRUDE
+  extern float extrude_min_temp;
+#endif
+
 extern int fanSpeed;
 
 #ifdef BARICUDA
@@ -301,8 +321,8 @@ extern int fanSpeed;
   extern float retract_recover_length, retract_recover_length_swap, retract_recover_feedrate;
 #endif
 
-extern unsigned long starttime;
-extern unsigned long stoptime;
+extern millis_t print_job_start_ms;
+extern millis_t print_job_stop_ms;
 
 // Handling multiple extruders pins
 extern uint8_t active_extruder;
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 8bf0909886976c95a86c1e234973cf081a50255b..996bd60b9130d8aec23ab819a5d3409a4d2d3cda 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -49,13 +49,13 @@
 #include "motion_control.h"
 #include "cardreader.h"
 #include "watchdog.h"
-#include "ConfigurationStore.h"
+#include "configuration_store.h"
 #include "language.h"
 #include "pins_arduino.h"
 #include "math.h"
 
 #ifdef BLINKM
-  #include "BlinkM.h"
+  #include "blinkm.h"
   #include "Wire.h"
 #endif
 
@@ -67,136 +67,154 @@
   #include <SPI.h>
 #endif
 
-// look here for descriptions of G-codes: http://linuxcnc.org/handbook/gcode/g-code.html
-// http://objects.reprap.org/wiki/Mendel_User_Manual:_RepRapGCodes
-
-//Implemented Codes
-//-------------------
-// G0  -> G1
-// G1  - Coordinated Movement X Y Z E
-// G2  - CW ARC
-// G3  - CCW ARC
-// G4  - Dwell S<seconds> or P<milliseconds>
-// G10 - retract filament according to settings of M207
-// G11 - retract recover filament according to settings of M208
-// G28 - Home one or more axes
-// G29 - Detailed Z-Probe, probes the bed at 3 or more points.  Will fail if you haven't homed yet.
-// G30 - Single Z Probe, probes bed at current XY location.
-// G31 - Dock sled (Z_PROBE_SLED only)
-// G32 - Undock sled (Z_PROBE_SLED only)
-// G90 - Use Absolute Coordinates
-// G91 - Use Relative Coordinates
-// G92 - Set current position to coordinates given
-
-// M Codes
-// M0   - Unconditional stop - Wait for user to press a button on the LCD (Only if ULTRA_LCD is enabled)
-// M1   - Same as M0
-// M17  - Enable/Power all stepper motors
-// M18  - Disable all stepper motors; same as M84
-// M20  - List SD card
-// M21  - Init SD card
-// M22  - Release SD card
-// M23  - Select SD file (M23 filename.g)
-// M24  - Start/resume SD print
-// M25  - Pause SD print
-// M26  - Set SD position in bytes (M26 S12345)
-// M27  - Report SD print status
-// M28  - Start SD write (M28 filename.g)
-// M29  - Stop SD write
-// M30  - Delete file from SD (M30 filename.g)
-// M31  - Output time since last M109 or SD card start to serial
-// M32  - Select file and start SD print (Can be used _while_ printing from SD card files):
-//        syntax "M32 /path/filename#", or "M32 S<startpos bytes> !filename#"
-//        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)
-// M83  - Set E codes relative while in Absolute Coordinates (G90) mode
-// M84  - Disable steppers until next move,
-//        or use S<seconds> to specify an inactivity timeout, after which the steppers will be disabled.  S0 to disable the timeout.
-// M85  - Set inactivity shutdown timer with parameter S<seconds>. To disable set zero (default)
-// M92  - Set axis_steps_per_unit - same syntax as G92
-// M104 - Set extruder target temp
-// M105 - Read current temp
-// M106 - Fan on
-// M107 - Fan off
-// M109 - Sxxx Wait for extruder current temp to reach target temp. Waits only when heating
-//        Rxxx Wait for extruder current temp to reach target temp. Waits when heating and cooling
-//        IF AUTOTEMP is enabled, S<mintemp> B<maxtemp> F<factor>. Exit autotemp by any M109 without F
-// M112 - Emergency stop
-// M114 - Output current position to serial port
-// M115 - Capabilities string
-// M117 - display message
-// M119 - Output Endstop status to serial port
-// M120 - Enable endstop detection
-// M121 - Disable endstop detection
-// M126 - Solenoid Air Valve Open (BariCUDA support by jmil)
-// M127 - Solenoid Air Valve Closed (BariCUDA vent to atmospheric pressure by jmil)
-// M128 - EtoP Open (BariCUDA EtoP = electricity to air pressure transducer by jmil)
-// M129 - EtoP Closed (BariCUDA EtoP = electricity to air pressure transducer by jmil)
-// M140 - Set bed target temp
-// M150 - Set BlinkM Color Output R: Red<0-255> U(!): Green<0-255> B: Blue<0-255> over i2c, G for green does not work.
-// M190 - Sxxx Wait for bed current temp to reach target temp. Waits only when heating
-//        Rxxx Wait for bed current temp to reach target temp. Waits when heating and cooling
-// M200 D<millimeters>- set filament diameter and set E axis units to cubic millimeters (use S0 to set back to millimeters).
-// M201 - Set max acceleration in units/s^2 for print moves (M201 X1000 Y1000)
-// M202 - Set max acceleration in units/s^2 for travel moves (M202 X1000 Y1000) Unused in Marlin!!
-// M203 - Set maximum feedrate that your machine can sustain (M203 X200 Y200 Z300 E10000) in mm/sec
-// M204 - Set default acceleration: P for Printing moves, R for Retract only (no X, Y, Z) moves and T for Travel (non printing) moves (ex. M204 P800 T3000 R9000) in mm/sec^2
-// M205 -  advanced settings:  minimum travel speed S=while printing T=travel only,  B=minimum segment time X= maximum xy jerk, Z=maximum Z jerk, E=maximum E jerk
-// M206 - Set additional homing offset
-// M207 - Set retract length S[positive mm] F[feedrate mm/min] Z[additional zlift/hop], stays in mm regardless of M200 setting
-// M208 - Set recover=unretract length S[positive mm surplus to the M207 S*] F[feedrate mm/sec]
-// M209 - S<1=true/0=false> enable automatic retract detect if the slicer did not support G10/11: every normal extrude-only move will be classified as retract depending on the direction.
-// M218 - Set hotend offset (in mm): T<extruder_number> X<offset_on_X> Y<offset_on_Y>
-// M220 S<factor in percent>- set speed factor override percentage
-// M221 S<factor in percent>- set extrude factor override percentage
-// M226 P<pin number> S<pin state>- Wait until the specified pin reaches the state required
-// M240 - Trigger a camera to take a photograph
-// M250 - Set LCD contrast C<contrast value> (value 0..63)
-// M280 - Set servo position absolute. P: servo index, S: angle or microseconds
-// M300 - Play beep sound S<frequency Hz> P<duration ms>
-// M301 - Set PID parameters P I and D
-// M302 - Allow cold extrudes, or set the minimum extrude S<temperature>.
-// M303 - PID relay autotune S<temperature> sets the target temperature. (default target temperature = 150C)
-// M304 - Set bed PID parameters P I and D
-// M380 - Activate solenoid on active extruder
-// M381 - Disable all solenoids
-// M400 - Finish all moves
-// M401 - Lower z-probe if present
-// M402 - Raise z-probe if present
-// M404 - N<dia in mm> Enter the nominal filament width (3mm, 1.75mm ) or will display nominal filament width without parameters
-// M405 - Turn on Filament Sensor extrusion control.  Optional D<delay in cm> to set delay in centimeters between sensor and extruder
-// M406 - Turn off Filament Sensor extrusion control
-// M407 - Display measured filament diameter
-// M500 - Store parameters in EEPROM
-// M501 - Read parameters from EEPROM (if you need reset them after you changed them temporarily).
-// M502 - Revert to the default "factory settings". You still need to store them in EEPROM afterwards if you want to.
-// M503 - Print the current settings (from memory not from EEPROM). Use S0 to leave off headings.
-// M540 - Use S[0|1] to enable or disable the stop SD card print on endstop hit (requires ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
-// M600 - Pause for filament change X[pos] Y[pos] Z[relative lift] E[initial retract] L[later retract distance for removal]
-// M665 - Set delta configurations
-// M666 - Set delta endstop adjustment
-// M605 - Set dual x-carriage movement mode: S<mode> [ X<duplication x-offset> R<duplication temp offset> ]
-// M907 - Set digital trimpot motor current using axis codes.
-// M908 - Control digital trimpot directly.
-// M350 - Set microstepping mode.
-// M351 - Toggle MS1 MS2 pins directly.
-
-// ************ SCARA Specific - This can change to suit future G-code regulations
-// M360 - SCARA calibration: Move to cal-position ThetaA (0 deg calibration)
-// M361 - SCARA calibration: Move to cal-position ThetaB (90 deg calibration - steps per degree)
-// M362 - SCARA calibration: Move to cal-position PsiA (0 deg calibration)
-// M363 - SCARA calibration: Move to cal-position PsiB (90 deg calibration - steps per degree)
-// M364 - SCARA calibration: Move to cal-position PSIC (90 deg to Theta calibration position)
-// M365 - SCARA calibration: Scaling factor, X, Y, Z axis
-//************* SCARA End ***************
-
-// M928 - Start SD logging (M928 filename.g) - ended by M29
-// M999 - Restart after being stopped by error
+/**
+ * Look here for descriptions of G-codes:
+ *  - http://linuxcnc.org/handbook/gcode/g-code.html
+ *  - http://objects.reprap.org/wiki/Mendel_User_Manual:_RepRapGCodes
+ *
+ * Help us document these G-codes online:
+ *  - http://reprap.org/wiki/G-code
+ *  - https://github.com/MarlinFirmware/Marlin/wiki/Marlin-G-Code
+ */
+
+/**
+ * Implemented Codes
+ * -------------------
+ *
+ * "G" Codes
+ *
+ * G0  -> G1
+ * G1  - Coordinated Movement X Y Z E
+ * G2  - CW ARC
+ * G3  - CCW ARC
+ * G4  - Dwell S<seconds> or P<milliseconds>
+ * G10 - retract filament according to settings of M207
+ * G11 - retract recover filament according to settings of M208
+ * G28 - Home one or more axes
+ * G29 - Detailed Z-Probe, probes the bed at 3 or more points.  Will fail if you haven't homed yet.
+ * G30 - Single Z Probe, probes bed at current XY location.
+ * G31 - Dock sled (Z_PROBE_SLED only)
+ * G32 - Undock sled (Z_PROBE_SLED only)
+ * G90 - Use Absolute Coordinates
+ * G91 - Use Relative Coordinates
+ * G92 - Set current position to coordinates given
+ *
+ * "M" Codes
+ *
+ * M0   - Unconditional stop - Wait for user to press a button on the LCD (Only if ULTRA_LCD is enabled)
+ * M1   - Same as M0
+ * M17  - Enable/Power all stepper motors
+ * M18  - Disable all stepper motors; same as M84
+ * M20  - List SD card
+ * M21  - Init SD card
+ * M22  - Release SD card
+ * M23  - Select SD file (M23 filename.g)
+ * M24  - Start/resume SD print
+ * M25  - Pause SD print
+ * M26  - Set SD position in bytes (M26 S12345)
+ * M27  - Report SD print status
+ * M28  - Start SD write (M28 filename.g)
+ * M29  - Stop SD write
+ * M30  - Delete file from SD (M30 filename.g)
+ * M31  - Output time since last M109 or SD card start to serial
+ * M32  - Select file and start SD print (Can be used _while_ printing from SD card files):
+ *        syntax "M32 /path/filename#", or "M32 S<startpos bytes> !filename#"
+ *        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 [P # 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)
+ * M83  - Set E codes relative while in Absolute Coordinates (G90) mode
+ * M84  - Disable steppers until next move,
+ *        or use S<seconds> to specify an inactivity timeout, after which the steppers will be disabled.  S0 to disable the timeout.
+ * M85  - Set inactivity shutdown timer with parameter S<seconds>. To disable set zero (default)
+ * M92  - Set axis_steps_per_unit - same syntax as G92
+ * M104 - Set extruder target temp
+ * M105 - Read current temp
+ * M106 - Fan on
+ * M107 - Fan off
+ * M109 - Sxxx Wait for extruder current temp to reach target temp. Waits only when heating
+ *        Rxxx Wait for extruder current temp to reach target temp. Waits when heating and cooling
+ *        IF AUTOTEMP is enabled, S<mintemp> B<maxtemp> F<factor>. Exit autotemp by any M109 without F
+ * M111 - Set debug flags with S<mask>. See flag bits defined in Marlin.h.
+ * M112 - Emergency stop
+ * M114 - Output current position to serial port
+ * M115 - Capabilities string
+ * M117 - display message
+ * M119 - Output Endstop status to serial port
+ * M120 - Enable endstop detection
+ * M121 - Disable endstop detection
+ * M126 - Solenoid Air Valve Open (BariCUDA support by jmil)
+ * M127 - Solenoid Air Valve Closed (BariCUDA vent to atmospheric pressure by jmil)
+ * M128 - EtoP Open (BariCUDA EtoP = electricity to air pressure transducer by jmil)
+ * M129 - EtoP Closed (BariCUDA EtoP = electricity to air pressure transducer by jmil)
+ * M140 - Set bed target temp
+ * M145 - Set the heatup state H<hotend> B<bed> F<fan speed> for S<material> (0=PLA, 1=ABS)
+ * M150 - Set BlinkM Color Output R: Red<0-255> U(!): Green<0-255> B: Blue<0-255> over i2c, G for green does not work.
+ * M190 - Sxxx Wait for bed current temp to reach target temp. Waits only when heating
+ *        Rxxx Wait for bed current temp to reach target temp. Waits when heating and cooling
+ * M200 - set filament diameter and set E axis units to cubic millimeters (use S0 to set back to millimeters).:D<millimeters>- 
+ * M201 - Set max acceleration in units/s^2 for print moves (M201 X1000 Y1000)
+ * M202 - Set max acceleration in units/s^2 for travel moves (M202 X1000 Y1000) Unused in Marlin!!
+ * M203 - Set maximum feedrate that your machine can sustain (M203 X200 Y200 Z300 E10000) in mm/sec
+ * M204 - Set default acceleration: P for Printing moves, R for Retract only (no X, Y, Z) moves and T for Travel (non printing) moves (ex. M204 P800 T3000 R9000) in mm/sec^2
+ * M205 -  advanced settings:  minimum travel speed S=while printing T=travel only,  B=minimum segment time X= maximum xy jerk, Z=maximum Z jerk, E=maximum E jerk
+ * M206 - Set additional homing offset
+ * M207 - Set retract length S[positive mm] F[feedrate mm/min] Z[additional zlift/hop], stays in mm regardless of M200 setting
+ * M208 - Set recover=unretract length S[positive mm surplus to the M207 S*] F[feedrate mm/sec]
+ * M209 - S<1=true/0=false> enable automatic retract detect if the slicer did not support G10/11: every normal extrude-only move will be classified as retract depending on the direction.
+ * M218 - Set hotend offset (in mm): T<extruder_number> X<offset_on_X> Y<offset_on_Y>
+ * M220 - Set speed factor override percentage: S<factor in percent>
+ * M221 - Set extrude factor override percentage: S<factor in percent>
+ * M226 - Wait until the specified pin reaches the state required: P<pin number> S<pin state>
+ * M240 - Trigger a camera to take a photograph
+ * M250 - Set LCD contrast C<contrast value> (value 0..63)
+ * M280 - Set servo position absolute. P: servo index, S: angle or microseconds
+ * M300 - Play beep sound S<frequency Hz> P<duration ms>
+ * M301 - Set PID parameters P I and D
+ * M302 - Allow cold extrudes, or set the minimum extrude S<temperature>.
+ * M303 - PID relay autotune S<temperature> sets the target temperature. (default target temperature = 150C)
+ * M304 - Set bed PID parameters P I and D
+ * M380 - Activate solenoid on active extruder
+ * M381 - Disable all solenoids
+ * M400 - Finish all moves
+ * M401 - Lower z-probe if present
+ * M402 - Raise z-probe if present
+ * M404 - N<dia in mm> Enter the nominal filament width (3mm, 1.75mm ) or will display nominal filament width without parameters
+ * M405 - Turn on Filament Sensor extrusion control.  Optional D<delay in cm> to set delay in centimeters between sensor and extruder
+ * M406 - Turn off Filament Sensor extrusion control
+ * M407 - Display measured filament diameter
+ * M410 - Quickstop. Abort all the planned moves
+ * M420 - Enable/Disable Mesh Leveling (with current values) S1=enable S0=disable
+ * M421 - Set a single Z coordinate in the Mesh Leveling grid. X<mm> Y<mm> Z<mm>
+ * M500 - Store parameters in EEPROM
+ * M501 - Read parameters from EEPROM (if you need reset them after you changed them temporarily).
+ * M502 - Revert to the default "factory settings". You still need to store them in EEPROM afterwards if you want to.
+ * M503 - Print the current settings (from memory not from EEPROM). Use S0 to leave off headings.
+ * M540 - Use S[0|1] to enable or disable the stop SD card print on endstop hit (requires ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
+ * M600 - Pause for filament change X[pos] Y[pos] Z[relative lift] E[initial retract] L[later retract distance for removal]
+ * M665 - Set delta configurations: L<diagonal rod> R<delta radius> S<segments/s>
+ * M666 - Set delta endstop adjustment
+ * M605 - Set dual x-carriage movement mode: S<mode> [ X<duplication x-offset> R<duplication temp offset> ]
+ * M907 - Set digital trimpot motor current using axis codes.
+ * M908 - Control digital trimpot directly.
+ * M350 - Set microstepping mode.
+ * M351 - Toggle MS1 MS2 pins directly.
+ *
+ * ************ SCARA Specific - This can change to suit future G-code regulations
+ * M360 - SCARA calibration: Move to cal-position ThetaA (0 deg calibration)
+ * M361 - SCARA calibration: Move to cal-position ThetaB (90 deg calibration - steps per degree)
+ * M362 - SCARA calibration: Move to cal-position PsiA (0 deg calibration)
+ * M363 - SCARA calibration: Move to cal-position PsiB (90 deg calibration - steps per degree)
+ * M364 - SCARA calibration: Move to cal-position PSIC (90 deg to Theta calibration position)
+ * M365 - SCARA calibration: Scaling factor, X, Y, Z axis
+ * ************* SCARA End ***************
+ *
+ * M928 - Start SD logging (M928 filename.g) - ended by M29
+ * M999 - Restart after being stopped by error
+ */
 
 #ifdef SDSUPPORT
   CardReader card;
@@ -204,18 +222,24 @@
 
 bool Running = true;
 
+uint8_t marlin_debug_flags = DEBUG_INFO|DEBUG_ERRORS;
+
 static float feedrate = 1500.0, next_feedrate, saved_feedrate;
 float current_position[NUM_AXIS] = { 0.0 };
 static float destination[NUM_AXIS] = { 0.0 };
 bool axis_known_position[3] = { false };
 
 static long gcode_N, gcode_LastN, Stopped_gcode_LastN = 0;
-static char cmdbuffer[BUFSIZE][MAX_CMD_SIZE];
+
+static int cmd_queue_index_r = 0;
+static int cmd_queue_index_w = 0;
+static int commands_in_queue = 0;
+static char command_queue[BUFSIZE][MAX_CMD_SIZE];
 
 float homing_feedrate[] = HOMING_FEEDRATE;
 bool axis_relative_modes[] = AXIS_RELATIVE_MODES;
-int feedmultiply = 100; //100->1 200->2
-int saved_feedmultiply;
+int feedrate_multiplier = 100; //100->1 200->2
+int saved_feedrate_multiplier;
 int extruder_multiply[EXTRUDERS] = ARRAY_BY_EXTRUDERS(100, 100, 100, 100);
 bool volumetric_enabled = false;
 float filament_size[EXTRUDERS] = ARRAY_BY_EXTRUDERS(DEFAULT_NOMINAL_FILAMENT_DIA, DEFAULT_NOMINAL_FILAMENT_DIA, DEFAULT_NOMINAL_FILAMENT_DIA, DEFAULT_NOMINAL_FILAMENT_DIA);
@@ -234,9 +258,6 @@ const char axis_codes[NUM_AXIS] = {'X', 'Y', 'Z', 'E'};
 
 static float offset[3] = { 0 };
 static bool relative_mode = false;  //Determines Absolute or Relative Coordinates
-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;
@@ -244,13 +265,13 @@ static char *strchr_pointer; ///< A pointer to find chars in the command string
 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
+millis_t previous_cmd_ms = 0;
+static millis_t max_inactive_time = 0;
+static millis_t stepper_inactive_time = DEFAULT_STEPPER_DEACTIVE_TIME * 1000L;
+millis_t print_job_start_ms = 0; ///< Print job start time
+millis_t print_job_stop_ms = 0;  ///< Print job stop time
 static uint8_t target_extruder;
-bool CooldownNoWait = true;
+bool no_wait_for_cooling = true;
 bool target_direction;
 
 #ifdef ENABLE_AUTO_BED_LEVELING
@@ -357,7 +378,7 @@ bool target_direction;
 #endif
 
 #ifdef FILAMENT_RUNOUT_SENSOR
-   static bool filrunoutEnqued = false;
+   static bool filrunoutEnqueued = false;
 #endif
 
 #ifdef SDSUPPORT
@@ -365,7 +386,7 @@ bool target_direction;
 #endif
 
 #if NUM_SERVOS > 0
-  Servo servos[NUM_SERVOS];
+  Servo servo[NUM_SERVOS];
 #endif
 
 #ifdef CHDK
@@ -380,12 +401,13 @@ bool target_direction;
 void get_arc_coordinates();
 bool setTargetedHotend(int code);
 
-void serial_echopair_P(const char *s_P, float v)
-    { serialprintPGM(s_P); SERIAL_ECHO(v); }
-void serial_echopair_P(const char *s_P, double v)
-    { serialprintPGM(s_P); SERIAL_ECHO(v); }
-void serial_echopair_P(const char *s_P, unsigned long v)
-    { serialprintPGM(s_P); SERIAL_ECHO(v); }
+void serial_echopair_P(const char *s_P, float v)         { serialprintPGM(s_P); SERIAL_ECHO(v); }
+void serial_echopair_P(const char *s_P, double v)        { serialprintPGM(s_P); SERIAL_ECHO(v); }
+void serial_echopair_P(const char *s_P, unsigned long v) { serialprintPGM(s_P); SERIAL_ECHO(v); }
+
+#ifdef PREVENT_DANGEROUS_EXTRUDE
+  float extrude_min_temp = EXTRUDE_MINTEMP;
+#endif
 
 #ifdef SDSUPPORT
   #include "SdFatUtil.h"
@@ -409,8 +431,10 @@ void serial_echopair_P(const char *s_P, unsigned long v)
   }
 #endif //!SDSUPPORT
 
-//Injects the next command from the pending sequence of commands, when possible
-//Return false if and only if no command was pending
+/**
+ * Inject the next command from the command queue, when possible
+ * Return false only if no command was pending
+ */
 static bool drain_queued_commands_P() {
   if (!queued_commands_P) return false;
 
@@ -424,7 +448,7 @@ static bool drain_queued_commands_P() {
   char c;
   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 (enqueuecommand(cmd)) {      // buffer was not full (else we will retry later)
     if (c)
       queued_commands_P += i + 1; // move to next command
     else
@@ -433,45 +457,46 @@ static bool drain_queued_commands_P() {
   return true;
 }
 
-//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 executed asap (when possible)
+/**
+ * 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 enqueuecommands_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
-//that is really done in a non-safe way.
-//needs overworking someday
-//Returns false if it failed to do so
-bool enquecommand(const char *cmd)
-{
-  if(*cmd==';')
-    return false;
-  if(buflen >= BUFSIZE)
-    return false;
-  //this is dangerous if a mixing of serial and this happens
-  strcpy(&(cmdbuffer[bufindw][0]),cmd);
+/**
+ * Copy a command directly into the main command buffer, from RAM.
+ *
+ * This is done in a non-safe way and needs a rework someday.
+ * Returns false if it doesn't add any command
+ */
+bool enqueuecommand(const char *cmd) {
+
+  if (*cmd == ';' || commands_in_queue >= BUFSIZE) return false;
+
+  // This is dangerous if a mixing of serial and this happens
+  char *command = command_queue[cmd_queue_index_w];
+  strcpy(command, cmd);
   SERIAL_ECHO_START;
-  SERIAL_ECHOPGM(MSG_Enqueing);
-  SERIAL_ECHO(cmdbuffer[bufindw]);
+  SERIAL_ECHOPGM(MSG_Enqueueing);
+  SERIAL_ECHO(command);
   SERIAL_ECHOLNPGM("\"");
-  bufindw= (bufindw + 1)%BUFSIZE;
-  buflen += 1;
+  cmd_queue_index_w = (cmd_queue_index_w + 1) % BUFSIZE;
+  commands_in_queue++;
   return true;
 }
 
-void setup_killpin()
-{
+void setup_killpin() {
   #if HAS_KILL
     SET_INPUT(KILL_PIN);
     WRITE(KILL_PIN, HIGH);
   #endif
 }
 
-void setup_filrunoutpin()
-{
+void setup_filrunoutpin() {
   #if HAS_FILRUNOUT
     pinMode(FILRUNOUT_PIN, INPUT);
     #ifdef ENDSTOPPULLUP_FIL_RUNOUT
@@ -481,8 +506,7 @@ void setup_filrunoutpin()
 }
 
 // Set home pin
-void setup_homepin(void)
-{
+void setup_homepin(void) {
   #if HAS_HOME
     SET_INPUT(HOME_PIN);
     WRITE(HOME_PIN, HIGH);
@@ -490,15 +514,13 @@ void setup_homepin(void)
 }
 
 
-void setup_photpin()
-{
+void setup_photpin() {
   #if HAS_PHOTOGRAPH
     OUT_WRITE(PHOTOGRAPH_PIN, LOW);
   #endif
 }
 
-void setup_powerhold()
-{
+void setup_powerhold() {
   #if HAS_SUICIDE
     OUT_WRITE(SUICIDE_PIN, HIGH);
   #endif
@@ -511,44 +533,58 @@ void setup_powerhold()
   #endif
 }
 
-void suicide()
-{
+void suicide() {
   #if HAS_SUICIDE
     OUT_WRITE(SUICIDE_PIN, LOW);
   #endif
 }
 
-void servo_init()
-{
+void servo_init() {
   #if NUM_SERVOS >= 1 && HAS_SERVO_0
-    servos[0].attach(SERVO0_PIN);
+    servo[0].attach(SERVO0_PIN);
   #endif
   #if NUM_SERVOS >= 2 && HAS_SERVO_1
-    servos[1].attach(SERVO1_PIN);
+    servo[1].attach(SERVO1_PIN);
   #endif
   #if NUM_SERVOS >= 3 && HAS_SERVO_2
-    servos[2].attach(SERVO2_PIN);
+    servo[2].attach(SERVO2_PIN);
   #endif
   #if NUM_SERVOS >= 4 && HAS_SERVO_3
-    servos[3].attach(SERVO3_PIN);
+    servo[3].attach(SERVO3_PIN);
   #endif
 
   // Set position of Servo Endstops that are defined
   #ifdef SERVO_ENDSTOPS
   for (int i = 0; i < 3; i++)
     if (servo_endstops[i] >= 0)
-      servos[servo_endstops[i]].write(servo_endstop_angles[i * 2 + 1]);
+      servo[servo_endstops[i]].write(servo_endstop_angles[i * 2 + 1]);
   #endif
 
   #if SERVO_LEVELING
     delay(PROBE_SERVO_DEACTIVATION_DELAY);
-    servos[servo_endstops[Z_AXIS]].detach();
+    servo[servo_endstops[Z_AXIS]].detach();
   #endif
 }
 
-
-void setup()
-{
+/**
+ * Marlin entry-point: Set up before the program loop
+ *  - Set up the kill pin, filament runout, power hold
+ *  - Start the serial port
+ *  - Print startup messages and diagnostics
+ *  - Get EEPROM or default settings
+ *  - Initialize managers for:
+ *    • temperature
+ *    • planner
+ *    • watchdog
+ *    • stepper
+ *    • photo pin
+ *    • servos
+ *    • LCD controller
+ *    • Digipot I2C
+ *    • Z probe sled
+ *    • status LEDs
+ */
+void setup() {
   setup_killpin();
   setup_filrunoutpin();
   setup_powerhold();
@@ -558,15 +594,16 @@ void setup()
 
   // Check startup - does nothing if bootloader sets MCUSR to 0
   byte mcu = MCUSR;
-  if(mcu & 1) SERIAL_ECHOLNPGM(MSG_POWERUP);
-  if(mcu & 2) SERIAL_ECHOLNPGM(MSG_EXTERNAL_RESET);
-  if(mcu & 4) SERIAL_ECHOLNPGM(MSG_BROWNOUT_RESET);
-  if(mcu & 8) SERIAL_ECHOLNPGM(MSG_WATCHDOG_RESET);
-  if(mcu & 32) SERIAL_ECHOLNPGM(MSG_SOFTWARE_RESET);
-  MCUSR=0;
+  if (mcu & 1) SERIAL_ECHOLNPGM(MSG_POWERUP);
+  if (mcu & 2) SERIAL_ECHOLNPGM(MSG_EXTERNAL_RESET);
+  if (mcu & 4) SERIAL_ECHOLNPGM(MSG_BROWNOUT_RESET);
+  if (mcu & 8) SERIAL_ECHOLNPGM(MSG_WATCHDOG_RESET);
+  if (mcu & 32) SERIAL_ECHOLNPGM(MSG_SOFTWARE_RESET);
+  MCUSR = 0;
 
   SERIAL_ECHOPGM(MSG_MARLIN);
-  SERIAL_ECHOLNPGM(STRING_VERSION);
+  SERIAL_ECHOLNPGM(" " STRING_VERSION);
+
   #ifdef STRING_VERSION_CONFIG_H
     #ifdef STRING_CONFIG_H_AUTHOR
       SERIAL_ECHO_START;
@@ -578,17 +615,16 @@ void setup()
       SERIAL_ECHOLNPGM(__DATE__);
     #endif // STRING_CONFIG_H_AUTHOR
   #endif // STRING_VERSION_CONFIG_H
+
   SERIAL_ECHO_START;
   SERIAL_ECHOPGM(MSG_FREE_MEMORY);
   SERIAL_ECHO(freeMemory());
   SERIAL_ECHOPGM(MSG_PLANNER_BUFFER_BYTES);
   SERIAL_ECHOLN((int)sizeof(block_t)*BLOCK_BUFFER_SIZE);
+
   #ifdef SDSUPPORT
-  for(int8_t i = 0; i < BUFSIZE; i++)
-  {
-    fromsd[i] = false;
-  }
-  #endif //!SDSUPPORT
+    for (int8_t i = 0; i < BUFSIZE; i++) fromsd[i] = false;
+  #endif
 
   // loads data from EEPROM if available else uses defaults (and resets step acceleration rate)
   Config_RetrieveSettings();
@@ -599,7 +635,6 @@ void setup()
   st_init();    // Initialize stepper, this enables interrupts!
   setup_photpin();
   servo_init();
-  
 
   lcd_init();
   _delay_ms(1000);  // wait 1sec to display the splash screen
@@ -611,52 +646,73 @@ void setup()
   #ifdef DIGIPOT_I2C
     digipot_i2c_init();
   #endif
-#ifdef Z_PROBE_SLED
-  pinMode(SERVO0_PIN, OUTPUT);
-  digitalWrite(SERVO0_PIN, LOW); // turn it off
-#endif // Z_PROBE_SLED
+
+  #ifdef Z_PROBE_SLED
+    pinMode(SERVO0_PIN, OUTPUT);
+    digitalWrite(SERVO0_PIN, LOW); // turn it off
+  #endif // Z_PROBE_SLED
+
   setup_homepin();
   
-#ifdef STAT_LED_RED
-  pinMode(STAT_LED_RED, OUTPUT);
-  digitalWrite(STAT_LED_RED, LOW); // turn it off
-#endif
-#ifdef STAT_LED_BLUE
-  pinMode(STAT_LED_BLUE, OUTPUT);
-  digitalWrite(STAT_LED_BLUE, LOW); // turn it off
-#endif  
-}
+  #ifdef STAT_LED_RED
+    pinMode(STAT_LED_RED, OUTPUT);
+    digitalWrite(STAT_LED_RED, LOW); // turn it off
+  #endif
 
+  #ifdef STAT_LED_BLUE
+    pinMode(STAT_LED_BLUE, OUTPUT);
+    digitalWrite(STAT_LED_BLUE, LOW); // turn it off
+  #endif  
+}
 
+/**
+ * The main Marlin program loop
+ *
+ *  - Save or log commands to SD
+ *  - Process available commands (if not saving)
+ *  - Call heater manager
+ *  - Call inactivity manager
+ *  - Call endstop manager
+ *  - Call LCD update
+ */
 void loop() {
-  if (buflen < BUFSIZE - 1) get_command();
+  if (commands_in_queue < BUFSIZE - 1) get_command();
 
   #ifdef SDSUPPORT
     card.checkautostart(false);
   #endif
 
-  if (buflen) {
+  if (commands_in_queue) {
+
     #ifdef SDSUPPORT
+
       if (card.saving) {
-        if (strstr_P(cmdbuffer[bufindr], PSTR("M29")) == NULL) {
-          card.write_command(cmdbuffer[bufindr]);
+        char *command = command_queue[cmd_queue_index_r];
+        if (strstr_P(command, PSTR("M29"))) {
+          // M29 closes the file
+          card.closefile();
+          SERIAL_PROTOCOLLNPGM(MSG_FILE_SAVED);
+        }
+        else {
+          // Write the string from the read buffer to SD
+          card.write_command(command);
           if (card.logging)
-            process_commands();
+            process_commands(); // The card is saving because it's logging
           else
             SERIAL_PROTOCOLLNPGM(MSG_OK);
         }
-        else {
-          card.closefile();
-          SERIAL_PROTOCOLLNPGM(MSG_FILE_SAVED);
-        }
       }
       else
         process_commands();
+
     #else
+
       process_commands();
+
     #endif // SDSUPPORT
-    buflen--;
-    bufindr = (bufindr + 1) % BUFSIZE;
+
+    commands_in_queue--;
+    cmd_queue_index_r = (cmd_queue_index_r + 1) % BUFSIZE;
   }
   // Check heater every n milliseconds
   manage_heater();
@@ -665,50 +721,56 @@ void loop() {
   lcd_update();
 }
 
-void get_command()
-{
-  if (drain_queued_commands_P()) // priority is given to non-serial commands
-    return;
+/**
+ * Add to the circular command queue the next command from:
+ *  - The command-injection queue (queued_commands_P)
+ *  - The active serial input (usually USB)
+ *  - The SD card file being actively printed
+ */
+void get_command() {
+
+  if (drain_queued_commands_P()) return; // priority is given to non-serial commands
   
-  while( MYSERIAL.available() > 0  && buflen < BUFSIZE) {
+  while (MYSERIAL.available() > 0 && commands_in_queue < BUFSIZE) {
+
     serial_char = MYSERIAL.read();
-    if(serial_char == '\n' ||
-       serial_char == '\r' ||
-       serial_count >= (MAX_CMD_SIZE - 1) )
-    {
+
+    if (serial_char == '\n' || serial_char == '\r' ||
+       serial_count >= (MAX_CMD_SIZE - 1)
+    ) {
       // end of line == end of comment
       comment_mode = false;
 
-      if(!serial_count) {
-        // short cut for empty lines
-        return;
-      }
-      cmdbuffer[bufindw][serial_count] = 0; //terminate string
+      if (!serial_count) return; // shortcut for empty lines
+
+      char *command = command_queue[cmd_queue_index_w];
+      command[serial_count] = 0; // terminate string
+
       #ifdef SDSUPPORT
-      fromsd[bufindw] = false;
-      #endif //!SDSUPPORT
-      if(strchr(cmdbuffer[bufindw], 'N') != NULL)
-      {
-        strchr_pointer = strchr(cmdbuffer[bufindw], 'N');
+        fromsd[cmd_queue_index_w] = false;
+      #endif
+
+      if (strchr(command, 'N') != NULL) {
+        strchr_pointer = strchr(command, 'N');
         gcode_N = (strtol(strchr_pointer + 1, NULL, 10));
-        if(gcode_N != gcode_LastN+1 && (strstr_P(cmdbuffer[bufindw], PSTR("M110")) == NULL) ) {
+        if (gcode_N != gcode_LastN + 1 && strstr_P(command, PSTR("M110")) == NULL) {
           SERIAL_ERROR_START;
-          SERIAL_ERRORPGM(MSG_ERR_LINE_NO);
-          SERIAL_ERRORLN(gcode_LastN);
-          //Serial.println(gcode_N);
+          SERIAL_ERRORPGM(MSG_ERR_LINE_NO1);
+          SERIAL_ERROR(gcode_LastN + 1);
+          SERIAL_ERRORPGM(MSG_ERR_LINE_NO2);
+          SERIAL_ERRORLN(gcode_N);
           FlushSerialRequestResend();
           serial_count = 0;
           return;
         }
 
-        if(strchr(cmdbuffer[bufindw], '*') != NULL)
-        {
+        if (strchr(command, '*') != NULL) {
           byte checksum = 0;
           byte count = 0;
-          while(cmdbuffer[bufindw][count] != '*') checksum = checksum^cmdbuffer[bufindw][count++];
-          strchr_pointer = strchr(cmdbuffer[bufindw], '*');
+          while (command[count] != '*') checksum ^= command[count++];
+          strchr_pointer = strchr(command, '*');
 
-          if(strtol(strchr_pointer + 1, NULL, 10) != checksum) {
+          if (strtol(strchr_pointer + 1, NULL, 10) != checksum) {
             SERIAL_ERROR_START;
             SERIAL_ERRORPGM(MSG_ERR_CHECKSUM_MISMATCH);
             SERIAL_ERRORLN(gcode_LastN);
@@ -718,8 +780,7 @@ void get_command()
           }
           //if no errors, continue parsing
         }
-        else
-        {
+        else {
           SERIAL_ERROR_START;
           SERIAL_ERRORPGM(MSG_ERR_NO_CHECKSUM);
           SERIAL_ERRORLN(gcode_LastN);
@@ -731,10 +792,8 @@ void get_command()
         gcode_LastN = gcode_N;
         //if no errors, continue parsing
       }
-      else  // if we don't receive 'N' but still see '*'
-      {
-        if((strchr(cmdbuffer[bufindw], '*') != NULL))
-        {
+      else {  // if we don't receive 'N' but still see '*'
+        if ((strchr(command, '*') != NULL)) {
           SERIAL_ERROR_START;
           SERIAL_ERRORPGM(MSG_ERR_NO_LINENUMBER_WITH_CHECKSUM);
           SERIAL_ERRORLN(gcode_LastN);
@@ -742,111 +801,107 @@ void get_command()
           return;
         }
       }
-      if((strchr(cmdbuffer[bufindw], 'G') != NULL)){
-        strchr_pointer = strchr(cmdbuffer[bufindw], 'G');
-        switch(strtol(strchr_pointer + 1, NULL, 10)){
-        case 0:
-        case 1:
-        case 2:
-        case 3:
-          if (IsStopped()) {
-            SERIAL_ERRORLNPGM(MSG_ERR_STOPPED);
-            LCD_MESSAGEPGM(MSG_STOPPED);
-          }
-          break;
-        default:
-          break;
-        }
 
+      if (strchr(command, 'G') != NULL) {
+        strchr_pointer = strchr(command, 'G');
+        switch (strtol(strchr_pointer + 1, NULL, 10)) {
+          case 0:
+          case 1:
+          case 2:
+          case 3:
+            if (IsStopped()) {
+              SERIAL_ERRORLNPGM(MSG_ERR_STOPPED);
+              LCD_MESSAGEPGM(MSG_STOPPED);
+            }
+            break;
+          default:
+            break;
+        }
       }
 
-      //If command was e-stop process now
-      if(strcmp(cmdbuffer[bufindw], "M112") == 0)
-        kill();
+      // If command was e-stop process now
+      if (strcmp(command, "M112") == 0) kill();
 
-      bufindw = (bufindw + 1)%BUFSIZE;
-      buflen += 1;
+      cmd_queue_index_w = (cmd_queue_index_w + 1) % BUFSIZE;
+      commands_in_queue += 1;
 
       serial_count = 0; //clear buffer
     }
-    else if(serial_char == '\\') {  //Handle escapes
-       
-        if(MYSERIAL.available() > 0  && buflen < BUFSIZE) {
-            // if we have one more character, copy it over
-            serial_char = MYSERIAL.read();
-            cmdbuffer[bufindw][serial_count++] = serial_char;
-        }
-
-        //otherwise do nothing        
+    else if (serial_char == '\\') {  // Handle escapes
+      if (MYSERIAL.available() > 0  && commands_in_queue < BUFSIZE) {
+        // if we have one more character, copy it over
+        serial_char = MYSERIAL.read();
+        command_queue[cmd_queue_index_w][serial_count++] = serial_char;
+      }
+      // otherwise do nothing
     }
     else { // its not a newline, carriage return or escape char
-        if(serial_char == ';') comment_mode = true;
-        if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;
+      if (serial_char == ';') comment_mode = true;
+      if (!comment_mode) command_queue[cmd_queue_index_w][serial_count++] = serial_char;
     }
   }
-  #ifdef SDSUPPORT
-  if(!card.sdprinting || serial_count!=0){
-    return;
-  }
 
-  //'#' stops reading from SD to the buffer prematurely, so procedural macro calls are possible
-  // if it occurs, stop_buffering is triggered and the buffer is ran dry.
-  // this character _can_ occur in serial com, due to checksums. however, no checksums are used in SD printing
-
-  static bool stop_buffering=false;
-  if(buflen==0) stop_buffering=false;
-
-  while( !card.eof()  && buflen < BUFSIZE && !stop_buffering) {
-    int16_t n=card.get();
-    serial_char = (char)n;
-    if(serial_char == '\n' ||
-       serial_char == '\r' ||
-       (serial_char == '#' && comment_mode == false) ||
-       (serial_char == ':' && comment_mode == false) ||
-       serial_count >= (MAX_CMD_SIZE - 1)||n==-1)
-    {
-      if(card.eof()){
-        SERIAL_PROTOCOLLNPGM(MSG_FILE_PRINTED);
-        stoptime=millis();
-        char time[30];
-        unsigned long t=(stoptime-starttime)/1000;
-        int hours, minutes;
-        minutes=(t/60)%60;
-        hours=t/60/60;
-        sprintf_P(time, PSTR("%i "MSG_END_HOUR" %i "MSG_END_MINUTE),hours, minutes);
-        SERIAL_ECHO_START;
-        SERIAL_ECHOLN(time);
-        lcd_setstatus(time, true);
-        card.printingHasFinished();
-        card.checkautostart(true);
+  #ifdef SDSUPPORT
 
-      }
-      if(serial_char=='#')
-        stop_buffering=true;
+    if (!card.sdprinting || serial_count) return;
+
+    // '#' stops reading from SD to the buffer prematurely, so procedural macro calls are possible
+    // if it occurs, stop_buffering is triggered and the buffer is ran dry.
+    // this character _can_ occur in serial com, due to checksums. however, no checksums are used in SD printing
+
+    static bool stop_buffering = false;
+    if (commands_in_queue == 0) stop_buffering = false;
+
+    while (!card.eof() && commands_in_queue < BUFSIZE && !stop_buffering) {
+      int16_t n = card.get();
+      serial_char = (char)n;
+      if (serial_char == '\n' || serial_char == '\r' ||
+          ((serial_char == '#' || serial_char == ':') && !comment_mode) ||
+          serial_count >= (MAX_CMD_SIZE - 1) || n == -1
+      ) {
+        if (card.eof()) {
+          SERIAL_PROTOCOLLNPGM(MSG_FILE_PRINTED);
+          print_job_stop_ms = millis();
+          char time[30];
+          millis_t t = (print_job_stop_ms - print_job_start_ms) / 1000;
+          int hours = t / 60 / 60, minutes = (t / 60) % 60;
+          sprintf_P(time, PSTR("%i " MSG_END_HOUR " %i " MSG_END_MINUTE), hours, minutes);
+          SERIAL_ECHO_START;
+          SERIAL_ECHOLN(time);
+          lcd_setstatus(time, true);
+          card.printingHasFinished();
+          card.checkautostart(true);
+        }
+        if (serial_char == '#') stop_buffering = true;
 
-      if(!serial_count)
-      {
+        if (!serial_count) {
+          comment_mode = false; //for new command
+          return; //if empty line
+        }
+        command_queue[cmd_queue_index_w][serial_count] = 0; //terminate string
+        // if (!comment_mode) {
+        fromsd[cmd_queue_index_w] = true;
+        commands_in_queue += 1;
+        cmd_queue_index_w = (cmd_queue_index_w + 1) % BUFSIZE;
+        // }
         comment_mode = false; //for new command
-        return; //if empty line
+        serial_count = 0; //clear buffer
+      }
+      else {
+        if (serial_char == ';') comment_mode = true;
+        if (!comment_mode) command_queue[cmd_queue_index_w][serial_count++] = serial_char;
       }
-      cmdbuffer[bufindw][serial_count] = 0; //terminate string
-//      if(!comment_mode){
-        fromsd[bufindw] = true;
-        buflen += 1;
-        bufindw = (bufindw + 1)%BUFSIZE;
-//      }
-      comment_mode = false; //for new command
-      serial_count = 0; //clear buffer
-    }
-    else
-    {
-      if(serial_char == ';') comment_mode = true;
-      if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;
     }
-  }
 
-  #endif //SDSUPPORT
+  #endif // SDSUPPORT
+}
 
+bool code_has_value() {
+  int i = 1;
+  char c = strchr_pointer[i];
+  if (c == '-' || c == '+') c = strchr_pointer[++i];
+  if (c == '.') c = strchr_pointer[++i];
+  return (c >= '0' && c <= '9');
 }
 
 float code_value() {
@@ -867,7 +922,7 @@ 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);
+  strchr_pointer = strchr(command_queue[cmd_queue_index_r], code);
   return (strchr_pointer != NULL);  //Return True if a character was found
 }
 
@@ -884,12 +939,12 @@ static const PROGMEM type array##_P[3] =        \
 static inline type array(int axis)          \
     { return pgm_read_any(&array##_P[axis]); }
 
-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_bump_mm, HOME_BUMP_MM);
-XYZ_CONSTS_FROM_CONFIG(signed char, home_dir,  HOME_DIR);
+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_bump_mm,   HOME_BUMP_MM);
+XYZ_CONSTS_FROM_CONFIG(signed char, home_dir, HOME_DIR);
 
 #ifdef DUAL_X_CARRIAGE
 
@@ -917,7 +972,7 @@ XYZ_CONSTS_FROM_CONFIG(signed char, home_dir,  HOME_DIR);
   static float inactive_extruder_x_pos = X2_MAX_POS; // used in mode 0 & 1
   static bool active_extruder_parked = false; // used in mode 1 & 2
   static float raised_parked_position[NUM_AXIS]; // used in mode 1
-  static unsigned long delayed_move_time = 0; // used in mode 1
+  static millis_t delayed_move_time = 0; // used in mode 1
   static float duplicate_extruder_x_offset = DEFAULT_DUPLICATION_X_OFFSET; // used in mode 2
   static float duplicate_extruder_temp_offset = 0; // used in mode 2
   bool extruder_duplication_enabled = false; // used in mode 2
@@ -945,10 +1000,10 @@ static void axis_is_at_home(int axis) {
   #endif
 
   #ifdef SCARA
-    float homeposition[3];
    
-    if (axis < 2) {
+    if (axis == X_AXIS || axis == Y_AXIS) {
 
+      float homeposition[3];
       for (int i = 0; i < 3; i++) homeposition[i] = base_home_pos(i);
 
       // SERIAL_ECHOPGM("homeposition[x]= "); SERIAL_ECHO(homeposition[0]);
@@ -978,17 +1033,18 @@ static void axis_is_at_home(int axis) {
       // inverse kinematic transform.
       min_pos[axis] = base_min_pos(axis); // + (delta[axis] - base_home_pos(axis));
       max_pos[axis] = base_max_pos(axis); // + (delta[axis] - base_home_pos(axis));
-    } 
-    else {
-      current_position[axis] = base_home_pos(axis) + home_offset[axis];
-      min_pos[axis] = base_min_pos(axis) + home_offset[axis];
-      max_pos[axis] = base_max_pos(axis) + home_offset[axis];
     }
-  #else
+    else
+  #endif
+  {
     current_position[axis] = base_home_pos(axis) + home_offset[axis];
     min_pos[axis] = base_min_pos(axis) + home_offset[axis];
     max_pos[axis] = base_max_pos(axis) + home_offset[axis];
-  #endif
+
+    #if defined(ENABLE_AUTO_BED_LEVELING) && Z_HOME_DIR < 0
+      if (axis == Z_AXIS) current_position[Z_AXIS] += zprobe_zoffset;
+    #endif
+  }
 }
 
 /**
@@ -1009,8 +1065,11 @@ inline void line_to_current_position() {
 inline void line_to_z(float zPosition) {
   plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
 }
+inline void line_to_destination(float mm_m) {
+  plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], mm_m/60, active_extruder);
+}
 inline void line_to_destination() {
-  plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+  line_to_destination(feedrate);
 }
 inline void sync_plan_position() {
   plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
@@ -1033,7 +1092,7 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
     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);
+      plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], (feedrate/60)*(feedrate_multiplier/100.0), active_extruder);
       set_current_to_destination();
     }
   #endif
@@ -1102,7 +1161,7 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
       // move down slowly until you find the bed
       feedrate = homing_feedrate[Z_AXIS] / 4;
       destination[Z_AXIS] = -10;
-      prepare_move_raw();
+      prepare_move_raw(); // this will also set_current_to_destination
       st_synchronize();
       endstops_hit_on_purpose(); // clear endstop hit flags
       
@@ -1140,15 +1199,16 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
       st_synchronize();
       endstops_hit_on_purpose(); // clear endstop hit flags
 
+      // Get the current stepper position after bumping an endstop
       current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
-      // make sure the planner knows where we are as it may be a bit different than we last said to move to
       sync_plan_position();
       
     #endif // !DELTA
   }
 
   /**
-   * 
+   *  Plan a move to (X, Y, Z) and set the current_position
+   *  The final current_position may not be the one that was requested
    */
   static void do_blocking_move_to(float x, float y, float z) {
     float oldFeedRate = feedrate;
@@ -1160,7 +1220,7 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
       destination[X_AXIS] = x;
       destination[Y_AXIS] = y;
       destination[Z_AXIS] = z;
-      prepare_move_raw();
+      prepare_move_raw(); // this will also set_current_to_destination
       st_synchronize();
 
     #else
@@ -1185,8 +1245,8 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
 
   static void setup_for_endstop_move() {
     saved_feedrate = feedrate;
-    saved_feedmultiply = feedmultiply;
-    feedmultiply = 100;
+    saved_feedrate_multiplier = feedrate_multiplier;
+    feedrate_multiplier = 100;
     refresh_cmd_timeout();
     enable_endstops(true);
   }
@@ -1196,7 +1256,7 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
       enable_endstops(false);
     #endif
     feedrate = saved_feedrate;
-    feedmultiply = saved_feedmultiply;
+    feedrate_multiplier = saved_feedrate_multiplier;
     refresh_cmd_timeout();
   }
 
@@ -1207,12 +1267,12 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
       // Engage Z Servo endstop if enabled
       if (servo_endstops[Z_AXIS] >= 0) {
         #if SERVO_LEVELING
-          servos[servo_endstops[Z_AXIS]].attach(0);
+          servo[servo_endstops[Z_AXIS]].attach(0);
         #endif
-        servos[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2]);
+        servo[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2]);
         #if SERVO_LEVELING
           delay(PROBE_SERVO_DEACTIVATION_DELAY);
-          servos[servo_endstops[Z_AXIS]].detach();
+          servo[servo_endstops[Z_AXIS]].detach();
         #endif
       }
 
@@ -1224,17 +1284,17 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
       destination[X_AXIS] = Z_PROBE_ALLEN_KEY_DEPLOY_X;
       destination[Y_AXIS] = Z_PROBE_ALLEN_KEY_DEPLOY_Y;
       destination[Z_AXIS] = Z_PROBE_ALLEN_KEY_DEPLOY_Z;
-      prepare_move_raw();
+      prepare_move_raw(); // this will also set_current_to_destination
 
       // Home X to touch the belt
       feedrate = homing_feedrate[X_AXIS]/10;
       destination[X_AXIS] = 0;
-      prepare_move_raw();
+      prepare_move_raw(); // this will also set_current_to_destination
       
       // Home Y for safety
       feedrate = homing_feedrate[X_AXIS]/2;
       destination[Y_AXIS] = 0;
-      prepare_move_raw();
+      prepare_move_raw(); // this will also set_current_to_destination
       
       st_synchronize();
 
@@ -1266,19 +1326,19 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
       if (servo_endstops[Z_AXIS] >= 0) {
 
         #if Z_RAISE_AFTER_PROBING > 0
-          do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + Z_RAISE_AFTER_PROBING);
+          do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + Z_RAISE_AFTER_PROBING); // this also updates current_position
           st_synchronize();
         #endif
 
         #if SERVO_LEVELING
-          servos[servo_endstops[Z_AXIS]].attach(0);
+          servo[servo_endstops[Z_AXIS]].attach(0);
         #endif
 
-        servos[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2 + 1]);
+        servo[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2 + 1]);
 
         #if SERVO_LEVELING
           delay(PROBE_SERVO_DEACTIVATION_DELAY);
-          servos[servo_endstops[Z_AXIS]].detach();
+          servo[servo_endstops[Z_AXIS]].detach();
         #endif
       }
 
@@ -1287,29 +1347,29 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
       // Move up for safety
       feedrate = homing_feedrate[X_AXIS];
       destination[Z_AXIS] = current_position[Z_AXIS] + Z_RAISE_AFTER_PROBING;
-      prepare_move_raw();
+      prepare_move_raw(); // this will also set_current_to_destination
 
       // Move to the start position to initiate retraction
       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();
+      prepare_move_raw(); // this will also set_current_to_destination
 
       // 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_STOW_DEPTH;
-      prepare_move_raw();
+      prepare_move_raw(); // this will also set_current_to_destination
       
       // Move up for safety
       feedrate = homing_feedrate[Z_AXIS]/2;
       destination[Z_AXIS] = current_position[Z_AXIS] + Z_PROBE_ALLEN_KEY_STOW_DEPTH * 2;
-      prepare_move_raw();
+      prepare_move_raw(); // this will also set_current_to_destination
       
       // Home XY for safety
       feedrate = homing_feedrate[X_AXIS]/2;
       destination[X_AXIS] = 0;
       destination[Y_AXIS] = 0;
-      prepare_move_raw();
+      prepare_move_raw(); // this will also set_current_to_destination
       
       st_synchronize();
 
@@ -1343,8 +1403,8 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
   // 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=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]);
+    do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_before); // this also updates current_position
+    do_blocking_move_to(x - X_PROBE_OFFSET_FROM_EXTRUDER, y - Y_PROBE_OFFSET_FROM_EXTRUDER, current_position[Z_AXIS]); // this also updates current_position
 
     #if !defined(Z_PROBE_SLED) && !defined(Z_PROBE_ALLEN_KEY)
       if (retract_action & ProbeDeploy) deploy_z_probe();
@@ -1355,7 +1415,7 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
 
     #if Z_RAISE_BETWEEN_PROBINGS > 0
       if (retract_action == ProbeStay) {
-        do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
+        do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS); // this also updates current_position
         st_synchronize();
       }
     #endif
@@ -1365,7 +1425,7 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
     #endif
 
     if (verbose_level > 2) {
-      SERIAL_PROTOCOLPGM(MSG_BED);
+      SERIAL_PROTOCOLPGM("Bed");
       SERIAL_PROTOCOLPGM(" X: ");
       SERIAL_PROTOCOL_F(x, 3);
       SERIAL_PROTOCOLPGM(" Y: ");
@@ -1452,13 +1512,11 @@ static void homeaxis(AxisEnum axis) {
 
   if (axis == X_AXIS ? HOMEAXIS_DO(X) : axis == Y_AXIS ? HOMEAXIS_DO(Y) : axis == Z_AXIS ? HOMEAXIS_DO(Z) : 0) {
 
-    int axis_home_dir;
-
-    #ifdef DUAL_X_CARRIAGE
-      if (axis == X_AXIS) axis_home_dir = x_home_dir(active_extruder);
-    #else
-      axis_home_dir = home_dir(axis);
-    #endif
+    int axis_home_dir =
+      #ifdef DUAL_X_CARRIAGE
+        (axis == X_AXIS) ? x_home_dir(active_extruder) :
+      #endif
+      home_dir(axis);
 
     // Set the axis position as setup for the move
     current_position[axis] = 0;
@@ -1472,7 +1530,7 @@ static void homeaxis(AxisEnum axis) {
       #endif
         {
           if (servo_endstops[axis] > -1)
-            servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2]);
+            servo[servo_endstops[axis]].write(servo_endstop_angles[axis * 2]);
         }
 
     #endif // SERVO_ENDSTOPS && !Z_PROBE_SLED
@@ -1491,11 +1549,15 @@ static void homeaxis(AxisEnum axis) {
     current_position[axis] = 0;
     sync_plan_position();
 
+    enable_endstops(false); // Disable endstops while moving away
+
     // 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();
 
+    enable_endstops(true); // Enable endstops for next homing move
+
     // Slow down the feedrate for the next move
     set_homing_bump_feedrate(axis);
 
@@ -1532,15 +1594,18 @@ static void homeaxis(AxisEnum axis) {
     #ifdef DELTA
       // retrace by the amount specified in endstop_adj
       if (endstop_adj[axis] * axis_home_dir < 0) {
+        enable_endstops(false); // Disable endstops while moving away
         sync_plan_position();
         destination[axis] = endstop_adj[axis];
         line_to_destination();
         st_synchronize();
+        enable_endstops(true); // Enable endstops for next homing move
       }
     #endif
 
     // Set the axis position to its home position (plus home offsets)
     axis_is_at_home(axis);
+    sync_plan_position();
 
     destination[axis] = current_position[axis];
     feedrate = 0.0;
@@ -1550,7 +1615,7 @@ static void homeaxis(AxisEnum axis) {
     // Retract Servo endstop if enabled
     #ifdef SERVO_ENDSTOPS
       if (servo_endstops[axis] > -1)
-        servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2 + 1]);
+        servo[servo_endstops[axis]].write(servo_endstop_angles[axis * 2 + 1]);
     #endif
 
     #if SERVO_LEVELING && !defined(Z_PROBE_SLED)
@@ -1619,12 +1684,12 @@ static void homeaxis(AxisEnum axis) {
     #define SLED_DOCKING_OFFSET 0
   #endif
 
-  //
-  // Method to dock/undock a sled designed by Charles Bell.
-  //
-  // dock[in]     If true, move to MAX_X and engage the electromagnet
-  // offset[in]   The additional distance to move to adjust docking location
-  //
+  /**
+   * Method to dock/undock a sled designed by Charles Bell.
+   *
+   * dock[in]     If true, move to MAX_X and engage the electromagnet
+   * offset[in]   The additional distance to move to adjust docking location
+   */
   static void dock_sled(bool dock, int offset=0) {
     if (!axis_known_position[X_AXIS] || !axis_known_position[Y_AXIS]) {
       LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
@@ -1634,12 +1699,12 @@ static void homeaxis(AxisEnum axis) {
     }
 
     if (dock) {
-      do_blocking_move_to(X_MAX_POS + SLED_DOCKING_OFFSET + offset, current_position[Y_AXIS], current_position[Z_AXIS]);
+      do_blocking_move_to(X_MAX_POS + SLED_DOCKING_OFFSET + offset, current_position[Y_AXIS], current_position[Z_AXIS]); // this also updates current_position
       digitalWrite(SERVO0_PIN, LOW); // turn off magnet
     } else {
       float z_loc = current_position[Z_AXIS];
       if (z_loc < Z_RAISE_BEFORE_PROBING + 5) z_loc = Z_RAISE_BEFORE_PROBING;
-      do_blocking_move_to(X_MAX_POS + SLED_DOCKING_OFFSET + offset, Y_PROBE_OFFSET_FROM_EXTRUDER, z_loc);
+      do_blocking_move_to(X_MAX_POS + SLED_DOCKING_OFFSET + offset, Y_PROBE_OFFSET_FROM_EXTRUDER, z_loc); // this also updates current_position
       digitalWrite(SERVO0_PIN, HIGH); // turn on magnet
     }
   }
@@ -1658,9 +1723,10 @@ static void homeaxis(AxisEnum axis) {
 inline void gcode_G0_G1() {
   if (IsRunning()) {
     get_coordinates(); // For X Y Z E F
+
     #ifdef FWRETRACT
-      if (autoretract_enabled)
-      if (!(code_seen('X') || code_seen('Y') || code_seen('Z')) && code_seen('E')) {
+
+      if (autoretract_enabled && !(code_seen('X') || code_seen('Y') || code_seen('Z')) && code_seen('E')) {
         float echange = destination[E_AXIS] - current_position[E_AXIS];
         // Is this move an attempt to retract or recover?
         if ((echange < -MIN_RETRACT && !retracted[active_extruder]) || (echange > MIN_RETRACT && retracted[active_extruder])) {
@@ -1670,7 +1736,9 @@ inline void gcode_G0_G1() {
           return;
         }
       }
+
     #endif //FWRETRACT
+
     prepare_move();
     //ClearToSend();
   }
@@ -1691,16 +1759,17 @@ inline void gcode_G2_G3(bool clockwise) {
  * G4: Dwell S<seconds> or P<milliseconds>
  */
 inline void gcode_G4() {
-  unsigned long codenum = 0;
-
-  LCD_MESSAGEPGM(MSG_DWELL);
+  millis_t codenum = 0;
 
   if (code_seen('P')) codenum = code_value_long(); // milliseconds to wait
   if (code_seen('S')) codenum = code_value_long() * 1000; // seconds to wait
 
   st_synchronize();
   refresh_cmd_timeout();
-  codenum += previous_millis_cmd;  // keep track of when we started waiting
+  codenum += previous_cmd_ms;  // keep track of when we started waiting
+
+  if (!lcd_hasstatus()) LCD_MESSAGEPGM(MSG_DWELL);
+
   while (millis() < codenum) {
     manage_heater();
     manage_inactivity();
@@ -1743,12 +1812,6 @@ inline void gcode_G4() {
  *  Y   Home to the Y endstop
  *  Z   Home to the Z endstop
  *
- * If numbers are included with XYZ set the position as with G92
- * Currently adds the home_offset, which may be wrong and removed soon.
- *
- *  Xn  Home X, setting X to n + home_offset[X_AXIS]
- *  Yn  Home Y, setting Y to n + home_offset[Y_AXIS]
- *  Zn  Home Z, setting Z to n + home_offset[Z_AXIS]
  */
 inline void gcode_G28() {
 
@@ -1767,8 +1830,8 @@ inline void gcode_G28() {
   #endif
 
   saved_feedrate = feedrate;
-  saved_feedmultiply = feedmultiply;
-  feedmultiply = 100;
+  saved_feedrate_multiplier = feedrate_multiplier;
+  feedrate_multiplier = 100;
   refresh_cmd_timeout();
 
   enable_endstops(true);
@@ -1808,23 +1871,26 @@ inline void gcode_G28() {
           homeY = code_seen(axis_codes[Y_AXIS]),
           homeZ = code_seen(axis_codes[Z_AXIS]);
 
-    home_all_axis = !(homeX || homeY || homeZ) || (homeX && homeY && homeZ);
+    home_all_axis = (!homeX && !homeY && !homeZ) || (homeX && homeY && homeZ);
 
-    #if Z_HOME_DIR > 0                      // If homing away from BED do Z first
+    if (home_all_axis || homeZ) {
 
-      if (home_all_axis || homeZ) HOMEAXIS(Z);
+      #if Z_HOME_DIR > 0  // If homing away from BED do Z first
 
-    #elif !defined(Z_SAFE_HOMING) && defined(Z_RAISE_BEFORE_HOMING) && Z_RAISE_BEFORE_HOMING > 0
+        HOMEAXIS(Z);
 
-      // 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
+      #elif !defined(Z_SAFE_HOMING) && defined(Z_RAISE_BEFORE_HOMING) && Z_RAISE_BEFORE_HOMING > 0
+
+        // Raise Z before homing any other axes
+        // (Does this need to be "negative home direction?" Why not just use Z_RAISE_BEFORE_HOMING?)
+        destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS);
         feedrate = max_feedrate[Z_AXIS] * 60;
         line_to_destination();
         st_synchronize();
-      }
 
-    #endif
+      #endif
+
+    } // home_all_axis || homeZ
 
     #ifdef QUICK_HOME
 
@@ -1870,6 +1936,11 @@ inline void gcode_G28() {
 
     #endif // QUICK_HOME
 
+    #ifdef HOME_Y_BEFORE_X
+      // Home Y
+      if (home_all_axis || homeY) HOMEAXIS(Y);
+    #endif
+
     // Home X
     if (home_all_axis || homeX) {
       #ifdef DUAL_X_CARRIAGE
@@ -1889,101 +1960,94 @@ inline void gcode_G28() {
       #endif
     }
 
-    // Home Y
-    if (home_all_axis || homeY) HOMEAXIS(Y);
-
-    // Set the X position, if included
-    // Adds the home_offset as well, which may be wrong
-    if (code_seen(axis_codes[X_AXIS])) {
-      float v = code_value();
-      if (v) current_position[X_AXIS] = v
-        #ifndef SCARA
-          + home_offset[X_AXIS]
-        #endif
-      ;
-    }
-
-    // Set the Y position, if included
-    // Adds the home_offset as well, which may be wrong
-    if (code_seen(axis_codes[Y_AXIS])) {
-      float v = code_value();
-      if (v) current_position[Y_AXIS] = v
-        #ifndef SCARA
-          + home_offset[Y_AXIS]
-        #endif
-      ;
-    }
+    #ifndef HOME_Y_BEFORE_X
+      // Home Y
+      if (home_all_axis || homeY) HOMEAXIS(Y);
+    #endif
 
     // Home Z last if homing towards the bed
     #if Z_HOME_DIR < 0
 
-      #ifndef Z_SAFE_HOMING
+      if (home_all_axis || homeZ) {
 
-        if (home_all_axis || homeZ) HOMEAXIS(Z);
+        #ifdef Z_SAFE_HOMING
 
-      #else // Z_SAFE_HOMING
+          if (home_all_axis) {
 
-        if (home_all_axis) {
-          destination[X_AXIS] = round(Z_SAFE_HOMING_X_POINT - X_PROBE_OFFSET_FROM_EXTRUDER);
-          destination[Y_AXIS] = round(Z_SAFE_HOMING_Y_POINT - Y_PROBE_OFFSET_FROM_EXTRUDER);
-          destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS);    // Set destination away from bed
-          feedrate = XY_TRAVEL_SPEED;
-          current_position[Z_AXIS] = 0;
+            current_position[Z_AXIS] = 0;
+            sync_plan_position();
 
-          sync_plan_position();
-          line_to_destination();
-          st_synchronize();
-          current_position[X_AXIS] = destination[X_AXIS];
-          current_position[Y_AXIS] = destination[Y_AXIS];
+            //
+            // Set the probe (or just the nozzle) destination to the safe homing point
+            //
+            // NOTE: If current_position[X_AXIS] or current_position[Y_AXIS] were set above
+            // then this may not work as expected.
+            destination[X_AXIS] = round(Z_SAFE_HOMING_X_POINT - X_PROBE_OFFSET_FROM_EXTRUDER);
+            destination[Y_AXIS] = round(Z_SAFE_HOMING_Y_POINT - Y_PROBE_OFFSET_FROM_EXTRUDER);
+            destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS);    // Set destination away from bed
+            feedrate = XY_TRAVEL_SPEED;
+            // This could potentially move X, Y, Z all together
+            line_to_destination();
+            st_synchronize();
 
-          HOMEAXIS(Z);
-        }
+            // Set current X, Y is the Z_SAFE_HOMING_POINT minus PROBE_OFFSET_FROM_EXTRUDER
+            current_position[X_AXIS] = destination[X_AXIS];
+            current_position[Y_AXIS] = destination[Y_AXIS];
 
-        // Let's see if X and Y are homed and probe is inside bed area.
-        if (homeZ) {
-
-          if (axis_known_position[X_AXIS] && axis_known_position[Y_AXIS]) {
-
-            float cpx = current_position[X_AXIS], cpy = current_position[Y_AXIS];
-            if (   cpx >= X_MIN_POS - X_PROBE_OFFSET_FROM_EXTRUDER
-                && cpx <= X_MAX_POS - X_PROBE_OFFSET_FROM_EXTRUDER
-                && cpy >= Y_MIN_POS - Y_PROBE_OFFSET_FROM_EXTRUDER
-                && cpy <= Y_MAX_POS - Y_PROBE_OFFSET_FROM_EXTRUDER) {
-              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] * 60;  // max_feedrate is in mm/s. line_to_destination is feedrate/60.
-              line_to_destination();
-              st_synchronize();
-              HOMEAXIS(Z);
-            }
-            else {
+            // Home the Z axis
+            HOMEAXIS(Z);
+          }
+
+          else if (homeZ) { // Don't need to Home Z twice
+
+            // Let's see if X and Y are homed
+            if (axis_known_position[X_AXIS] && axis_known_position[Y_AXIS]) {
+
+              // Make sure the probe is within the physical limits
+              // NOTE: This doesn't necessarily ensure the probe is also within the bed!
+              float cpx = current_position[X_AXIS], cpy = current_position[Y_AXIS];
+              if (   cpx >= X_MIN_POS - X_PROBE_OFFSET_FROM_EXTRUDER
+                  && cpx <= X_MAX_POS - X_PROBE_OFFSET_FROM_EXTRUDER
+                  && cpy >= Y_MIN_POS - Y_PROBE_OFFSET_FROM_EXTRUDER
+                  && cpy <= Y_MAX_POS - Y_PROBE_OFFSET_FROM_EXTRUDER) {
+                // Set the plan current position to X, Y, 0
+                current_position[Z_AXIS] = 0;
+                plan_set_position(cpx, cpy, 0, current_position[E_AXIS]); // = sync_plan_position
+
+                // Set Z destination away from bed and raise the axis
+                // NOTE: This should always just be Z_RAISE_BEFORE_HOMING unless...???
+                destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS);
+                feedrate = max_feedrate[Z_AXIS] * 60;  // feedrate (mm/m) = max_feedrate (mm/s)
+                line_to_destination();
+                st_synchronize();
+
+                // Home the Z axis
+                HOMEAXIS(Z);
+              }
+              else {
                 LCD_MESSAGEPGM(MSG_ZPROBE_OUT);
                 SERIAL_ECHO_START;
                 SERIAL_ECHOLNPGM(MSG_ZPROBE_OUT);
+              }
+            }
+            else {
+              LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
+              SERIAL_ECHO_START;
+              SERIAL_ECHOLNPGM(MSG_POSITION_UNKNOWN);
             }
-          }
-          else {
-            LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
-            SERIAL_ECHO_START;
-            SERIAL_ECHOLNPGM(MSG_POSITION_UNKNOWN);
-          }
-        }
 
-      #endif // Z_SAFE_HOMING
+          } // !home_all_axes && homeZ
 
-    #endif // Z_HOME_DIR < 0
+        #else // !Z_SAFE_HOMING
 
-    // Set the Z position, if included
-    // Adds the home_offset as well, which may be wrong
-    if (code_seen(axis_codes[Z_AXIS])) {
-      float v = code_value();
-      if (v) current_position[Z_AXIS] = v + home_offset[Z_AXIS];
-    }
+          HOMEAXIS(Z);
+
+        #endif // !Z_SAFE_HOMING
+
+      } // home_all_axis || homeZ
+
+    #endif // Z_HOME_DIR < 0
 
-    #if defined(ENABLE_AUTO_BED_LEVELING) && (Z_HOME_DIR < 0)
-      if (home_all_axis || homeZ) current_position[Z_AXIS] += zprobe_zoffset;  // Add Z_Probe offset (the distance is negative)
-    #endif
     sync_plan_position();
 
   #endif // else DELTA
@@ -2012,7 +2076,7 @@ inline void gcode_G28() {
   #endif
 
   feedrate = saved_feedrate;
-  feedmultiply = saved_feedmultiply;
+  feedrate_multiplier = saved_feedrate_multiplier;
   refresh_cmd_timeout();
   endstops_hit_on_purpose(); // clear endstop hit flags
 }
@@ -2077,7 +2141,7 @@ inline void gcode_G28() {
       case MeshStart:
         mbl.reset();
         probe_point = 0;
-        enquecommands_P(PSTR("G28\nG29 S2"));
+        enqueuecommands_P(PSTR("G28\nG29 S2"));
         break;
 
       case MeshNext:
@@ -2116,7 +2180,7 @@ inline void gcode_G28() {
           SERIAL_PROTOCOLLNPGM("Mesh probing done.");
           probe_point = -1;
           mbl.active = 1;
-          enquecommands_P(PSTR("G28"));
+          enqueuecommands_P(PSTR("G28"));
         }
         break;
 
@@ -2498,7 +2562,7 @@ inline void gcode_G28() {
     #endif
 
     #ifdef Z_PROBE_END_SCRIPT
-      enquecommands_P(PSTR(Z_PROBE_END_SCRIPT));
+      enqueuecommands_P(PSTR(Z_PROBE_END_SCRIPT));
       st_synchronize();
     #endif
   }
@@ -2514,7 +2578,7 @@ inline void gcode_G28() {
       feedrate = homing_feedrate[Z_AXIS];
 
       run_z_probe();
-      SERIAL_PROTOCOLPGM(MSG_BED);
+      SERIAL_PROTOCOLPGM("Bed");
       SERIAL_PROTOCOLPGM(" X: ");
       SERIAL_PROTOCOL(current_position[X_AXIS] + 0.0001);
       SERIAL_PROTOCOLPGM(" Y: ");
@@ -2560,7 +2624,7 @@ inline void gcode_G92() {
   inline void gcode_M0_M1() {
     char *src = strchr_pointer + 2;
 
-    unsigned long codenum = 0;
+    millis_t codenum = 0;
     bool hasP = false, hasS = false;
     if (code_seen('P')) {
       codenum = code_value_short(); // milliseconds to wait
@@ -2586,7 +2650,7 @@ inline void gcode_G92() {
     st_synchronize();
     refresh_cmd_timeout();
     if (codenum > 0) {
-      codenum += previous_millis_cmd;  // keep track of when we started waiting
+      codenum += previous_cmd_ms;  // keep track of when we started waiting
       while(millis() < codenum && !lcd_clicked()) {
         manage_heater();
         manage_inactivity();
@@ -2658,7 +2722,7 @@ inline void gcode_M17() {
    */
   inline void gcode_M24() {
     card.startFileprint();
-    starttime = millis();
+    print_job_start_ms = millis();
   }
 
   /**
@@ -2690,7 +2754,7 @@ inline void gcode_M17() {
     char* codepos = strchr_pointer + 4;
     char* starpos = strchr(codepos, '*');
     if (starpos) {
-      char* npos = strchr(cmdbuffer[bufindr], 'N');
+      char* npos = strchr(command_queue[cmd_queue_index_r], 'N');
       strchr_pointer = strchr(npos, ' ') + 1;
       *(starpos) = '\0';
     }
@@ -2713,7 +2777,7 @@ inline void gcode_M17() {
       card.closefile();
       char* starpos = strchr(strchr_pointer + 4, '*');
       if (starpos) {
-        char* npos = strchr(cmdbuffer[bufindr], 'N');
+        char* npos = strchr(command_queue[cmd_queue_index_r], 'N');
         strchr_pointer = strchr(npos, ' ') + 1;
         *(starpos) = '\0';
       }
@@ -2727,8 +2791,8 @@ inline void gcode_M17() {
  * M31: Get the time since the start of SD Print (or last M109)
  */
 inline void gcode_M31() {
-  stoptime = millis();
-  unsigned long t = (stoptime - starttime) / 1000;
+  print_job_stop_ms = millis();
+  millis_t t = (print_job_stop_ms - print_job_start_ms) / 1000;
   int min = t / 60, sec = t % 60;
   char time[30];
   sprintf_P(time, PSTR("%i min, %i sec"), min, sec);
@@ -2768,7 +2832,7 @@ inline void gcode_M31() {
 
       card.startFileprint();
       if (!call_procedure)
-        starttime = millis(); //procedure calls count as normal print time.
+        print_job_start_ms = millis(); //procedure calls count as normal print time.
     }
   }
 
@@ -2778,7 +2842,7 @@ inline void gcode_M31() {
   inline void gcode_M928() {
     char* starpos = strchr(strchr_pointer + 5, '*');
     if (starpos) {
-      char* npos = strchr(cmdbuffer[bufindr], 'N');
+      char* npos = strchr(command_queue[cmd_queue_index_r], 'N');
       strchr_pointer = strchr(npos, ' ') + 1;
       *(starpos) = '\0';
     }
@@ -2832,7 +2896,7 @@ inline void gcode_M42() {
    * M48: Z-Probe repeatability measurement function.
    *
    * Usage:
-   *   M48 <n#> <X#> <Y#> <V#> <E> <L#>
+   *   M48 <P#> <X#> <Y#> <V#> <E> <L#>
    *     P = Number of sampled points (4-50, default 10)
    *     X = Sample X position
    *     Y = Sample Y position
@@ -2844,16 +2908,12 @@ inline void gcode_M42() {
    * as been issued prior to invoking the M48 Z-Probe repeatability measurement function.
    * Any information generated by a prior G29 Bed leveling command will be lost and need to be
    * regenerated.
-   *
-   * The number of samples will default to 10 if not specified.  You can use upper or lower case
-   * letters for any of the options EXCEPT n.  n must be in lower case because Marlin uses a capital
-   * N for its communication protocol and will get horribly confused if you send it a capital N.
    */
   inline void gcode_M48() {
 
     double sum = 0.0, mean = 0.0, sigma = 0.0, sample_set[50];
-    int verbose_level = 1, n_samples = 10, n_legs = 0;
-    
+    uint8_t verbose_level = 1, n_samples = 10, n_legs = 0;
+
     if (code_seen('V') || code_seen('v')) {
       verbose_level = code_value_short();
       if (verbose_level < 0 || verbose_level > 4 ) {
@@ -2865,7 +2925,7 @@ inline void gcode_M42() {
     if (verbose_level > 0)
       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`!
+    if (code_seen('P') || code_seen('p')) {
       n_samples = code_value_short();
       if (n_samples < 4 || n_samples > 50) {
         SERIAL_PROTOCOLPGM("?Sample size not plausible (4-50).\n");
@@ -2873,12 +2933,12 @@ inline void gcode_M42() {
       }
     }
 
-    double X_probe_location, Y_probe_location,
-           X_current = X_probe_location = st_get_position_mm(X_AXIS),
-           Y_current = Y_probe_location = st_get_position_mm(Y_AXIS),
+    double X_current = st_get_position_mm(X_AXIS),
+           Y_current = st_get_position_mm(Y_AXIS),
            Z_current = st_get_position_mm(Z_AXIS),
-           Z_start_location = Z_current + Z_RAISE_BEFORE_PROBING,
-           ext_position = st_get_position_mm(E_AXIS);
+           E_current = st_get_position_mm(E_AXIS),
+           X_probe_location = X_current, Y_probe_location = Y_current,
+           Z_start_location = Z_current + Z_RAISE_BEFORE_PROBING;
 
     bool deploy_probe_for_each_reading = code_seen('E') || code_seen('e');
 
@@ -2913,10 +2973,7 @@ inline void gcode_M42() {
 
     st_synchronize();
     plan_bed_level_matrix.set_to_identity();
-    plan_buffer_line(X_current, Y_current, Z_start_location,
-        ext_position,
-        homing_feedrate[Z_AXIS] / 60,
-        active_extruder);
+    plan_buffer_line(X_current, Y_current, Z_start_location, E_current, homing_feedrate[Z_AXIS] / 60, active_extruder);
     st_synchronize();
 
     //
@@ -2925,10 +2982,10 @@ inline void gcode_M42() {
     // use that as a starting point for each probe.
     //
     if (verbose_level > 2)
-      SERIAL_PROTOCOL("Positioning the probe...\n");
+      SERIAL_PROTOCOLPGM("Positioning the probe...\n");
 
     plan_buffer_line( X_probe_location, Y_probe_location, Z_start_location,
-        ext_position,
+        E_current,
         homing_feedrate[X_AXIS]/60,
         active_extruder);
     st_synchronize();
@@ -2936,7 +2993,7 @@ inline void gcode_M42() {
     current_position[X_AXIS] = X_current = st_get_position_mm(X_AXIS);
     current_position[Y_AXIS] = Y_current = st_get_position_mm(Y_AXIS);
     current_position[Z_AXIS] = Z_current = st_get_position_mm(Z_AXIS);
-    current_position[E_AXIS] = ext_position = st_get_position_mm(E_AXIS);
+    current_position[E_AXIS] = E_current = st_get_position_mm(E_AXIS);
 
     // 
     // OK, do the inital probe to get us close to the bed.
@@ -2952,7 +3009,7 @@ inline void gcode_M42() {
     Z_start_location = st_get_position_mm(Z_AXIS) + Z_RAISE_BEFORE_PROBING;
 
     plan_buffer_line( X_probe_location, Y_probe_location, Z_start_location,
-        ext_position,
+        E_current,
         homing_feedrate[X_AXIS]/60,
         active_extruder);
     st_synchronize();
@@ -2960,12 +3017,12 @@ inline void gcode_M42() {
 
     if (deploy_probe_for_each_reading) stow_z_probe();
 
-    for (uint16_t n=0; n < n_samples; n++) {
-
-      do_blocking_move_to(X_probe_location, Y_probe_location, Z_start_location); // Make sure we are at the probe location
+    for (uint8_t n=0; n < n_samples; n++) {
+      // Make sure we are at the probe location
+      do_blocking_move_to(X_probe_location, Y_probe_location, Z_start_location); // this also updates current_position
 
       if (n_legs) {
-        unsigned long ms = millis();
+        millis_t ms = millis();
         double radius = ms % (X_MAX_LENGTH / 4),       // limit how far out to go
                theta = RADIANS(ms % 360L);
         float dir = (ms & 0x0001) ? 1 : -1;            // clockwise or counter clockwise
@@ -2975,15 +3032,15 @@ inline void gcode_M42() {
         //SERIAL_ECHOPAIR("   direction: ",dir);
         //SERIAL_EOL;
 
-        for (int l = 0; l < n_legs - 1; l++) {
+        for (uint8_t l = 0; l < n_legs - 1; l++) {
           ms = millis();
           theta += RADIANS(dir * (ms % 20L));
           radius += (ms % 10L) - 5L;
           if (radius < 0.0) radius = -radius;
 
           X_current = X_probe_location + cos(theta) * radius;
-          Y_current = Y_probe_location + sin(theta) * radius;
           X_current = constrain(X_current, X_MIN_POS, X_MAX_POS);
+          Y_current = Y_probe_location + sin(theta) * radius;
           Y_current = constrain(Y_current, Y_MIN_POS, Y_MAX_POS);
 
           if (verbose_level > 3) {
@@ -2992,11 +3049,12 @@ inline void gcode_M42() {
             SERIAL_EOL;
           }
 
-          do_blocking_move_to(X_current, Y_current, Z_current);
+          do_blocking_move_to(X_current, Y_current, Z_current); // this also updates current_position
 
         } // n_legs loop
 
-        do_blocking_move_to(X_probe_location, Y_probe_location, Z_start_location); // Go back to the probe location
+        // Go back to the probe location
+        do_blocking_move_to(X_probe_location, Y_probe_location, Z_start_location); // this also updates current_position
 
       } // n_legs
 
@@ -3014,7 +3072,7 @@ inline void gcode_M42() {
       // Get the current mean for the data points we have so far
       //
       sum = 0.0;
-      for (int j = 0; j <= n; j++) sum += sample_set[j];
+      for (uint8_t j = 0; j <= n; j++) sum += sample_set[j];
       mean = sum / (n + 1);
 
       //
@@ -3022,7 +3080,7 @@ inline void gcode_M42() {
       // data points we have so far
       //
       sum = 0.0;
-      for (int j = 0; j <= n; j++) {
+      for (uint8_t j = 0; j <= n; j++) {
         float ss = sample_set[j] - mean;
         sum += ss * ss;
       }
@@ -3098,7 +3156,7 @@ inline void gcode_M104() {
 inline void gcode_M105() {
   if (setTargetedHotend(105)) return;
 
-  #if HAS_TEMP_0 || HAS_TEMP_BED
+  #if HAS_TEMP_0 || HAS_TEMP_BED || defined(HEATER_0_USES_MAX6675)
     SERIAL_PROTOCOLPGM("ok");
     #if HAS_TEMP_0
       SERIAL_PROTOCOLPGM(" T:");
@@ -3183,8 +3241,8 @@ inline void gcode_M109() {
 
   LCD_MESSAGEPGM(MSG_HEATING);
 
-  CooldownNoWait = code_seen('S');
-  if (CooldownNoWait || code_seen('R')) {
+  no_wait_for_cooling = code_seen('S');
+  if (no_wait_for_cooling || code_seen('R')) {
     float temp = code_value();
     setTargetHotend(temp, target_extruder);
     #ifdef DUAL_X_CARRIAGE
@@ -3202,7 +3260,7 @@ inline void gcode_M109() {
 
   setWatch();
 
-  unsigned long timetemp = millis();
+  millis_t temp_ms = millis();
 
   /* See if we are heating up or cooling down */
   target_direction = isHeatingHotend(target_extruder); // true if heating, false if cooling
@@ -3210,26 +3268,26 @@ inline void gcode_M109() {
   cancel_heatup = false;
 
   #ifdef TEMP_RESIDENCY_TIME
-    long residencyStart = -1;
+    long residency_start_ms = -1;
     /* continue to loop until we have reached the target temp
       _and_ until TEMP_RESIDENCY_TIME hasn't passed since we reached it */
-    while((!cancel_heatup)&&((residencyStart == -1) ||
-          (residencyStart >= 0 && (((unsigned int) (millis() - residencyStart)) < (TEMP_RESIDENCY_TIME * 1000UL)))) )
+    while((!cancel_heatup)&&((residency_start_ms == -1) ||
+          (residency_start_ms >= 0 && (((unsigned int) (millis() - residency_start_ms)) < (TEMP_RESIDENCY_TIME * 1000UL)))) )
   #else
-    while ( target_direction ? (isHeatingHotend(target_extruder)) : (isCoolingHotend(target_extruder)&&(CooldownNoWait==false)) )
+    while ( target_direction ? (isHeatingHotend(target_extruder)) : (isCoolingHotend(target_extruder)&&(no_wait_for_cooling==false)) )
   #endif //TEMP_RESIDENCY_TIME
 
     { // while loop
-      if (millis() > timetemp + 1000UL) { //Print temp & remaining time every 1s while waiting
+      if (millis() > temp_ms + 1000UL) { //Print temp & remaining time every 1s while waiting
         SERIAL_PROTOCOLPGM("T:");
         SERIAL_PROTOCOL_F(degHotend(target_extruder),1);
         SERIAL_PROTOCOLPGM(" E:");
         SERIAL_PROTOCOL((int)target_extruder);
         #ifdef TEMP_RESIDENCY_TIME
           SERIAL_PROTOCOLPGM(" W:");
-          if (residencyStart > -1) {
-            timetemp = ((TEMP_RESIDENCY_TIME * 1000UL) - (millis() - residencyStart)) / 1000UL;
-            SERIAL_PROTOCOLLN( timetemp );
+          if (residency_start_ms > -1) {
+            temp_ms = ((TEMP_RESIDENCY_TIME * 1000UL) - (millis() - residency_start_ms)) / 1000UL;
+            SERIAL_PROTOCOLLN(temp_ms);
           }
           else {
             SERIAL_PROTOCOLLNPGM("?");
@@ -3237,7 +3295,7 @@ inline void gcode_M109() {
         #else
           SERIAL_EOL;
         #endif
-        timetemp = millis();
+        temp_ms = millis();
       }
       manage_heater();
       manage_inactivity();
@@ -3245,18 +3303,18 @@ 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(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) )
+        if ((residency_start_ms == -1 &&  target_direction && (degHotend(target_extruder) >= (degTargetHotend(target_extruder)-TEMP_WINDOW))) ||
+            (residency_start_ms == -1 && !target_direction && (degHotend(target_extruder) <= (degTargetHotend(target_extruder)+TEMP_WINDOW))) ||
+            (residency_start_ms > -1 && labs(degHotend(target_extruder) - degTargetHotend(target_extruder)) > TEMP_HYSTERESIS) )
         {
-          residencyStart = millis();
+          residency_start_ms = millis();
         }
       #endif //TEMP_RESIDENCY_TIME
     }
 
   LCD_MESSAGEPGM(MSG_HEATING_COMPLETE);
   refresh_cmd_timeout();
-  starttime = previous_millis_cmd;
+  print_job_start_ms = previous_cmd_ms;
 }
 
 #if HAS_TEMP_BED
@@ -3267,19 +3325,19 @@ inline void gcode_M109() {
    */
   inline void gcode_M190() {
     LCD_MESSAGEPGM(MSG_BED_HEATING);
-    CooldownNoWait = code_seen('S');
-    if (CooldownNoWait || code_seen('R'))
+    no_wait_for_cooling = code_seen('S');
+    if (no_wait_for_cooling || code_seen('R'))
       setTargetBed(code_value());
 
-    unsigned long timetemp = millis();
+    millis_t temp_ms = millis();
     
     cancel_heatup = false;
     target_direction = isHeatingBed(); // true if heating, false if cooling
 
-    while ( (target_direction)&&(!cancel_heatup) ? (isHeatingBed()) : (isCoolingBed()&&(CooldownNoWait==false)) ) {
-      unsigned long ms = millis();
-      if (ms > timetemp + 1000UL) { //Print Temp Reading every 1 second while heating up.
-        timetemp = ms;
+    while ((target_direction && !cancel_heatup) ? isHeatingBed() : isCoolingBed() && !no_wait_for_cooling) {
+      millis_t ms = millis();
+      if (ms > temp_ms + 1000UL) { //Print Temp Reading every 1 second while heating up.
+        temp_ms = ms;
         float tt = degHotend(active_extruder);
         SERIAL_PROTOCOLPGM("T:");
         SERIAL_PROTOCOL(tt);
@@ -3300,12 +3358,17 @@ inline void gcode_M109() {
 #endif // HAS_TEMP_BED
 
 /**
- * M112: Emergency Stop
+ * M111: Set the debug level
  */
-inline void gcode_M112() {
-  kill();
+inline void gcode_M111() {
+  marlin_debug_flags = code_seen('S') ? code_value_short() : DEBUG_INFO|DEBUG_ERRORS;
 }
 
+/**
+ * M112: Emergency Stop
+ */
+inline void gcode_M112() { kill(); }
+
 #ifdef BARICUDA
 
   #if HAS_HEATER_1
@@ -3339,6 +3402,62 @@ inline void gcode_M140() {
   if (code_seen('S')) setTargetBed(code_value());
 }
 
+#ifdef ULTIPANEL
+
+  /**
+   * M145: Set the heatup state for a material in the LCD menu
+   *   S<material> (0=PLA, 1=ABS)
+   *   H<hotend temp>
+   *   B<bed temp>
+   *   F<fan speed>
+   */
+  inline void gcode_M145() {
+    uint8_t material = code_seen('S') ? code_value_short() : 0;
+    if (material < 0 || material > 1) {
+      SERIAL_ERROR_START;
+      SERIAL_ERRORLNPGM(MSG_ERR_MATERIAL_INDEX);
+    }
+    else {
+      int v;
+      switch (material) {
+        case 0:
+          if (code_seen('H')) {
+            v = code_value_short();
+            plaPreheatHotendTemp = constrain(v, EXTRUDE_MINTEMP, HEATER_0_MAXTEMP - 15);
+          }
+          if (code_seen('F')) {
+            v = code_value_short();
+            plaPreheatFanSpeed = constrain(v, 0, 255);
+          }
+          #if TEMP_SENSOR_BED != 0
+            if (code_seen('B')) {
+              v = code_value_short();
+              plaPreheatHPBTemp = constrain(v, BED_MINTEMP, BED_MAXTEMP - 15);
+            }
+          #endif
+          break;
+        case 1:
+          if (code_seen('H')) {
+            v = code_value_short();
+            absPreheatHotendTemp = constrain(v, EXTRUDE_MINTEMP, HEATER_0_MAXTEMP - 15);
+          }
+          if (code_seen('F')) {
+            v = code_value_short();
+            absPreheatFanSpeed = constrain(v, 0, 255);
+          }
+          #if TEMP_SENSOR_BED != 0
+            if (code_seen('B')) {
+              v = code_value_short();
+              absPreheatHPBTemp = constrain(v, BED_MINTEMP, BED_MAXTEMP - 15);
+            }
+          #endif
+          break;
+      }
+    }
+  }
+
+#endif
+
 #if HAS_POWER_SWITCH
 
   /**
@@ -3369,7 +3488,7 @@ inline void gcode_M140() {
  *      This code should ALWAYS be available for EMERGENCY SHUTDOWN!
  */
 inline void gcode_M81() {
-  disable_heater();
+  disable_all_heaters();
   st_synchronize();
   disable_e0();
   disable_e1();
@@ -3446,7 +3565,8 @@ inline void gcode_M85() {
 }
 
 /**
- * M92: Set inactivity shutdown timer with parameter S<seconds>. To disable set zero (default)
+ * M92: Set axis steps-per-unit for one or more axes, X, Y, Z, and E.
+ *      (Follows the same syntax as G92)
  */
 inline void gcode_M92() {
   for(int8_t i=0; i < NUM_AXIS; i++) {
@@ -3747,41 +3867,56 @@ inline void gcode_M206() {
    * M666: Set delta endstop adjustment
    */
   inline void gcode_M666() {
-    for (int8_t i = 0; i < 3; i++) {
+    for (int8_t i = X_AXIS; i <= Z_AXIS; i++) {
       if (code_seen(axis_codes[i])) {
         endstop_adj[i] = code_value();
       }
     }
   }
-#elif defined(Z_DUAL_ENDSTOPS)
+#elif defined(Z_DUAL_ENDSTOPS) // !DELTA && defined(Z_DUAL_ENDSTOPS)
   /**
    * M666: For Z Dual Endstop setup, set z axis offset to the z2 axis.
    */
   inline void gcode_M666() {
-   if (code_seen('Z')) z_endstop_adj = code_value();
-   SERIAL_ECHOPAIR("Z Endstop Adjustment set to (mm):", z_endstop_adj );
-   SERIAL_EOL;
+    if (code_seen('Z')) z_endstop_adj = code_value();
+    SERIAL_ECHOPAIR("Z Endstop Adjustment set to (mm):", z_endstop_adj);
+    SERIAL_EOL;
   }
   
-#endif // DELTA
+#endif // !DELTA && defined(Z_DUAL_ENDSTOPS)
 
 #ifdef FWRETRACT
 
   /**
-   * M207: Set retract length S[positive mm] F[feedrate mm/min] Z[additional zlift/hop]
+   * M207: Set firmware retraction values
+   *
+   *   S[+mm]    retract_length
+   *   W[+mm]    retract_length_swap (multi-extruder)
+   *   F[mm/min] retract_feedrate
+   *   Z[mm]     retract_zlift
    */
   inline void gcode_M207() {
     if (code_seen('S')) retract_length = code_value();
     if (code_seen('F')) retract_feedrate = code_value() / 60;
     if (code_seen('Z')) retract_zlift = code_value();
+    #if EXTRUDERS > 1
+      if (code_seen('W')) retract_length_swap = code_value();
+    #endif
   }
 
   /**
-   * M208: Set retract recover length S[positive mm surplus to the M207 S*] F[feedrate mm/min]
+   * M208: Set firmware un-retraction values
+   *
+   *   S[+mm]    retract_recover_length (in addition to M207 S*)
+   *   W[+mm]    retract_recover_length_swap (multi-extruder)
+   *   F[mm/min] retract_recover_feedrate
    */
   inline void gcode_M208() {
     if (code_seen('S')) retract_recover_length = code_value();
     if (code_seen('F')) retract_recover_feedrate = code_value() / 60;
+    #if EXTRUDERS > 1
+      if (code_seen('W')) retract_recover_length_swap = code_value();
+    #endif
   }
 
   /**
@@ -3801,7 +3936,7 @@ inline void gcode_M206() {
         default:
           SERIAL_ECHO_START;
           SERIAL_ECHOPGM(MSG_UNKNOWN_COMMAND);
-          SERIAL_ECHO(cmdbuffer[bufindr]);
+          SERIAL_ECHO(command_queue[cmd_queue_index_r]);
           SERIAL_ECHOLNPGM("\"");
           return;
       }
@@ -3847,7 +3982,7 @@ inline void gcode_M206() {
  * M220: Set speed percentage factor, aka "Feed Rate" (M220 S95)
  */
 inline void gcode_M220() {
-  if (code_seen('S')) feedmultiply = code_value();
+  if (code_seen('S')) feedrate_multiplier = code_value();
 }
 
 /**
@@ -3928,12 +4063,12 @@ inline void gcode_M226() {
       servo_position = code_value();
       if ((servo_index >= 0) && (servo_index < NUM_SERVOS)) {
         #if SERVO_LEVELING
-          servos[servo_index].attach(0);
+          servo[servo_index].attach(0);
         #endif
-        servos[servo_index].write(servo_position);
+        servo[servo_index].write(servo_position);
         #if SERVO_LEVELING
           delay(PROBE_SERVO_DEACTIVATION_DELAY);
-          servos[servo_index].detach();
+          servo[servo_index].detach();
         #endif
       }
       else {
@@ -3948,38 +4083,26 @@ inline void gcode_M226() {
       SERIAL_PROTOCOL(" Servo ");
       SERIAL_PROTOCOL(servo_index);
       SERIAL_PROTOCOL(": ");
-      SERIAL_PROTOCOL(servos[servo_index].read());
+      SERIAL_PROTOCOL(servo[servo_index].read());
       SERIAL_EOL;
     }
   }
 
 #endif // NUM_SERVOS > 0
 
-#if defined(LARGE_FLASH) && (BEEPER > 0 || defined(ULTRALCD) || defined(LCD_USE_I2C_BUZZER))
+#if BEEPER > 0 || defined(ULTRALCD) || defined(LCD_USE_I2C_BUZZER)
 
   /**
    * M300: Play beep sound S<frequency Hz> P<duration ms>
    */
   inline void gcode_M300() {
-    int beepS = code_seen('S') ? code_value() : 110;
-    int beepP = code_seen('P') ? code_value() : 1000;
-    if (beepS > 0) {
-      #if BEEPER > 0
-        tone(BEEPER, beepS);
-        delay(beepP);
-        noTone(BEEPER);
-      #elif defined(ULTRALCD)
-        lcd_buzz(beepS, beepP);
-      #elif defined(LCD_USE_I2C_BUZZER)
-        lcd_buzz(beepP, beepS);
-      #endif
-    }
-    else {
-      delay(beepP);
-    }
+    uint16_t beepS = code_seen('S') ? code_value_short() : 110;
+    uint32_t beepP = code_seen('P') ? code_value_long() : 1000;
+    if (beepP > 5000) beepP = 5000; // limit to 5 seconds
+    lcd_buzz(beepP, beepS);
   }
 
-#endif // LARGE_FLASH && (BEEPER>0 || ULTRALCD || LCD_USE_I2C_BUZZER)
+#endif // BEEPER>0 || ULTRALCD || LCD_USE_I2C_BUZZER
 
 #ifdef PIDTEMP
 
@@ -4083,7 +4206,7 @@ inline void gcode_M226() {
 
 #endif // CHDK || PHOTOGRAPH_PIN
 
-#ifdef DOGLCD
+#ifdef HAS_LCD_CONTRAST
 
   /**
    * M250: Read and optionally set the LCD contrast
@@ -4095,10 +4218,12 @@ inline void gcode_M226() {
     SERIAL_EOL;
   }
 
-#endif // DOGLCD
+#endif // HAS_LCD_CONTRAST
 
 #ifdef PREVENT_DANGEROUS_EXTRUDE
 
+  void set_extrude_min_temp(float temp) { extrude_min_temp = temp; }
+
   /**
    * M302: Allow cold extrudes, or set the minimum extrude S<temperature>.
    */
@@ -4249,14 +4374,34 @@ inline void gcode_M400() { st_synchronize(); }
 
 #if defined(ENABLE_AUTO_BED_LEVELING) && (defined(SERVO_ENDSTOPS) || defined(Z_PROBE_ALLEN_KEY)) && not defined(Z_PROBE_SLED)
 
+  #ifdef SERVO_ENDSTOPS
+    void raise_z_for_servo() {
+      float zpos = current_position[Z_AXIS], z_dest = Z_RAISE_BEFORE_HOMING;
+      if (!axis_known_position[Z_AXIS]) z_dest += zpos;
+      if (zpos < z_dest)
+        do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_dest); // also updates current_position
+    }
+  #endif
+
   /**
    * M401: Engage Z Servo endstop if available
    */
-  inline void gcode_M401() { deploy_z_probe(); }
+  inline void gcode_M401() {
+    #ifdef SERVO_ENDSTOPS
+      raise_z_for_servo();
+    #endif
+    deploy_z_probe();
+  }
+
   /**
    * M402: Retract Z Servo endstop if enabled
    */
-  inline void gcode_M402() { stow_z_probe(); }
+  inline void gcode_M402() {
+    #ifdef SERVO_ENDSTOPS
+      raise_z_for_servo();
+    #endif
+    stow_z_probe();
+  }
 
 #endif
 
@@ -4316,6 +4461,49 @@ inline void gcode_M400() { st_synchronize(); }
 
 #endif // FILAMENT_SENSOR
 
+/**
+ * M410: Quickstop - Abort all planned moves
+ *
+ * This will stop the carriages mid-move, so most likely they
+ * will be out of sync with the stepper position after this.
+ */
+inline void gcode_M410() { quickStop(); }
+
+
+#ifdef MESH_BED_LEVELING
+
+  /**
+   * M420: Enable/Disable Mesh Bed Leveling
+   */
+  inline void gcode_M420() { if (code_seen('S') && code_has_value()) mbl.active = !!code_value_short(); }
+
+  /**
+   * M421: Set a single Mesh Bed Leveling Z coordinate
+   */
+  inline void gcode_M421() {
+    float x, y, z;
+    bool err = false, hasX, hasY, hasZ;
+    if ((hasX = code_seen('X'))) x = code_value();
+    if ((hasY = code_seen('Y'))) y = code_value();
+    if ((hasZ = code_seen('Z'))) z = code_value();
+
+    if (!hasX || !hasY || !hasZ) {
+      SERIAL_ERROR_START;
+      SERIAL_ERRORLNPGM(MSG_ERR_M421_REQUIRES_XYZ);
+      err = true;
+    }
+
+    if (x >= MESH_NUM_X_POINTS || y >= MESH_NUM_Y_POINTS) {
+      SERIAL_ERROR_START;
+      SERIAL_ERRORLNPGM(MSG_ERR_MESH_INDEX_OOB);
+      err = true;
+    }
+
+    if (!err) mbl.set_z(select_x_index(x), select_y_index(y), z);
+  }
+
+#endif
+
 /**
  * M500: Store settings in EEPROM
  */
@@ -4362,7 +4550,7 @@ inline void gcode_M503() {
     if (code_seen('Z')) {
       value = code_value();
       if (Z_PROBE_OFFSET_RANGE_MIN <= value && value <= Z_PROBE_OFFSET_RANGE_MAX) {
-        zprobe_zoffset = -value; // compare w/ line 278 of ConfigurationStore.cpp
+        zprobe_zoffset = -value; // compare w/ line 278 of configuration_store.cpp
         SERIAL_ECHO_START;
         SERIAL_ECHOLNPGM(MSG_ZPROBE_ZOFFSET " " MSG_OK);
         SERIAL_EOL;
@@ -4451,24 +4639,10 @@ inline void gcode_M503() {
     LCD_ALERTMESSAGEPGM(MSG_FILAMENTCHANGE);
     uint8_t cnt = 0;
     while (!lcd_clicked()) {
-      cnt++;
+      if (++cnt == 0) lcd_quick_feedback(); // every 256th frame till the lcd is clicked
       manage_heater();
       manage_inactivity(true);
       lcd_update();
-      if (cnt == 0) {
-        #if BEEPER > 0
-          OUT_WRITE(BEEPER,HIGH);
-          delay(3);
-          WRITE(BEEPER,LOW);
-          delay(3);
-        #else
-          #if !defined(LCD_FEEDBACK_FREQUENCY_HZ) || !defined(LCD_FEEDBACK_FREQUENCY_DURATION_MS)
-            lcd_buzz(1000/6, 100);
-          #else
-            lcd_buzz(LCD_FEEDBACK_FREQUENCY_DURATION_MS, LCD_FEEDBACK_FREQUENCY_HZ);
-          #endif
-        #endif
-      }
     } // while(!lcd_clicked)
 
     //return to normal
@@ -4495,7 +4669,7 @@ inline void gcode_M503() {
     #endif        
 
     #ifdef FILAMENT_RUNOUT_SENSOR
-      filrunoutEnqued = false;
+      filrunoutEnqueued = false;
     #endif
     
   }
@@ -4629,6 +4803,9 @@ inline void gcode_M999() {
   FlushSerialRequestResend();
 }
 
+/**
+ * T0-T3: Switch tool, usually switching extruders
+ */
 inline void gcode_T() {
   int tmp_extruder = code_value();
   if (tmp_extruder >= EXTRUDERS) {
@@ -4737,6 +4914,12 @@ inline void gcode_T() {
  * This is called from the main loop()
  */
 void process_commands() {
+
+  if ((marlin_debug_flags & DEBUG_ECHO)) {
+    SERIAL_ECHO_START;
+    SERIAL_ECHOLN(command_queue[cmd_queue_index_r]);
+  }
+
   if (code_seen('G')) {
 
     int gCode = code_value_short();
@@ -4875,34 +5058,38 @@ void process_commands() {
         gcode_M104();
         break;
 
-      case 112: //  M112 Emergency Stop
+      case 111: // M111: Set debug level
+        gcode_M111();
+        break;
+
+      case 112: // M112: Emergency Stop
         gcode_M112();
         break;
 
-      case 140: // M140 Set bed temp
+      case 140: // M140: Set bed temp
         gcode_M140();
         break;
 
-      case 105: // M105 Read current temperature
+      case 105: // M105: Read current temperature
         gcode_M105();
         return;
         break;
 
-      case 109: // M109 Wait for temperature
+      case 109: // M109: Wait for temperature
         gcode_M109();
         break;
 
       #if HAS_TEMP_BED
-        case 190: // M190 - Wait for bed heater to reach target.
+        case 190: // M190: Wait for bed heater to reach target
           gcode_M190();
           break;
       #endif // HAS_TEMP_BED
 
       #if HAS_FAN
-        case 106: //M106 Fan On
+        case 106: // M106: Fan On
           gcode_M106();
           break;
-        case 107: //M107 Fan Off
+        case 107: // M107: Fan Off
           gcode_M107();
           break;
       #endif // HAS_FAN
@@ -4910,20 +5097,20 @@ void process_commands() {
       #ifdef BARICUDA
         // PWM for HEATER_1_PIN
         #if HAS_HEATER_1
-          case 126: // M126 valve open
+          case 126: // M126: valve open
             gcode_M126();
             break;
-          case 127: // M127 valve closed
+          case 127: // M127: valve closed
             gcode_M127();
             break;
         #endif // HAS_HEATER_1
 
         // PWM for HEATER_2_PIN
         #if HAS_HEATER_2
-          case 128: // M128 valve open
+          case 128: // M128: valve open
             gcode_M128();
             break;
-          case 129: // M129 valve closed
+          case 129: // M129: valve closed
             gcode_M129();
             break;
         #endif // HAS_HEATER_2
@@ -4931,13 +5118,13 @@ void process_commands() {
 
       #if HAS_POWER_SWITCH
 
-        case 80: // M80 - Turn on Power Supply
+        case 80: // M80: Turn on Power Supply
           gcode_M80();
           break;
 
       #endif // HAS_POWER_SWITCH
 
-      case 81: // M81 - Turn off Power, including Power Supply, if possible
+      case 81: // M81: Turn off Power, including Power Supply, if possible
         gcode_M81();
         break;
 
@@ -4947,35 +5134,42 @@ void process_commands() {
       case 83:
         gcode_M83();
         break;
-      case 18: //compatibility
+      case 18: // (for compatibility)
       case 84: // M84
         gcode_M18_M84();
         break;
       case 85: // M85
         gcode_M85();
         break;
-      case 92: // M92
+      case 92: // M92: Set the steps-per-unit for one or more axes
         gcode_M92();
         break;
-      case 115: // M115
+      case 115: // M115: Report capabilities
         gcode_M115();
         break;
-      case 117: // M117 display message
+      case 117: // M117: Set LCD message text
         gcode_M117();
         break;
-      case 114: // M114
+      case 114: // M114: Report current position
         gcode_M114();
         break;
-      case 120: // M120
+      case 120: // M120: Enable endstops
         gcode_M120();
         break;
-      case 121: // M121
+      case 121: // M121: Disable endstops
         gcode_M121();
         break;
-      case 119: // M119
+      case 119: // M119: Report endstop states
         gcode_M119();
         break;
-        //TODO: update for all axis, use for loop
+
+      #ifdef ULTIPANEL
+
+        case 145: // M145: Set material heatup parameters
+          gcode_M145();
+          break;
+
+      #endif
 
       #ifdef BLINKM
 
@@ -5057,11 +5251,11 @@ void process_commands() {
           break;
       #endif // NUM_SERVOS > 0
 
-      #if defined(LARGE_FLASH) && (BEEPER > 0 || defined(ULTRALCD) || defined(LCD_USE_I2C_BUZZER))
+      #if BEEPER > 0 || defined(ULTRALCD) || defined(LCD_USE_I2C_BUZZER)
         case 300: // M300 - Play beep tone
           gcode_M300();
           break;
-      #endif // LARGE_FLASH && (BEEPER>0 || ULTRALCD || LCD_USE_I2C_BUZZER)
+      #endif // BEEPER > 0 || ULTRALCD || LCD_USE_I2C_BUZZER
 
       #ifdef PIDTEMP
         case 301: // M301
@@ -5081,11 +5275,11 @@ void process_commands() {
           break;
       #endif // CHDK || PHOTOGRAPH_PIN
 
-      #ifdef DOGLCD
+      #ifdef HAS_LCD_CONTRAST
         case 250: // M250  Set LCD contrast value: C<value> (value 0..63)
           gcode_M250();
           break;
-      #endif // DOGLCD
+      #endif // HAS_LCD_CONTRAST
 
       #ifdef PREVENT_DANGEROUS_EXTRUDE
         case 302: // allow cold extrudes, or set the minimum extrude temperature
@@ -5146,6 +5340,19 @@ void process_commands() {
           break;
       #endif // FILAMENT_SENSOR
 
+      case 410: // M410 quickstop - Abort all the planned moves.
+        gcode_M410();
+        break;
+
+      #ifdef MESH_BED_LEVELING
+        case 420: // M420 Enable/Disable Mesh Bed Leveling
+          gcode_M420();
+          break;
+        case 421: // M421 Set a Mesh Bed Leveling Z coordinate
+          gcode_M421();
+          break;
+      #endif
+
       case 500: // M500 Store settings in EEPROM
         gcode_M500();
         break;
@@ -5218,7 +5425,7 @@ void process_commands() {
   else {
     SERIAL_ECHO_START;
     SERIAL_ECHOPGM(MSG_UNKNOWN_COMMAND);
-    SERIAL_ECHO(cmdbuffer[bufindr]);
+    SERIAL_ECHO(command_queue[cmd_queue_index_r]);
     SERIAL_ECHOLNPGM("\"");
   }
 
@@ -5226,7 +5433,7 @@ void process_commands() {
 }
 
 void FlushSerialRequestResend() {
-  //char cmdbuffer[bufindr][100]="Resend:";
+  //char command_queue[cmd_queue_index_r][100]="Resend:";
   MYSERIAL.flush();
   SERIAL_PROTOCOLPGM(MSG_RESEND);
   SERIAL_PROTOCOLLN(gcode_LastN + 1);
@@ -5236,7 +5443,7 @@ void FlushSerialRequestResend() {
 void ClearToSend() {
   refresh_cmd_timeout();
   #ifdef SDSUPPORT
-    if (fromsd[bufindr]) return;
+    if (fromsd[cmd_queue_index_r]) return;
   #endif
   SERIAL_PROTOCOLLNPGM(MSG_OK);
 }
@@ -5268,25 +5475,23 @@ void get_arc_coordinates() {
   offset[1] = code_seen('J') ? code_value() : 0;
 }
 
-void clamp_to_software_endstops(float target[3])
-{
+void clamp_to_software_endstops(float target[3]) {
   if (min_software_endstops) {
-    if (target[X_AXIS] < min_pos[X_AXIS]) target[X_AXIS] = min_pos[X_AXIS];
-    if (target[Y_AXIS] < min_pos[Y_AXIS]) target[Y_AXIS] = min_pos[Y_AXIS];
+    NOLESS(target[X_AXIS], min_pos[X_AXIS]);
+    NOLESS(target[Y_AXIS], min_pos[Y_AXIS]);
     
     float negative_z_offset = 0;
     #ifdef ENABLE_AUTO_BED_LEVELING
-      if (Z_PROBE_OFFSET_FROM_EXTRUDER < 0) negative_z_offset = negative_z_offset + Z_PROBE_OFFSET_FROM_EXTRUDER;
-      if (home_offset[Z_AXIS] < 0) negative_z_offset = negative_z_offset + home_offset[Z_AXIS];
+      if (Z_PROBE_OFFSET_FROM_EXTRUDER < 0) negative_z_offset += Z_PROBE_OFFSET_FROM_EXTRUDER;
+      if (home_offset[Z_AXIS] < 0) negative_z_offset += home_offset[Z_AXIS];
     #endif
-    
-    if (target[Z_AXIS] < min_pos[Z_AXIS]+negative_z_offset) target[Z_AXIS] = min_pos[Z_AXIS]+negative_z_offset;
+    NOLESS(target[Z_AXIS], min_pos[Z_AXIS] + negative_z_offset);
   }
 
   if (max_software_endstops) {
-    if (target[X_AXIS] > max_pos[X_AXIS]) target[X_AXIS] = max_pos[X_AXIS];
-    if (target[Y_AXIS] > max_pos[Y_AXIS]) target[Y_AXIS] = max_pos[Y_AXIS];
-    if (target[Z_AXIS] > max_pos[Z_AXIS]) target[Z_AXIS] = max_pos[Z_AXIS];
+    NOMORE(target[X_AXIS], max_pos[X_AXIS]);
+    NOMORE(target[Y_AXIS], max_pos[Y_AXIS]);
+    NOMORE(target[Z_AXIS], max_pos[Z_AXIS]);
   }
 }
 
@@ -5441,21 +5646,48 @@ void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_
 }
 #endif  // MESH_BED_LEVELING
 
+#ifdef PREVENT_DANGEROUS_EXTRUDE
+
+  inline float prevent_dangerous_extrude(float &curr_e, float &dest_e) {
+    float de = dest_e - curr_e;
+    if (de) {
+      if (degHotend(active_extruder) < extrude_min_temp) {
+        curr_e = dest_e; // Behave as if the move really took place, but ignore E part
+        SERIAL_ECHO_START;
+        SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
+        return 0;
+      }
+      #ifdef PREVENT_LENGTHY_EXTRUDE
+        if (labs(de) > EXTRUDE_MAXLENGTH) {
+          curr_e = dest_e; // Behave as if the move really took place, but ignore E part
+          SERIAL_ECHO_START;
+          SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
+          return 0;
+        }
+      #endif
+    }
+    return de;
+  }
+
+#endif // PREVENT_DANGEROUS_EXTRUDE
+
 void prepare_move() {
   clamp_to_software_endstops(destination);
   refresh_cmd_timeout();
-  
+
+  #ifdef PREVENT_DANGEROUS_EXTRUDE
+    (void)prevent_dangerous_extrude(current_position[E_AXIS], destination[E_AXIS]);
+  #endif
+
   #ifdef SCARA //for now same as delta-code
 
     float difference[NUM_AXIS];
     for (int8_t i = 0; i < NUM_AXIS; i++) difference[i] = destination[i] - current_position[i];
 
-    float cartesian_mm = sqrt(  sq(difference[X_AXIS]) +
-                                sq(difference[Y_AXIS]) +
-                                sq(difference[Z_AXIS]));
+    float cartesian_mm = sqrt(sq(difference[X_AXIS]) + sq(difference[Y_AXIS]) + sq(difference[Z_AXIS]));
     if (cartesian_mm < 0.000001) { cartesian_mm = abs(difference[E_AXIS]); }
     if (cartesian_mm < 0.000001) { return; }
-    float seconds = 6000 * cartesian_mm / feedrate / feedmultiply;
+    float seconds = 6000 * cartesian_mm / feedrate / feedrate_multiplier;
     int steps = max(1, int(scara_segments_per_second * seconds));
 
     //SERIAL_ECHOPGM("mm="); SERIAL_ECHO(cartesian_mm);
@@ -5464,9 +5696,7 @@ void prepare_move() {
 
     for (int s = 1; s <= steps; s++) {
       float fraction = float(s) / float(steps);
-      for(int8_t i = 0; i < NUM_AXIS; i++) {
-        destination[i] = current_position[i] + difference[i] * fraction;
-      }
+      for (int8_t i = 0; i < NUM_AXIS; i++) destination[i] = current_position[i] + difference[i] * fraction;
   
       calculate_delta(destination);
       //SERIAL_ECHOPGM("destination[X_AXIS]="); SERIAL_ECHOLN(destination[X_AXIS]);
@@ -5476,9 +5706,7 @@ void prepare_move() {
       //SERIAL_ECHOPGM("delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]);
       //SERIAL_ECHOPGM("delta[Z_AXIS]="); SERIAL_ECHOLN(delta[Z_AXIS]);
 
-      plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],
-        destination[E_AXIS], feedrate*feedmultiply/60/100.0,
-        active_extruder);
+      plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], feedrate/60*feedrate_multiplier/100.0, active_extruder);
     }
 
   #endif // SCARA
@@ -5488,12 +5716,10 @@ void prepare_move() {
     float difference[NUM_AXIS];
     for (int8_t i=0; i < NUM_AXIS; i++) difference[i] = destination[i] - current_position[i];
 
-    float cartesian_mm = sqrt(sq(difference[X_AXIS]) +
-                              sq(difference[Y_AXIS]) +
-                              sq(difference[Z_AXIS]));
+    float cartesian_mm = sqrt(sq(difference[X_AXIS]) + sq(difference[Y_AXIS]) + sq(difference[Z_AXIS]));
     if (cartesian_mm < 0.000001) cartesian_mm = abs(difference[E_AXIS]);
     if (cartesian_mm < 0.000001) return;
-    float seconds = 6000 * cartesian_mm / feedrate / feedmultiply;
+    float seconds = 6000 * cartesian_mm / feedrate / feedrate_multiplier;
     int steps = max(1, int(delta_segments_per_second * seconds));
 
     // SERIAL_ECHOPGM("mm="); SERIAL_ECHO(cartesian_mm);
@@ -5507,9 +5733,7 @@ void prepare_move() {
       #ifdef ENABLE_AUTO_BED_LEVELING
         adjust_delta(destination);
       #endif
-      plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],
-                       destination[E_AXIS], feedrate*feedmultiply/60/100.0,
-                       active_extruder);
+      plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], feedrate/60*feedrate_multiplier/100.0, active_extruder);
     }
 
   #endif // DELTA
@@ -5519,8 +5743,8 @@ void prepare_move() {
       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);
+        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;
@@ -5528,39 +5752,37 @@ void prepare_move() {
       }
       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)
+          // This is a travel move (with no extrusion)
+          // Skip it, but keep track of the current position
+          // (so it can be used as the start of the next 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];
+            NOLESS(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);
+        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
 
   #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])) {
+    // Do not use feedrate_multiplier 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);
+        mesh_plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], (feedrate/60)*(feedrate_multiplier/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);
+        line_to_destination(feedrate * feedrate_multiplier / 100.0);
       #endif  // MESH_BED_LEVELING
     }
   #endif // !(DELTA || SCARA)
@@ -5572,7 +5794,7 @@ void prepare_arc_move(char isclockwise) {
   float r = hypot(offset[X_AXIS], offset[Y_AXIS]); // Compute arc radius for mc_arc
 
   // Trace the arc
-  mc_arc(current_position, destination, offset, X_AXIS, Y_AXIS, Z_AXIS, feedrate*feedmultiply/60/100.0, r, isclockwise, active_extruder);
+  mc_arc(current_position, destination, offset, X_AXIS, Y_AXIS, Z_AXIS, feedrate*feedrate_multiplier/60/100.0, r, isclockwise, active_extruder);
 
   // 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
@@ -5583,11 +5805,11 @@ void prepare_arc_move(char isclockwise) {
 
 #if HAS_CONTROLLERFAN
 
-unsigned long lastMotor = 0; // Last time a motor was turned on
-unsigned long lastMotorCheck = 0; // Last time the state was checked
+millis_t lastMotor = 0; // Last time a motor was turned on
+millis_t lastMotorCheck = 0; // Last time the state was checked
 
 void controllerFan() {
-  uint32_t ms = millis();
+  millis_t ms = millis();
   if (ms >= lastMotorCheck + 2500) { // Not a time critical function, so we only check every 2500ms
     lastMotorCheck = ms;
     if (X_ENABLE_READ == X_ENABLE_ON || Y_ENABLE_READ == Y_ENABLE_ON || Z_ENABLE_READ == Z_ENABLE_ON || soft_pwm_bed > 0
@@ -5694,36 +5916,28 @@ void calculate_delta(float cartesian[3]){
 #endif
 
 #ifdef TEMP_STAT_LEDS
-static bool blue_led = false;
-static bool red_led = false;
-static uint32_t stat_update = 0;
-
-void handle_status_leds(void) {
-  float max_temp = 0.0;
-  if(millis() > stat_update) {
-    stat_update += 500; // Update every 0.5s
-    for (int8_t cur_extruder = 0; cur_extruder < EXTRUDERS; ++cur_extruder) {
-       max_temp = max(max_temp, degHotend(cur_extruder));
-       max_temp = max(max_temp, degTargetHotend(cur_extruder));
-    }
-    #if HAS_TEMP_BED
-      max_temp = max(max_temp, degTargetBed());
-      max_temp = max(max_temp, degBed());
-    #endif
-    if((max_temp > 55.0) && (red_led == false)) {
-      digitalWrite(STAT_LED_RED, 1);
-      digitalWrite(STAT_LED_BLUE, 0);
-      red_led = true;
-      blue_led = false;
-    }
-    if((max_temp < 54.0) && (blue_led == false)) {
-      digitalWrite(STAT_LED_RED, 0);
-      digitalWrite(STAT_LED_BLUE, 1);
-      red_led = false;
-      blue_led = true;
+
+  static bool red_led = false;
+  static millis_t next_status_led_update_ms = 0;
+
+  void handle_status_leds(void) {
+    float max_temp = 0.0;
+    if (millis() > next_status_led_update_ms) {
+      next_status_led_update_ms += 500; // Update every 0.5s
+      for (int8_t cur_extruder = 0; cur_extruder < EXTRUDERS; ++cur_extruder)
+         max_temp = max(max(max_temp, degHotend(cur_extruder)), degTargetHotend(cur_extruder));
+      #if HAS_TEMP_BED
+        max_temp = max(max(max_temp, degTargetBed()), degBed());
+      #endif
+      bool new_led = (max_temp > 55.0) ? true : (max_temp < 54.0) ? false : red_led;
+      if (new_led != red_led) {
+        red_led = new_led;
+        digitalWrite(STAT_LED_RED, new_led ? HIGH : LOW);
+        digitalWrite(STAT_LED_BLUE, new_led ? LOW : HIGH);
+      }
     }
   }
-}
+
 #endif
 
 void enable_all_steppers() {
@@ -5765,13 +5979,13 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) {
       filrunout();
   #endif
 
-  if (buflen < BUFSIZE - 1) get_command();
+  if (commands_in_queue < BUFSIZE - 1) get_command();
 
-  unsigned long ms = millis();
+  millis_t ms = millis();
 
-  if (max_inactive_time && ms > previous_millis_cmd + max_inactive_time) kill();
+  if (max_inactive_time && ms > previous_cmd_ms + max_inactive_time) kill();
 
-  if (stepper_inactive_time && ms > previous_millis_cmd + stepper_inactive_time
+  if (stepper_inactive_time && ms > previous_cmd_ms + stepper_inactive_time
       && !ignore_stepper_queue && !blocks_queued())
     disable_all_steppers();
 
@@ -5807,7 +6021,7 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) {
     const int HOME_DEBOUNCE_DELAY = 750;
     if (!READ(HOME_PIN)) {
       if (!homeDebounceCount) {
-        enquecommands_P(PSTR("G28"));
+        enqueuecommands_P(PSTR("G28"));
         LCD_ALERTMESSAGEPGM(MSG_AUTO_HOME);
       }
       if (homeDebounceCount < HOME_DEBOUNCE_DELAY)
@@ -5822,7 +6036,7 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) {
   #endif
 
   #ifdef EXTRUDER_RUNOUT_PREVENT
-    if (ms > previous_millis_cmd + EXTRUDER_RUNOUT_SECONDS * 1000)
+    if (ms > previous_cmd_ms + EXTRUDER_RUNOUT_SECONDS * 1000)
     if (degHotend(active_extruder) > EXTRUDER_RUNOUT_MINTEMP) {
       bool oldstatus;
       switch(active_extruder) {
@@ -5856,7 +6070,7 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) {
       current_position[E_AXIS] = oldepos;
       destination[E_AXIS] = oldedes;
       plan_set_e_position(oldepos);
-      previous_millis_cmd = ms; // refresh_cmd_timeout()
+      previous_cmd_ms = ms; // refresh_cmd_timeout()
       st_synchronize();
       switch(active_extruder) {
         case 0:
@@ -5901,7 +6115,7 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) {
 void kill()
 {
   cli(); // Stop interrupts
-  disable_heater();
+  disable_all_heaters();
 
   disable_all_steppers();
 
@@ -5922,18 +6136,18 @@ void kill()
 }
 
 #ifdef FILAMENT_RUNOUT_SENSOR
-   void filrunout()
-   {
-      if filrunoutEnqued == false {
-         filrunoutEnqued = true;
-         enquecommand("M600");
-      }
-   }
+
+  void filrunout() {
+    if (!filrunoutEnqueued) {
+      filrunoutEnqueued = true;
+      enqueuecommand("M600");
+    }
+  }
+
 #endif
 
-void Stop()
-{
-  disable_heater();
+void Stop() {
+  disable_all_heaters();
   if (IsRunning()) {
     Running = false;
     Stopped_gcode_LastN = gcode_LastN; // Save last g_code for restart
diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h
index d1be7703d9100e1138add4bbd14839dd57260def..3bba4584cde745833719908be6cd68dccff6f756 100644
--- a/Marlin/SanityCheck.h
+++ b/Marlin/SanityCheck.h
@@ -87,8 +87,8 @@
   /**
    * Required LCD language
    */
-  #if !defined(DOGLCD) && defined(ULTRA_LCD) && !defined(DISPLAY_CHARSET_HD44780_JAPAN) && !defined(DISPLAY_CHARSET_HD44780_WESTERN)
-    #error You must enable either DISPLAY_CHARSET_HD44780_JAPAN or DISPLAY_CHARSET_HD44780_WESTERN for your LCD controller.
+  #if !defined(DOGLCD) && defined(ULTRA_LCD) && !defined(DISPLAY_CHARSET_HD44780_JAPAN) && !defined(DISPLAY_CHARSET_HD44780_WESTERN)&& !defined(DISPLAY_CHARSET_HD44780_CYRILLIC)
+    #error You must enable either DISPLAY_CHARSET_HD44780_JAPAN or DISPLAY_CHARSET_HD44780_WESTERN  or DISPLAY_CHARSET_HD44780_CYRILLIC for your LCD controller.
   #endif
 
   /**
@@ -101,6 +101,9 @@
     #ifdef ENABLE_AUTO_BED_LEVELING
       #error Select ENABLE_AUTO_BED_LEVELING or MESH_BED_LEVELING, not both
     #endif
+    #if MESH_NUM_X_POINTS > 7 || MESH_NUM_Y_POINTS > 7
+      #error MESH_NUM_X_POINTS and MESH_NUM_Y_POINTS need to be less than 8
+    #endif
   #endif
 
   /**
@@ -183,7 +186,7 @@
         #define Y_PROBE_ERROR
       #endif
       #ifdef Y_PROBE_ERROR
-        #error The Y axis probing range is to small to fit all the points defined in AUTO_BED_LEVELING_GRID_POINTS
+        #error The Y axis probing range is too small to fit all the points defined in AUTO_BED_LEVELING_GRID_POINTS
       #endif
 
       #undef PROBE_SIZE_X
diff --git a/Marlin/Servo.cpp b/Marlin/Servo.cpp
index 27a7d3cf3e8b64b80a71fcc98f8acdb360aa1188..576b394fb70a0c2652d8ec74d9f7e89627e510cf 100644
--- a/Marlin/Servo.cpp
+++ b/Marlin/Servo.cpp
@@ -1,5 +1,5 @@
 /*
- Servo.cpp - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
+ servo.cpp - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
  Copyright (c) 2009 Michael Margolis.  All right reserved.
 
  This library is free software; you can redistribute it and/or
diff --git a/Marlin/Servo.h b/Marlin/Servo.h
index 682a3b37981b37301261db7fef2f85d271ae4839..4647561754898d4770f2e56743dec924702095a4 100644
--- a/Marlin/Servo.h
+++ b/Marlin/Servo.h
@@ -1,5 +1,5 @@
 /*
-  Servo.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
+  servo.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
   Copyright (c) 2009 Michael Margolis.  All right reserved.
 
   This library is free software; you can redistribute it and/or
@@ -42,8 +42,8 @@
    detach()    - Stops an attached servos from pulsing its i/o pin.
  */
 
-#ifndef Servo_h
-#define Servo_h
+#ifndef servo_h
+#define servo_h
 
 #include <inttypes.h>
 
diff --git a/Marlin/BlinkM.cpp b/Marlin/blinkm.cpp
similarity index 83%
rename from Marlin/BlinkM.cpp
rename to Marlin/blinkm.cpp
index b340f968c09c8bea0c1a77d9ea50a107f50094f6..1ec5b656ccb4fed89726710382854347b967a58e 100644
--- a/Marlin/BlinkM.cpp
+++ b/Marlin/blinkm.cpp
@@ -1,11 +1,11 @@
 /*
-  BlinkM.cpp - Library for controlling a BlinkM over i2c
+  blinkm.cpp - Library for controlling a BlinkM over i2c
   Created by Tim Koster, August 21 2013.
 */
 #include "Marlin.h"
 #ifdef BLINKM
 
-#include "BlinkM.h"
+#include "blinkm.h"
 
 void SendColors(byte red, byte grn, byte blu) {
   Wire.begin(); 
diff --git a/Marlin/BlinkM.h b/Marlin/blinkm.h
similarity index 94%
rename from Marlin/BlinkM.h
rename to Marlin/blinkm.h
index 5b802b718b842c35ceab8a8b66b066b77968ebb1..08169e589c97cdb1e3b8574920a24df88459a6a6 100644
--- a/Marlin/BlinkM.h
+++ b/Marlin/blinkm.h
@@ -1,5 +1,5 @@
 /*
-  BlinkM.h
+  blinkm.h
   Library header file for BlinkM library
  */
 #if ARDUINO >= 100
diff --git a/Marlin/cardreader.cpp b/Marlin/cardreader.cpp
index 877b72b9277af3d3c096683a3fd90fa1051ab36c..639b4f244aaac39de89b019355ea3139af6b5ae0 100644
--- a/Marlin/cardreader.cpp
+++ b/Marlin/cardreader.cpp
@@ -25,7 +25,7 @@ CardReader::CardReader() {
     OUT_WRITE(SDPOWER, HIGH);
   #endif //SDPOWER
 
-  autostart_atmillis = millis() + 5000;
+  next_autostart_ms = millis() + 5000;
 }
 
 char *createFilename(char *buffer, const dir_t &p) { //buffer > 12characters
@@ -397,7 +397,7 @@ void CardReader::write_command(char *buf) {
 }
 
 void CardReader::checkautostart(bool force) {
-  if (!force && (!autostart_stilltocheck || autostart_atmillis < millis()))
+  if (!force && (!autostart_stilltocheck || next_autostart_ms < millis()))
     return;
 
   autostart_stilltocheck = false;
@@ -421,8 +421,8 @@ void CardReader::checkautostart(bool force) {
     if (p.name[9] != '~' && strncmp((char*)p.name, autoname, 5) == 0) {
       char cmd[30];
       sprintf_P(cmd, PSTR("M23 %s"), autoname);
-      enquecommand(cmd);
-      enquecommands_P(PSTR("M24"));
+      enqueuecommand(cmd);
+      enqueuecommands_P(PSTR("M24"));
       found = true;
     }
   }
@@ -508,7 +508,7 @@ void CardReader::printingHasFinished() {
     sdprinting = false;
     if (SD_FINISHED_STEPPERRELEASE) {
       //finishAndDisableSteppers();
-      enquecommands_P(PSTR(SD_FINISHED_RELEASECOMMAND));
+      enqueuecommands_P(PSTR(SD_FINISHED_RELEASECOMMAND));
     }
     autotempShutdown();
   }
diff --git a/Marlin/cardreader.h b/Marlin/cardreader.h
index b55d09a65a17f8d9e3a10abe2ce149ae957e6524..03d4303e30cf71af013b99afc6b808195c789b5a 100644
--- a/Marlin/cardreader.h
+++ b/Marlin/cardreader.h
@@ -62,7 +62,7 @@ private:
   uint32_t filespos[SD_PROCEDURE_DEPTH];
   char filenames[SD_PROCEDURE_DEPTH][MAXPATHNAMELENGTH];
   uint32_t filesize;
-  unsigned long autostart_atmillis;
+  millis_t next_autostart_ms;
   uint32_t sdpos;
 
   bool autostart_stilltocheck; //the sd start is delayed, because otherwise the serial cannot answer fast enought to make contact with the hostsoftware.
diff --git a/Marlin/ConfigurationStore.cpp b/Marlin/configuration_store.cpp
similarity index 73%
rename from Marlin/ConfigurationStore.cpp
rename to Marlin/configuration_store.cpp
index 56de81684854ec666a5083bea99981507c439b2c..782b5d320a4847723fba6615e4e5696f38bd3565 100644
--- a/Marlin/ConfigurationStore.cpp
+++ b/Marlin/configuration_store.cpp
@@ -1,5 +1,5 @@
 /**
- * ConfigurationStore.cpp
+ * configuration_store.cpp
  *
  * Configuration and EEPROM storage
  *
@@ -20,72 +20,72 @@
  * V19 EEPROM Layout:
  *
  *  ver
- *  axis_steps_per_unit (x4)
- *  max_feedrate (x4)
- *  max_acceleration_units_per_sq_second (x4)
- *  acceleration
- *  retract_acceleration
- *  travel_acceleration
- *  minimumfeedrate
- *  mintravelfeedrate
- *  minsegmenttime
- *  max_xy_jerk
- *  max_z_jerk
- *  max_e_jerk
- *  home_offset (x3)
+ *  M92 XYZE  axis_steps_per_unit (x4)
+ *  M203 XYZE max_feedrate (x4)
+ *  M201 XYZE max_acceleration_units_per_sq_second (x4)
+ *  M204 P    acceleration
+ *  M204 R    retract_acceleration
+ *  M204 T    travel_acceleration
+ *  M205 S    minimumfeedrate
+ *  M205 T    mintravelfeedrate
+ *  M205 B    minsegmenttime
+ *  M205 X    max_xy_jerk
+ *  M205 Z    max_z_jerk
+ *  M205 E    max_e_jerk
+ *  M206 XYZ  home_offset (x3)
  *
  * Mesh bed leveling:
- *  active
- *  mesh_num_x
- *  mesh_num_y
- *  z_values[][]
- *  zprobe_zoffset
+ *  M420 S    active
+ *            mesh_num_x (set in firmware)
+ *            mesh_num_y (set in firmware)
+ *  M421 XYZ  z_values[][]
+ *  M851      zprobe_zoffset
  *
  * DELTA:
- *  endstop_adj (x3)
- *  delta_radius
- *  delta_diagonal_rod
- *  delta_segments_per_second
+ *  M666 XYZ  endstop_adj (x3)
+ *  M665 R    delta_radius
+ *  M665 L    delta_diagonal_rod
+ *  M665 S    delta_segments_per_second
  *
  * ULTIPANEL:
- *  plaPreheatHotendTemp
- *  plaPreheatHPBTemp
- *  plaPreheatFanSpeed
- *  absPreheatHotendTemp
- *  absPreheatHPBTemp
- *  absPreheatFanSpeed
+ *  M145 S0 H plaPreheatHotendTemp
+ *  M145 S0 B plaPreheatHPBTemp
+ *  M145 S0 F plaPreheatFanSpeed
+ *  M145 S1 H absPreheatHotendTemp
+ *  M145 S1 B absPreheatHPBTemp
+ *  M145 S1 F absPreheatFanSpeed
  *
  * PIDTEMP:
- *  Kp[0], Ki[0], Kd[0], Kc[0]
- *  Kp[1], Ki[1], Kd[1], Kc[1]
- *  Kp[2], Ki[2], Kd[2], Kc[2]
- *  Kp[3], Ki[3], Kd[3], Kc[3]
+ *  M301 E0 PIDC  Kp[0], Ki[0], Kd[0], Kc[0]
+ *  M301 E1 PIDC  Kp[1], Ki[1], Kd[1], Kc[1]
+ *  M301 E2 PIDC  Kp[2], Ki[2], Kd[2], Kc[2]
+ *  M301 E3 PIDC  Kp[3], Ki[3], Kd[3], Kc[3]
  *
  * PIDTEMPBED:
- *  bedKp, bedKi, bedKd
+ *  M304 PID  bedKp, bedKi, bedKd
  *
  * DOGLCD:
- *  lcd_contrast
+ *  M250 C    lcd_contrast
  *
  * SCARA:
- *  axis_scaling (x3)
+ *  M365 XYZ  axis_scaling (x3)
  *
  * FWRETRACT:
- *  autoretract_enabled
- *  retract_length
- *  retract_length_swap
- *  retract_feedrate
- *  retract_zlift
- *  retract_recover_length
- *  retract_recover_length_swap
- *  retract_recover_feedrate
+ *  M209 S    autoretract_enabled
+ *  M207 S    retract_length
+ *  M207 W    retract_length_swap
+ *  M207 F    retract_feedrate
+ *  M207 Z    retract_zlift
+ *  M208 S    retract_recover_length
+ *  M208 W    retract_recover_length_swap
+ *  M208 F    retract_recover_feedrate
  *
- *  volumetric_enabled
+ *  M200 D    volumetric_enabled (D>0 makes this enabled)
  *
- *  filament_size (x4)
+ *  M200 T D  filament_size (x4) (T0..3)
  *
- * Z_DUAL_ENDSTOPS
- *  z_endstop_adj
+ * Z_DUAL_ENDSTOPS:
+ *  M666 Z    z_endstop_adj
  *
  */
 #include "Marlin.h"
@@ -93,11 +93,11 @@
 #include "planner.h"
 #include "temperature.h"
 #include "ultralcd.h"
-#include "ConfigurationStore.h"
+#include "configuration_store.h"
 
 #ifdef MESH_BED_LEVELING
-   #include "mesh_bed_leveling.h"
-#endif  // MESH_BED_LEVELING
+  #include "mesh_bed_leveling.h"
+#endif
 
 void _EEPROM_writeData(int &pos, uint8_t* value, uint8_t size) {
   uint8_t c;
@@ -122,7 +122,9 @@ void _EEPROM_readData(int &pos, uint8_t* value, uint8_t size) {
 #define EEPROM_WRITE_VAR(pos, value) _EEPROM_writeData(pos, (uint8_t*)&value, sizeof(value))
 #define EEPROM_READ_VAR(pos, value) _EEPROM_readData(pos, (uint8_t*)&value, sizeof(value))
 
-//======================================================================================
+/**
+ * Store Configuration Settings - M500
+ */
 
 #define DUMMY_PID_VALUE 3000.0f
 
@@ -166,9 +168,7 @@ void Config_StoreSettings()  {
     EEPROM_WRITE_VAR(i, mesh_num_x);
     EEPROM_WRITE_VAR(i, mesh_num_y);
     dummy = 0.0f;
-    for (int q=0; q<mesh_num_x*mesh_num_y; q++) {
-      EEPROM_WRITE_VAR(i, dummy);
-    }
+    for (int q=0; q<mesh_num_x*mesh_num_y; q++) EEPROM_WRITE_VAR(i, dummy);
   #endif // MESH_BED_LEVELING
 
   #ifndef ENABLE_AUTO_BED_LEVELING
@@ -235,7 +235,7 @@ void Config_StoreSettings()  {
   EEPROM_WRITE_VAR(i, bedKi);
   EEPROM_WRITE_VAR(i, bedKd);
 
-  #ifndef DOGLCD
+  #ifndef HAS_LCD_CONTRAST
     int lcd_contrast = 32;
   #endif
   EEPROM_WRITE_VAR(i, lcd_contrast);
@@ -286,6 +286,10 @@ void Config_StoreSettings()  {
   SERIAL_ECHOLNPGM(" bytes)");
 }
 
+/**
+ * Retrieve Configuration Settings - M501
+ */
+
 void Config_RetrieveSettings() {
 
   int i = EEPROM_OFFSET;
@@ -319,30 +323,21 @@ void Config_RetrieveSettings() {
     EEPROM_READ_VAR(i, max_e_jerk);
     EEPROM_READ_VAR(i, home_offset);
 
-    uint8_t mesh_num_x = 0;
-    uint8_t mesh_num_y = 0;
+    uint8_t dummy_uint8 = 0, mesh_num_x = 0, mesh_num_y = 0;
+    EEPROM_READ_VAR(i, dummy_uint8);
+    EEPROM_READ_VAR(i, mesh_num_x);
+    EEPROM_READ_VAR(i, mesh_num_y);
     #ifdef MESH_BED_LEVELING
-      EEPROM_READ_VAR(i, mbl.active);
-      EEPROM_READ_VAR(i, mesh_num_x);
-      EEPROM_READ_VAR(i, mesh_num_y);
-      if (mesh_num_x != MESH_NUM_X_POINTS ||
-          mesh_num_y != MESH_NUM_Y_POINTS) {
-        mbl.reset();
-        for (int q=0; q<mesh_num_x*mesh_num_y; q++) {
-          EEPROM_READ_VAR(i, dummy);
-        }
-      } else {
+      mbl.active = dummy_uint8;
+      if (mesh_num_x == MESH_NUM_X_POINTS && mesh_num_y == MESH_NUM_Y_POINTS) {
         EEPROM_READ_VAR(i, mbl.z_values);
+      } else {
+        mbl.reset();
+        for (int q = 0; q < mesh_num_x * mesh_num_y; q++) EEPROM_READ_VAR(i, dummy);
       }
     #else
-      uint8_t dummy_uint8 = 0;
-      EEPROM_READ_VAR(i, dummy_uint8);
-      EEPROM_READ_VAR(i, mesh_num_x);
-      EEPROM_READ_VAR(i, mesh_num_y);
-      for (int q=0; q<mesh_num_x*mesh_num_y; q++) {
-        EEPROM_READ_VAR(i, dummy);
-      }
-    #endif  // MESH_BED_LEVELING
+      for (int q = 0; q < mesh_num_x * mesh_num_y; q++) EEPROM_READ_VAR(i, dummy);
+    #endif // MESH_BED_LEVELING
 
     #ifndef ENABLE_AUTO_BED_LEVELING
       float zprobe_zoffset = 0;
@@ -412,7 +407,7 @@ void Config_RetrieveSettings() {
       for (int q=2; q--;) EEPROM_READ_VAR(i, dummy); // bedKi, bedKd
     }
 
-    #ifndef DOGLCD
+    #ifndef HAS_LCD_CONTRAST
       int lcd_contrast;
     #endif
     EEPROM_READ_VAR(i, lcd_contrast);
@@ -467,6 +462,10 @@ void Config_RetrieveSettings() {
 
 #endif // EEPROM_SETTINGS
 
+/**
+ * Reset Configuration Settings - M502
+ */
+
 void Config_ResetDefault() {
   float tmp1[] = DEFAULT_AXIS_STEPS_PER_UNIT;
   float tmp2[] = DEFAULT_MAX_FEEDRATE;
@@ -522,7 +521,7 @@ void Config_ResetDefault() {
     absPreheatFanSpeed = ABS_PREHEAT_FAN_SPEED;
   #endif
 
-  #ifdef DOGLCD
+  #ifdef HAS_LCD_CONTRAST
     lcd_contrast = DEFAULT_LCD_CONTRAST;
   #endif
 
@@ -584,14 +583,20 @@ void Config_ResetDefault() {
 
 #ifndef DISABLE_M503
 
+/**
+ * Print Configuration Settings - M503
+ */
+
+#define CONFIG_ECHO_START do{ if (!forReplay) SERIAL_ECHO_START; }while(0)
+
 void Config_PrintSettings(bool forReplay) {
   // Always have this function, even with EEPROM_SETTINGS disabled, the current values will be shown
 
-  SERIAL_ECHO_START;
+  CONFIG_ECHO_START;
 
   if (!forReplay) {
     SERIAL_ECHOLNPGM("Steps per unit:");
-    SERIAL_ECHO_START;
+    CONFIG_ECHO_START;
   }
   SERIAL_ECHOPAIR("  M92 X", axis_steps_per_unit[X_AXIS]);
   SERIAL_ECHOPAIR(" Y", axis_steps_per_unit[Y_AXIS]);
@@ -599,23 +604,23 @@ void Config_PrintSettings(bool forReplay) {
   SERIAL_ECHOPAIR(" E", axis_steps_per_unit[E_AXIS]);
   SERIAL_EOL;
 
-  SERIAL_ECHO_START;
+  CONFIG_ECHO_START;
 
   #ifdef SCARA
     if (!forReplay) {
       SERIAL_ECHOLNPGM("Scaling factors:");
-      SERIAL_ECHO_START;
+      CONFIG_ECHO_START;
     }
     SERIAL_ECHOPAIR("  M365 X", axis_scaling[X_AXIS]);
     SERIAL_ECHOPAIR(" Y", axis_scaling[Y_AXIS]);
     SERIAL_ECHOPAIR(" Z", axis_scaling[Z_AXIS]);
     SERIAL_EOL;
-    SERIAL_ECHO_START;
+    CONFIG_ECHO_START;
   #endif // SCARA
 
   if (!forReplay) {
     SERIAL_ECHOLNPGM("Maximum feedrates (mm/s):");
-    SERIAL_ECHO_START;
+    CONFIG_ECHO_START;
   }
   SERIAL_ECHOPAIR("  M203 X", max_feedrate[X_AXIS]);
   SERIAL_ECHOPAIR(" Y", max_feedrate[Y_AXIS]);
@@ -623,160 +628,224 @@ void Config_PrintSettings(bool forReplay) {
   SERIAL_ECHOPAIR(" E", max_feedrate[E_AXIS]);
   SERIAL_EOL;
 
-  SERIAL_ECHO_START;
+  CONFIG_ECHO_START;
   if (!forReplay) {
     SERIAL_ECHOLNPGM("Maximum Acceleration (mm/s2):");
-    SERIAL_ECHO_START;
+    CONFIG_ECHO_START;
   }
-  SERIAL_ECHOPAIR("  M201 X", max_acceleration_units_per_sq_second[X_AXIS] );
-  SERIAL_ECHOPAIR(" Y", max_acceleration_units_per_sq_second[Y_AXIS] );
-  SERIAL_ECHOPAIR(" Z", max_acceleration_units_per_sq_second[Z_AXIS] );
+  SERIAL_ECHOPAIR("  M201 X", max_acceleration_units_per_sq_second[X_AXIS]);
+  SERIAL_ECHOPAIR(" Y", max_acceleration_units_per_sq_second[Y_AXIS]);
+  SERIAL_ECHOPAIR(" Z", max_acceleration_units_per_sq_second[Z_AXIS]);
   SERIAL_ECHOPAIR(" E", max_acceleration_units_per_sq_second[E_AXIS]);
   SERIAL_EOL;
-  SERIAL_ECHO_START;
+  CONFIG_ECHO_START;
   if (!forReplay) {
     SERIAL_ECHOLNPGM("Accelerations: P=printing, R=retract and T=travel");
-    SERIAL_ECHO_START;
+    CONFIG_ECHO_START;
   }
-  SERIAL_ECHOPAIR("  M204 P", acceleration );
+  SERIAL_ECHOPAIR("  M204 P", acceleration);
   SERIAL_ECHOPAIR(" R", retract_acceleration);
   SERIAL_ECHOPAIR(" T", travel_acceleration);
   SERIAL_EOL;
 
-  SERIAL_ECHO_START;
+  CONFIG_ECHO_START;
   if (!forReplay) {
     SERIAL_ECHOLNPGM("Advanced variables: S=Min feedrate (mm/s), T=Min travel feedrate (mm/s), B=minimum segment time (ms), X=maximum XY jerk (mm/s),  Z=maximum Z jerk (mm/s),  E=maximum E jerk (mm/s)");
-    SERIAL_ECHO_START;
+    CONFIG_ECHO_START;
   }
-  SERIAL_ECHOPAIR("  M205 S", minimumfeedrate );
-  SERIAL_ECHOPAIR(" T", mintravelfeedrate );
-  SERIAL_ECHOPAIR(" B", minsegmenttime );
-  SERIAL_ECHOPAIR(" X", max_xy_jerk );
+  SERIAL_ECHOPAIR("  M205 S", minimumfeedrate);
+  SERIAL_ECHOPAIR(" T", mintravelfeedrate);
+  SERIAL_ECHOPAIR(" B", minsegmenttime);
+  SERIAL_ECHOPAIR(" X", max_xy_jerk);
   SERIAL_ECHOPAIR(" Z", max_z_jerk);
   SERIAL_ECHOPAIR(" E", max_e_jerk);
   SERIAL_EOL;
 
-  SERIAL_ECHO_START;
+  CONFIG_ECHO_START;
   if (!forReplay) {
     SERIAL_ECHOLNPGM("Home offset (mm):");
-    SERIAL_ECHO_START;
+    CONFIG_ECHO_START;
   }
-  SERIAL_ECHOPAIR("  M206 X", home_offset[X_AXIS] );
-  SERIAL_ECHOPAIR(" Y", home_offset[Y_AXIS] );
-  SERIAL_ECHOPAIR(" Z", home_offset[Z_AXIS] );
+  SERIAL_ECHOPAIR("  M206 X", home_offset[X_AXIS]);
+  SERIAL_ECHOPAIR(" Y", home_offset[Y_AXIS]);
+  SERIAL_ECHOPAIR(" Z", home_offset[Z_AXIS]);
   SERIAL_EOL;
 
+  #ifdef MESH_BED_LEVELING
+    if (!forReplay) {
+      SERIAL_ECHOLNPGM("Mesh bed leveling:");
+      CONFIG_ECHO_START;
+    }
+    SERIAL_ECHOPAIR("  M420 S", (int32_t)mbl.active);
+    SERIAL_ECHOPAIR(" X", MESH_NUM_X_POINTS);
+    SERIAL_ECHOPAIR(" Y", MESH_NUM_Y_POINTS);
+    SERIAL_EOL;
+    for (int y=0; y<MESH_NUM_Y_POINTS; y++) {
+      for (int x=0; x<MESH_NUM_X_POINTS; x++) {
+        CONFIG_ECHO_START;
+        SERIAL_ECHOPAIR("  M421 X", mbl.get_x(x));
+        SERIAL_ECHOPAIR(" Y", mbl.get_y(y));
+        SERIAL_ECHOPAIR(" Z", mbl.z_values[y][x]);
+        SERIAL_EOL;
+      }
+    }
+  #endif
+
   #ifdef DELTA
-    SERIAL_ECHO_START;
+    CONFIG_ECHO_START;
     if (!forReplay) {
-      SERIAL_ECHOLNPGM("Endstop adjustement (mm):");
-      SERIAL_ECHO_START;
+      SERIAL_ECHOLNPGM("Endstop adjustment (mm):");
+      CONFIG_ECHO_START;
     }
-    SERIAL_ECHOPAIR("  M666 X", endstop_adj[X_AXIS] );
-    SERIAL_ECHOPAIR(" Y", endstop_adj[Y_AXIS] );
-    SERIAL_ECHOPAIR(" Z", endstop_adj[Z_AXIS] );
+    SERIAL_ECHOPAIR("  M666 X", endstop_adj[X_AXIS]);
+    SERIAL_ECHOPAIR(" Y", endstop_adj[Y_AXIS]);
+    SERIAL_ECHOPAIR(" Z", endstop_adj[Z_AXIS]);
     SERIAL_EOL;
-    SERIAL_ECHO_START;
+    CONFIG_ECHO_START;
     SERIAL_ECHOLNPGM("Delta settings: L=delta_diagonal_rod, R=delta_radius, S=delta_segments_per_second");
-    SERIAL_ECHO_START;
-    SERIAL_ECHOPAIR("  M665 L", delta_diagonal_rod );
-    SERIAL_ECHOPAIR(" R", delta_radius );
-    SERIAL_ECHOPAIR(" S", delta_segments_per_second );
+    CONFIG_ECHO_START;
+    SERIAL_ECHOPAIR("  M665 L", delta_diagonal_rod);
+    SERIAL_ECHOPAIR(" R", delta_radius);
+    SERIAL_ECHOPAIR(" S", delta_segments_per_second);
     SERIAL_EOL;
   #elif defined(Z_DUAL_ENDSTOPS)
-    SERIAL_ECHO_START;
+    CONFIG_ECHO_START;
     if (!forReplay) {
-      SERIAL_ECHOLNPGM("Z2 Endstop adjustement (mm):");
-      SERIAL_ECHO_START;
+      SERIAL_ECHOLNPGM("Z2 Endstop adjustment (mm):");
+      CONFIG_ECHO_START;
     }
-    SERIAL_ECHOPAIR("  M666 Z", z_endstop_adj );
+    SERIAL_ECHOPAIR("  M666 Z", z_endstop_adj);
     SERIAL_EOL;  
   #endif // DELTA
 
+  #ifdef ULTIPANEL
+    CONFIG_ECHO_START;
+    if (!forReplay) {
+      SERIAL_ECHOLNPGM("Material heatup parameters:");
+      CONFIG_ECHO_START;
+    }
+    SERIAL_ECHOPAIR("  M145 M0 H", (unsigned long)plaPreheatHotendTemp);
+    SERIAL_ECHOPAIR(" B", (unsigned long)plaPreheatHPBTemp);
+    SERIAL_ECHOPAIR(" F", (unsigned long)plaPreheatFanSpeed);
+    SERIAL_EOL;
+    CONFIG_ECHO_START;
+    SERIAL_ECHOPAIR("  M145 M1 H", (unsigned long)absPreheatHotendTemp);
+    SERIAL_ECHOPAIR(" B", (unsigned long)absPreheatHPBTemp);
+    SERIAL_ECHOPAIR(" F", (unsigned long)absPreheatFanSpeed);
+    SERIAL_EOL;
+  #endif // ULTIPANEL
+
   #if defined(PIDTEMP) || defined(PIDTEMPBED)
-    SERIAL_ECHO_START;
+
+    CONFIG_ECHO_START;
     if (!forReplay) {
       SERIAL_ECHOLNPGM("PID settings:");
-      SERIAL_ECHO_START;
     }
-    #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
+      #if EXTRUDERS > 1
+        if (forReplay) {
+          for (uint8_t i = 0; i < EXTRUDERS; i++) {
+            CONFIG_ECHO_START;
+            SERIAL_ECHOPAIR("  M301 E", (unsigned long)i);
+            SERIAL_ECHOPAIR(" P", PID_PARAM(Kp, i));
+            SERIAL_ECHOPAIR(" I", unscalePID_i(PID_PARAM(Ki, i)));
+            SERIAL_ECHOPAIR(" D", unscalePID_d(PID_PARAM(Kd, i)));
+            #ifdef PID_ADD_EXTRUSION_RATE
+              SERIAL_ECHOPAIR(" C", PID_PARAM(Kc, i));
+            #endif      
+            SERIAL_EOL;
+          }
+        }
+        else
+      #endif // EXTRUDERS > 1
+      // !forReplay || EXTRUDERS == 1
+      {
+        CONFIG_ECHO_START;
+        SERIAL_ECHOPAIR("  M301 P", PID_PARAM(Kp, 0)); // for compatibility with hosts, only echo values for E0
+        SERIAL_ECHOPAIR(" I", unscalePID_i(PID_PARAM(Ki, 0)));
+        SERIAL_ECHOPAIR(" D", unscalePID_d(PID_PARAM(Kd, 0)));
+        #ifdef PID_ADD_EXTRUSION_RATE
+          SERIAL_ECHOPAIR(" C", PID_PARAM(Kc, 0));
+        #endif      
+        SERIAL_EOL;
+      }
+    #endif // PIDTEMP
+
     #ifdef PIDTEMPBED
-      SERIAL_ECHOPAIR("  M304 P", bedKp); // for compatibility with hosts, only echos values for E0
+      CONFIG_ECHO_START;
+      SERIAL_ECHOPAIR("  M304 P", bedKp);
       SERIAL_ECHOPAIR(" I", unscalePID_i(bedKi));
       SERIAL_ECHOPAIR(" D", unscalePID_d(bedKd));
       SERIAL_EOL;
     #endif
+
+  #endif // PIDTEMP || PIDTEMPBED
+
+  #ifdef HAS_LCD_CONTRAST
+    CONFIG_ECHO_START;
+    if (!forReplay) {
+      SERIAL_ECHOLNPGM("LCD Contrast:");
+      CONFIG_ECHO_START;
+    }
+    SERIAL_ECHOPAIR("  M250 C", (unsigned long)lcd_contrast);
+    SERIAL_EOL;
   #endif
 
   #ifdef FWRETRACT
 
-    SERIAL_ECHO_START;
+    CONFIG_ECHO_START;
     if (!forReplay) {
       SERIAL_ECHOLNPGM("Retract: S=Length (mm) F:Speed (mm/m) Z: ZLift (mm)");
-      SERIAL_ECHO_START;
+      CONFIG_ECHO_START;
     }
     SERIAL_ECHOPAIR("  M207 S", retract_length);
+    #if EXTRUDERS > 1
+      SERIAL_ECHOPAIR(" W", retract_length_swap);
+    #endif
     SERIAL_ECHOPAIR(" F", retract_feedrate*60);
     SERIAL_ECHOPAIR(" Z", retract_zlift);
     SERIAL_EOL;
-    SERIAL_ECHO_START;
+    CONFIG_ECHO_START;
     if (!forReplay) {
       SERIAL_ECHOLNPGM("Recover: S=Extra length (mm) F:Speed (mm/m)");
-      SERIAL_ECHO_START;
+      CONFIG_ECHO_START;
     }
     SERIAL_ECHOPAIR("  M208 S", retract_recover_length);
+    #if EXTRUDERS > 1
+      SERIAL_ECHOPAIR(" W", retract_recover_length_swap);
+    #endif
     SERIAL_ECHOPAIR(" F", retract_recover_feedrate*60);
     SERIAL_EOL;
-    SERIAL_ECHO_START;
+    CONFIG_ECHO_START;
     if (!forReplay) {
       SERIAL_ECHOLNPGM("Auto-Retract: S=0 to disable, 1 to interpret extrude-only moves as retracts or recoveries");
-      SERIAL_ECHO_START;
+      CONFIG_ECHO_START;
     }
     SERIAL_ECHOPAIR("  M209 S", (unsigned long)(autoretract_enabled ? 1 : 0));
     SERIAL_EOL;
 
-    #if EXTRUDERS > 1
-      if (!forReplay) {
-        SERIAL_ECHO_START;
-        SERIAL_ECHOLNPGM("Multi-extruder settings:");
-        SERIAL_ECHO_START;
-        SERIAL_ECHOPAIR("   Swap retract length (mm):    ", retract_length_swap);
-        SERIAL_EOL;
-        SERIAL_ECHO_START;
-        SERIAL_ECHOPAIR("   Swap rec. addl. length (mm): ", retract_recover_length_swap);
-        SERIAL_EOL;
-      }
-    #endif // EXTRUDERS > 1
-
   #endif // FWRETRACT
 
-  SERIAL_ECHO_START;
   if (volumetric_enabled) {
     if (!forReplay) {
+      CONFIG_ECHO_START;
       SERIAL_ECHOLNPGM("Filament settings:");
-      SERIAL_ECHO_START;
     }
+
+    CONFIG_ECHO_START;
     SERIAL_ECHOPAIR("  M200 D", filament_size[0]);
     SERIAL_EOL;
 
     #if EXTRUDERS > 1
-      SERIAL_ECHO_START;
+      CONFIG_ECHO_START;
       SERIAL_ECHOPAIR("  M200 T1 D", filament_size[1]);
       SERIAL_EOL;
       #if EXTRUDERS > 2
-        SERIAL_ECHO_START;
+        CONFIG_ECHO_START;
         SERIAL_ECHOPAIR("  M200 T2 D", filament_size[2]);
         SERIAL_EOL;
         #if EXTRUDERS > 3
-          SERIAL_ECHO_START;
+          CONFIG_ECHO_START;
           SERIAL_ECHOPAIR("  M200 T3 D", filament_size[3]);
           SERIAL_EOL;
         #endif
@@ -785,21 +854,23 @@ void Config_PrintSettings(bool forReplay) {
 
   } else {
     if (!forReplay) {
+      CONFIG_ECHO_START;
       SERIAL_ECHOLNPGM("Filament settings: Disabled");
     }
   }
 
   #ifdef ENABLE_AUTO_BED_LEVELING
-    SERIAL_ECHO_START;
     #ifdef CUSTOM_M_CODES
       if (!forReplay) {
+        CONFIG_ECHO_START;
         SERIAL_ECHOLNPGM("Z-Probe Offset (mm):");
-        SERIAL_ECHO_START;
       }
+      CONFIG_ECHO_START;
       SERIAL_ECHOPAIR("  M", (unsigned long)CUSTOM_M_CODE_SET_Z_PROBE_OFFSET);
       SERIAL_ECHOPAIR(" Z", -zprobe_zoffset);
     #else
       if (!forReplay) {
+        CONFIG_ECHO_START;
         SERIAL_ECHOPAIR("Z-Probe Offset (mm):", -zprobe_zoffset);
       }
     #endif
diff --git a/Marlin/ConfigurationStore.h b/Marlin/configuration_store.h
similarity index 83%
rename from Marlin/ConfigurationStore.h
rename to Marlin/configuration_store.h
index 3dc4a92cfd29c000073ddf641ac4871a68f68e3c..6f1a8fcbdc02e9a33d6507fa7baf476f686afad5 100644
--- a/Marlin/ConfigurationStore.h
+++ b/Marlin/configuration_store.h
@@ -1,5 +1,5 @@
-#ifndef CONFIGURATIONSTORE_H
-#define CONFIGURATIONSTORE_H
+#ifndef CONFIGURATION_STORE_H
+#define CONFIGURATION_STORE_H
 
 #include "Configuration.h"
 
@@ -19,4 +19,4 @@ void Config_ResetDefault();
   FORCE_INLINE void Config_RetrieveSettings() { Config_ResetDefault(); Config_PrintSettings(); }
 #endif
 
-#endif //CONFIGURATIONSTORE_H
+#endif //CONFIGURATION_STORE_H
diff --git a/Marlin/configurator/config/Configuration.h b/Marlin/configurator/config/Configuration.h
index a3f378e4d0ad777cf4db1fcf26c0120a276fc77c..8273876f7d0e04ee6c2b48371f46efdb29eb60c4 100644
--- a/Marlin/configurator/config/Configuration.h
+++ b/Marlin/configurator/config/Configuration.h
@@ -41,7 +41,6 @@ Here are some standard links for getting your machine calibrated:
 // startup. Implementation of an idea by Prof Braino to inform user that any changes made to this
 // build by the user have been successfully uploaded into firmware.
 #define STRING_VERSION "1.0.3 dev"
-#define STRING_URL "reprap.org"
 #define STRING_VERSION_CONFIG_H __DATE__ " " __TIME__ // build date and time
 #define STRING_CONFIG_H_AUTHOR "(none, default config)" // Who made the changes.
 #define STRING_SPLASH_LINE1 "v" STRING_VERSION // will be shown during bootup in line 1
@@ -68,8 +67,9 @@ Here are some standard links for getting your machine calibrated:
   #define MOTHERBOARD BOARD_RAMPS_13_EFB
 #endif
 
-// Define this to set a custom name for your generic Mendel,
-// #define CUSTOM_MENDEL_NAME "This Mendel"
+// Optional custom name for your RepStrap or other custom machine
+// Displayed in the LCD "Ready" message
+// #define CUSTOM_MACHINE_NAME "3D Printer"
 
 // Define this to set a unique identifier for this printer, (Used by some programs to differentiate between machines)
 // You can use an online service to generate a random UUID. (eg http://www.uuidgenerator.net/version4)
@@ -363,6 +363,7 @@ const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic
 #define E_ENABLE_ON 0 // For all extruders
 
 // Disables axis when it's not being used.
+// WARNING: When motors turn off there is a chance of losing position accuracy!
 #define DISABLE_X false
 #define DISABLE_Y false
 #define DISABLE_Z false
@@ -639,7 +640,8 @@ const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic
 // @section lcd
 
 // Define your display language below. Replace (en) with your language code and uncomment.
-// en, pl, fr, de, es, ru, bg, it, pt, pt-br, fi, an, nl, ca, eu, kana, kana_utf8, test
+
+// en, pl, fr, de, es, ru, bg, it, pt, pt-br, fi, an, nl, ca, eu, kana, kana_utf8, cn, test
 // See also language.h
 #define LANGUAGE_INCLUDE GENERATE_LANGUAGE_INCLUDE(en)
 
@@ -659,9 +661,9 @@ const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic
 //#define ENCODER_STEPS_PER_MENU_ITEM 5 // Set according to ENCODER_PULSES_PER_STEP or your liking
 //#define ULTIMAKERCONTROLLER //as available from the Ultimaker online store.
 //#define ULTIPANEL  //the UltiPanel as on Thingiverse
-//#define LCD_FEEDBACK_FREQUENCY_HZ 1000	// this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
 //#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click
-                                               // 0 to disable buzzer feedback  
+//#define LCD_FEEDBACK_FREQUENCY_HZ 1000         // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
+                                                 // 0 to disable buzzer feedback. Test with M300 S<frequency Hz> P<duration ms>
 
 // PanelOne from T3P3 (via RAMPS 1.4 AUX2/AUX3)
 // http://reprap.org/wiki/PanelOne
diff --git a/Marlin/configurator/config/Configuration_adv.h b/Marlin/configurator/config/Configuration_adv.h
index 6478eb923e90956a3fa862d1c1225ac2324830e5..b79035ab69517969eeb85368d30d7d691f3eed74 100644
--- a/Marlin/configurator/config/Configuration_adv.h
+++ b/Marlin/configurator/config/Configuration_adv.h
@@ -195,6 +195,9 @@
 #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.
 
+// When G28 is called, this option will make Y home before X
+// #define HOME_Y_BEFORE_X
+
 // @section machine
 
 #define AXIS_RELATIVE_MODES {false, false, false, false}
@@ -258,7 +261,6 @@
 #define ENCODER_RATE_MULTIPLIER         // If defined, certain menu edit operations automatically multiply the steps when the encoder is moved quickly
 #define ENCODER_10X_STEPS_PER_SEC 75    // If the encoder steps per sec exceeds this value, multiply steps moved x10 to quickly advance the value
 #define ENCODER_100X_STEPS_PER_SEC 160  // If the encoder steps per sec exceeds this value, multiply steps moved x100 to really quickly advance the value
-//#define ENCODER_RATE_MULTIPLIER_DEBUG  // If defined, output the encoder steps per second value
 
 //#define CHDK 4        //Pin for triggering CHDK to take a picture see how to use it here http://captain-slow.dk/2014/03/09/3d-printing-timelapses/
 #define CHDK_DELAY 50 //How long in ms the pin should stay HIGH before going LOW again
@@ -355,7 +357,7 @@ const unsigned int dropsegments=5; //everything with less than this number of st
 //#define HEATERS_PARALLEL
 
 //===========================================================================
-//=============================Buffers           ============================
+//================================= Buffers =================================
 //===========================================================================
 
 // @section hidden
diff --git a/Marlin/configurator/config/language.h b/Marlin/configurator/config/language.h
index 3fe7afd2edec6162817c7cd480de79b9fcbefa87..d8ceafb1a5670a24f6b111f55845113b59fa7262 100644
--- a/Marlin/configurator/config/language.h
+++ b/Marlin/configurator/config/language.h
@@ -70,12 +70,12 @@
   #endif
 #else
   #ifndef MACHINE_NAME
-    #define MACHINE_NAME "Mendel"
+    #define MACHINE_NAME "3D Printer"
   #endif
 #endif
 
 #ifdef CUSTOM_MENDEL_NAME
-  #warning CUSTOM_MENDEL_NAME deprecated - use CUSTOM_MACHINE_NAME
+  #error CUSTOM_MENDEL_NAME deprecated - use CUSTOM_MACHINE_NAME
   #define CUSTOM_MACHINE_NAME CUSTOM_MENDEL_NAME
 #endif
 
@@ -110,7 +110,7 @@
 
 // Serial Console Messages (do not translate those!)
 
-#define MSG_Enqueing                        "enqueing \""
+#define MSG_Enqueueing                      "enqueueing \""
 #define MSG_POWERUP                         "PowerUp"
 #define MSG_EXTERNAL_RESET                  " External Reset"
 #define MSG_BROWNOUT_RESET                  " Brown out Reset"
@@ -122,7 +122,8 @@
 #define MSG_PLANNER_BUFFER_BYTES            "  PlannerBufferBytes: "
 #define MSG_OK                              "ok"
 #define MSG_FILE_SAVED                      "Done saving file."
-#define MSG_ERR_LINE_NO                     "Line Number is not Last Line Number+1, Last Line: "
+#define MSG_ERR_LINE_NO1                    "Line Number out of sequence. Expected: "
+#define MSG_ERR_LINE_NO2                    " Got: "
 #define MSG_ERR_CHECKSUM_MISMATCH           "checksum mismatch, Last Line: "
 #define MSG_ERR_NO_CHECKSUM                 "No Checksum with line number, Last Line: "
 #define MSG_ERR_NO_LINENUMBER_WITH_CHECKSUM "No Line Number with checksum, Last Line: "
@@ -157,6 +158,9 @@
 #define MSG_Z_MAX                           "z_max: "
 #define MSG_Z2_MAX                          "z2_max: "
 #define MSG_Z_PROBE                         "z_probe: "
+#define MSG_ERR_MATERIAL_INDEX              "M145 S<index> out of range (0-1)"
+#define MSG_ERR_M421_REQUIRES_XYZ           "M421 requires XYZ parameters"
+#define MSG_ERR_MESH_INDEX_OOB              "Mesh XY index is out of bounds"
 #define MSG_M119_REPORT                     "Reporting endstop status"
 #define MSG_ENDSTOP_HIT                     "TRIGGERED"
 #define MSG_ENDSTOP_OPEN                    "open"
@@ -209,7 +213,7 @@
 #define MSG_OK_B                            "ok B:"
 #define MSG_OK_T                            "ok T:"
 #define MSG_AT                              " @:"
-#define MSG_PID_AUTOTUNE_FINISHED           MSG_PID_AUTOTUNE " finished! Put the last Kp, Ki and Kd constants from above into Configuration.h"
+#define MSG_PID_AUTOTUNE_FINISHED           MSG_PID_AUTOTUNE " finished! Put the last Kp, Ki and Kd constants from below into Configuration.h"
 #define MSG_PID_DEBUG                       " PID_DEBUG "
 #define MSG_PID_DEBUG_INPUT                 ": Input "
 #define MSG_PID_DEBUG_OUTPUT                " Output "
diff --git a/Marlin/configurator/index.html b/Marlin/configurator/index.html
index 9fa40a105da0cd53382ca4690c311bae6dcd3737..54353913d2ab7f2cae1a15ac283b9366e122545a 100644
--- a/Marlin/configurator/index.html
+++ b/Marlin/configurator/index.html
@@ -46,7 +46,7 @@
 
           <label class="newline">Motherboard:</label><select name="MOTHERBOARD"></select>
 
-          <label class="newline">Custom Name:</label><input name="CUSTOM_MENDEL_NAME" type="text" size="14" maxlength="12" value="" />
+          <label class="newline">Custom Name:</label><input name="CUSTOM_MACHINE_NAME" type="text" size="14" maxlength="12" value="" />
 
           <label class="newline">Machine UUID:</label><input name="MACHINE_UUID" type="text" size="38" maxlength="36" value="" />
 
diff --git a/Marlin/DOGMbitmaps.h b/Marlin/dogm_bitmaps.h
similarity index 100%
rename from Marlin/DOGMbitmaps.h
rename to Marlin/dogm_bitmaps.h
diff --git a/Marlin/dogm_font_data_ISO10646_CN.h b/Marlin/dogm_font_data_ISO10646_CN.h
new file mode 100644
index 0000000000000000000000000000000000000000..f74e8a60549f7e116cd7322687a4c6ead6a48146
--- /dev/null
+++ b/Marlin/dogm_font_data_ISO10646_CN.h
@@ -0,0 +1,270 @@
+/*
+  Fontname: ISO10646_CN
+  Copyright: A. Hardtung, public domain
+  Capital A Height: 7, '1' Height: 7
+  Calculated Max Values w=11 h=11 x= 2 y=10 dx=12 dy= 0 ascent=10 len=22
+  Font Bounding box     w=12 h=11 x= 0 y=-2
+  Calculated Min Values           x= 0 y=-1 dx= 0 dy= 0
+  Pure Font   ascent = 7 descent=-1
+  X Font      ascent = 7 descent=-1
+  Max Font    ascent =10 descent=-1
+*/
+#include <utility/u8g.h>
+const u8g_fntpgm_uint8_t ISO10646_CN[4105] U8G_SECTION(".progmem.ISO10646_CN") = {
+  0,12,11,0,254,7,1,146,3,33,32,255,255,10,255,7,
+  255,0,0,0,6,0,10,1,7,7,6,2,0,128,128,128,
+  128,128,0,128,3,2,2,6,1,5,160,160,5,7,7,6,
+  0,0,80,80,248,80,248,80,80,5,7,7,6,0,0,32,
+  120,160,112,40,240,32,5,7,7,6,0,0,192,200,16,32,
+  64,152,24,5,7,7,6,0,0,96,144,160,64,168,144,104,
+  2,3,3,6,1,4,192,64,128,3,7,7,6,1,0,32,
+  64,128,128,128,64,32,3,7,7,6,1,0,128,64,32,32,
+  32,64,128,5,5,5,6,0,1,32,168,112,168,32,5,5,
+  5,6,0,1,32,32,248,32,32,2,3,3,6,2,255,192,
+  64,128,5,1,1,6,0,3,248,2,2,2,6,2,0,192,
+  192,5,5,5,6,0,1,8,16,32,64,128,5,7,7,6,
+  0,0,112,136,152,168,200,136,112,3,7,7,6,1,0,64,
+  192,64,64,64,64,224,5,7,7,6,0,0,112,136,8,112,
+  128,128,248,5,7,7,6,0,0,248,16,32,16,8,8,240,
+  5,7,7,6,0,0,16,48,80,144,248,16,16,5,7,7,
+  6,0,0,248,128,240,8,8,136,112,5,7,7,6,0,0,
+  112,128,128,240,136,136,112,5,7,7,6,0,0,248,8,16,
+  32,32,32,32,5,7,7,6,0,0,112,136,136,112,136,136,
+  112,5,7,7,6,0,0,112,136,136,120,8,8,112,2,5,
+  5,6,2,0,192,192,0,192,192,2,6,6,6,2,255,192,
+  192,0,192,64,128,4,7,7,6,0,0,16,32,64,128,64,
+  32,16,5,3,3,6,0,2,248,0,248,4,7,7,6,0,
+  0,128,64,32,16,32,64,128,5,7,7,6,0,0,112,136,
+  8,16,32,0,32,5,7,7,6,0,0,112,136,8,104,168,
+  168,112,5,7,7,6,0,0,112,136,136,248,136,136,136,5,
+  7,7,6,0,0,240,136,136,240,136,136,240,5,7,7,6,
+  0,0,112,136,128,128,128,136,112,5,7,7,6,0,0,240,
+  136,136,136,136,136,240,5,7,7,6,0,0,248,128,128,240,
+  128,128,248,5,7,7,6,0,0,248,128,128,240,128,128,128,
+  5,7,7,6,0,0,112,136,128,184,136,136,112,5,7,7,
+  6,0,0,136,136,136,248,136,136,136,1,7,7,6,2,0,
+  128,128,128,128,128,128,128,5,7,7,6,0,0,56,16,16,
+  16,16,144,96,5,7,7,6,0,0,136,144,160,192,160,144,
+  136,5,7,7,6,0,0,128,128,128,128,128,128,248,5,7,
+  7,6,0,0,136,216,168,136,136,136,136,5,7,7,6,0,
+  0,136,136,200,168,152,136,136,5,7,7,6,0,0,112,136,
+  136,136,136,136,112,5,7,7,6,0,0,240,136,136,240,128,
+  128,128,5,7,7,6,0,0,112,136,136,136,168,144,104,5,
+  7,7,6,0,0,240,136,136,240,160,144,136,5,7,7,6,
+  0,0,120,128,128,112,8,8,240,5,7,7,6,0,0,248,
+  32,32,32,32,32,32,5,7,7,6,0,0,136,136,136,136,
+  136,136,112,5,7,7,6,0,0,136,136,136,136,136,80,32,
+  5,7,7,6,0,0,136,136,136,136,136,168,80,5,7,7,
+  6,0,0,136,136,80,32,80,136,136,5,7,7,6,0,0,
+  136,136,136,80,32,32,32,5,7,7,6,0,0,248,8,16,
+  32,64,128,248,3,7,7,6,0,0,224,128,128,128,128,128,
+  224,5,5,5,6,0,1,128,64,32,16,8,3,7,7,6,
+  0,0,224,32,32,32,32,32,224,5,3,3,6,0,4,32,
+  80,136,5,1,1,6,0,0,248,2,2,2,6,2,5,128,
+  64,5,5,5,6,0,0,112,8,120,136,120,5,7,7,6,
+  0,0,128,128,176,200,136,136,240,5,5,5,6,0,0,112,
+  128,128,136,112,5,7,7,6,0,0,8,8,104,152,136,136,
+  120,5,5,5,6,0,0,112,136,248,128,112,5,7,7,6,
+  0,0,48,72,224,64,64,64,64,5,6,6,6,0,255,112,
+  136,136,120,8,112,5,7,7,6,0,0,128,128,176,200,136,
+  136,136,1,7,7,6,2,0,128,0,128,128,128,128,128,3,
+  8,8,6,1,255,32,0,32,32,32,32,160,64,4,7,7,
+  6,1,0,128,128,144,160,192,160,144,3,7,7,6,1,0,
+  192,64,64,64,64,64,224,5,5,5,6,0,0,208,168,168,
+  168,168,5,5,5,6,0,0,176,200,136,136,136,5,5,5,
+  6,0,0,112,136,136,136,112,5,6,6,6,0,255,240,136,
+  136,240,128,128,5,6,6,6,0,255,120,136,136,120,8,8,
+  5,5,5,6,0,0,176,200,128,128,128,5,5,5,6,0,
+  0,112,128,112,8,240,4,7,7,6,0,0,64,64,224,64,
+  64,64,48,5,5,5,6,0,0,136,136,136,152,104,5,5,
+  5,6,0,0,136,136,136,80,32,5,5,5,6,0,0,136,
+  136,168,168,80,5,5,5,6,0,0,136,80,32,80,136,5,
+  6,6,6,0,255,136,136,136,120,8,112,5,5,5,6,0,
+  0,248,16,32,64,248,3,7,7,6,1,0,32,64,64,128,
+  64,64,32,1,7,7,6,2,0,128,128,128,128,128,128,128,
+  3,7,7,6,1,0,128,64,64,32,64,64,128,5,2,2,
+  6,0,3,104,144,0,0,0,6,0,10,0,0,0,12,0,
+  10,0,0,0,12,0,10,0,0,0,12,0,10,0,0,0,
+  12,0,10,0,0,0,12,0,10,0,0,0,12,0,10,0,
+  0,0,12,0,10,0,0,0,12,0,10,0,0,0,12,0,
+  10,0,0,0,12,0,10,0,0,0,12,0,10,0,0,0,
+  12,0,10,0,0,0,12,0,10,0,0,0,12,0,10,0,
+  0,0,12,0,10,0,0,0,12,0,10,0,0,0,12,0,
+  10,0,0,0,12,0,10,0,0,0,12,0,10,0,0,0,
+  12,0,10,0,0,0,12,0,10,0,0,0,12,0,10,0,
+  0,0,12,0,10,0,0,0,12,0,10,0,0,0,12,0,
+  10,0,0,0,12,0,10,0,0,0,12,0,10,0,0,0,
+  12,0,10,0,0,0,12,0,10,11,11,22,12,0,255,255,
+  224,2,0,2,0,4,0,13,0,20,128,36,64,196,32,4,
+  0,4,0,4,0,11,11,22,12,0,255,249,0,138,0,171,
+  224,172,64,170,64,170,64,170,64,170,128,33,0,82,128,140,
+  96,11,11,22,12,0,255,36,0,36,0,63,128,68,0,132,
+  0,4,0,255,224,10,0,17,0,32,128,192,96,11,11,22,
+  12,0,255,36,0,36,0,63,192,68,0,4,0,255,224,9,
+  0,9,0,17,32,33,32,64,224,11,11,22,12,0,255,32,
+  0,61,224,81,32,145,32,17,32,255,32,17,32,41,32,37,
+  224,69,32,128,0,11,11,22,12,0,255,32,128,127,192,8,
+  64,255,224,17,0,32,128,95,64,128,32,63,128,0,0,127,
+  192,11,11,22,12,0,255,34,64,71,224,148,128,228,128,47,
+  224,68,128,244,128,7,224,52,128,196,128,7,224,11,11,22,
+  12,0,255,4,128,143,224,73,0,25,0,47,192,9,0,9,
+  0,47,192,73,0,137,0,15,224,11,11,22,12,0,255,16,
+  0,63,128,81,0,14,0,49,128,192,96,63,128,36,128,63,
+  128,36,128,63,128,11,11,22,12,0,255,34,128,250,64,7,
+  224,250,128,138,128,138,128,250,128,34,128,178,128,170,160,100,
+  224,11,11,22,12,0,255,34,32,71,64,146,128,239,224,34,
+  0,71,192,236,64,7,192,52,64,199,192,4,64,11,11,22,
+  12,0,255,8,0,15,192,8,0,8,0,255,224,8,0,14,
+  0,9,128,8,64,8,0,8,0,10,11,22,12,0,255,255,
+  128,0,128,0,128,128,128,128,128,255,128,128,0,128,0,128,
+  64,128,64,127,192,11,11,22,12,0,255,71,192,65,0,239,
+  224,65,0,69,0,105,96,201,32,77,96,73,32,79,224,200,
+  32,11,11,22,12,0,255,8,0,4,0,4,0,10,0,10,
+  0,10,0,17,0,17,0,32,128,64,64,128,32,11,11,22,
+  12,0,255,34,64,34,0,247,224,34,0,35,224,53,32,229,
+  32,37,64,40,128,41,64,114,32,11,10,20,12,0,0,68,
+  64,68,64,68,64,127,192,4,0,4,0,132,32,132,32,132,
+  32,255,224,11,11,22,12,0,255,4,0,0,0,127,192,4,
+  0,4,0,4,0,127,192,4,0,4,0,4,0,255,224,11,
+  11,22,12,0,255,255,224,17,0,1,192,254,0,72,128,37,
+  0,4,0,255,224,21,0,36,128,196,96,11,11,22,12,0,
+  255,17,0,127,192,68,64,127,192,68,64,127,192,4,0,255,
+  224,4,0,4,0,4,0,9,11,22,12,0,255,16,0,255,
+  128,128,128,128,128,255,128,128,128,128,128,255,128,128,128,128,
+  128,255,128,11,11,22,12,0,255,113,0,1,0,3,224,249,
+  32,33,32,65,32,81,32,137,32,250,32,2,32,4,192,11,
+  11,22,12,0,255,127,192,17,0,17,0,17,0,17,0,255,
+  224,17,0,17,0,33,0,33,0,65,0,11,11,22,12,0,
+  255,33,0,34,0,244,64,87,224,80,32,87,192,148,64,84,
+  64,36,64,87,192,148,64,11,11,22,12,0,255,17,0,10,
+  0,127,192,4,0,4,0,255,224,4,0,10,0,17,0,32,
+  128,192,96,10,11,22,12,0,255,95,192,0,64,132,64,132,
+  64,191,64,132,64,140,64,148,64,164,64,140,64,129,192,11,
+  11,22,12,0,255,36,0,39,192,36,0,36,0,255,224,0,
+  0,20,64,36,128,71,0,12,0,112,0,11,11,22,12,0,
+  255,36,128,4,128,15,192,228,128,36,128,63,224,36,128,36,
+  128,40,128,80,0,143,224,11,11,22,12,0,255,8,0,8,
+  0,255,128,136,128,136,128,255,128,136,128,136,128,255,160,136,
+  32,7,224,11,11,22,12,0,255,39,128,36,128,244,128,36,
+  128,116,128,108,128,164,128,36,128,36,160,40,160,48,96,10,
+  11,22,12,0,255,255,192,128,64,128,64,158,64,146,64,146,
+  64,158,64,128,64,128,64,255,192,128,64,11,11,22,12,0,
+  255,127,192,68,0,95,192,80,64,95,192,80,64,95,192,66,
+  0,74,128,82,64,166,32,11,11,22,12,0,255,4,0,7,
+  224,4,0,127,192,64,64,64,64,64,64,127,192,0,0,82,
+  64,137,32,11,11,22,12,0,255,71,128,36,128,4,128,4,
+  128,232,96,32,0,47,192,36,64,34,128,49,0,38,192,11,
+  11,22,12,0,255,127,192,74,64,127,192,4,0,255,224,4,
+  0,63,128,32,128,36,128,36,128,255,224,11,11,22,12,0,
+  255,34,0,79,224,72,32,79,224,200,0,79,224,74,160,90,
+  160,111,224,74,160,72,96,11,11,22,12,0,255,243,192,36,
+  64,42,128,241,0,34,128,101,224,114,32,165,64,32,128,35,
+  0,44,0,11,11,22,12,0,255,4,0,255,224,128,32,0,
+  0,255,224,4,0,36,0,39,192,36,0,84,0,143,224,11,
+  11,22,12,0,255,115,224,16,128,81,0,35,224,250,32,42,
+  160,34,160,34,160,32,128,33,64,98,32,11,11,22,12,0,
+  255,34,0,247,128,34,128,54,128,226,160,37,160,36,96,104,
+  32,0,0,82,64,137,32,11,11,22,12,0,255,115,192,66,
+  0,66,0,123,224,74,64,74,64,122,64,74,64,66,64,68,
+  64,136,64,11,11,22,12,0,255,8,0,255,224,8,0,31,
+  192,48,64,95,192,144,64,31,192,16,64,16,64,16,192,11,
+  11,22,12,0,255,2,0,127,224,66,0,66,0,95,192,66,
+  0,71,0,74,128,82,64,98,32,130,0,11,11,22,12,0,
+  255,243,192,150,64,145,128,166,96,161,0,151,192,145,0,149,
+  0,231,224,129,0,129,0,11,11,22,12,0,255,15,128,136,
+  128,79,128,8,128,143,128,64,0,31,192,53,64,85,64,149,
+  64,63,224,11,11,22,12,0,255,39,224,32,128,248,128,32,
+  128,32,128,56,128,224,128,32,128,32,128,32,128,97,128,11,
+  11,22,12,0,255,31,224,145,0,87,192,20,64,23,192,148,
+  64,87,192,17,0,85,64,153,32,35,0,11,11,22,12,0,
+  255,32,128,39,224,242,64,33,128,34,64,52,32,226,64,34,
+  64,34,64,34,64,100,64,11,11,22,12,0,255,65,0,65,
+  0,79,224,233,32,73,32,73,32,111,224,201,32,73,32,73,
+  32,207,224,11,11,22,12,0,255,33,0,241,0,79,224,169,
+  32,249,32,47,224,57,32,233,32,41,32,47,224,40,32,11,
+  11,22,12,0,255,143,224,73,32,9,32,203,160,73,32,79,
+  224,72,32,75,160,74,160,107,160,80,224,11,11,22,12,0,
+  255,127,192,4,0,68,64,36,64,36,128,4,0,255,224,4,
+  0,4,0,4,0,4,0,11,11,22,12,0,255,130,0,66,
+  0,31,224,194,0,95,192,82,64,95,192,71,0,74,128,82,
+  64,191,224,11,11,22,12,0,255,4,0,127,224,72,128,127,
+  224,72,128,79,128,64,0,95,192,72,64,71,128,152,96,11,
+  11,22,12,0,255,1,0,239,224,161,0,164,64,175,224,164,
+  64,175,224,169,32,233,32,2,128,12,96,11,11,22,12,0,
+  255,20,192,246,160,188,96,167,128,168,128,191,224,169,32,239,
+  224,9,32,15,224,9,32,11,11,22,12,0,255,127,128,64,
+  128,66,128,98,128,84,128,72,128,72,128,84,160,98,160,64,
+  96,128,32,11,11,22,12,0,255,4,0,127,224,64,32,127,
+  224,64,0,125,224,84,32,76,160,84,96,100,160,141,96,11,
+  11,22,12,0,255,130,0,95,224,4,0,8,64,159,224,64,
+  32,10,128,10,128,74,160,146,160,34,96,11,11,22,12,0,
+  255,65,0,79,224,232,32,66,128,68,64,104,32,199,192,65,
+  0,65,0,65,0,207,224,11,11,22,12,0,255,80,32,125,
+  32,145,32,255,32,17,32,125,32,85,32,85,32,84,32,92,
+  32,16,224,11,11,22,12,0,255,63,128,32,128,63,128,32,
+  128,255,224,72,0,123,192,73,64,121,64,72,128,251,96,11,
+  11,22,12,0,255,4,0,4,0,4,0,36,128,36,64,68,
+  64,68,32,132,32,4,0,4,0,28,0,11,11,22,12,0,
+  255,4,0,4,0,4,0,255,224,4,0,10,0,10,0,17,
+  0,17,0,32,128,192,96,9,10,20,10,0,0,136,128,73,
+  0,8,0,255,128,0,128,0,128,127,128,0,128,0,128,255,
+  128,11,11,22,12,0,255,33,0,18,0,255,224,0,0,120,
+  128,74,128,122,128,74,128,122,128,72,128,89,128,11,11,22,
+  12,0,255,39,192,0,0,0,0,239,224,33,0,34,0,36,
+  64,47,224,32,32,80,0,143,224,11,11,22,12,0,255,32,
+  128,39,0,249,0,33,192,119,0,33,0,249,224,39,0,113,
+  32,169,32,32,224,11,11,22,12,0,255,16,64,16,64,253,
+  224,16,64,56,192,53,64,82,64,148,64,16,64,16,64,16,
+  192,11,11,22,12,0,255,0,64,248,64,11,224,8,64,136,
+  64,82,64,81,64,33,64,80,64,72,64,137,192,10,11,22,
+  12,0,255,132,0,132,64,132,128,245,0,134,0,132,0,132,
+  0,148,0,164,64,196,64,131,192,11,11,22,12,0,255,17,
+  32,125,0,17,0,255,224,41,0,253,64,73,64,124,128,8,
+  160,253,96,10,32,11,11,22,12,0,255,23,192,36,64,36,
+  64,103,192,161,0,47,224,33,0,35,128,37,64,41,32,33,
+  0,11,11,22,12,0,255,8,0,255,224,16,0,39,192,32,
+  128,97,0,175,224,33,0,33,0,33,0,35,0,11,11,22,
+  12,0,255,36,0,47,224,180,0,164,128,164,160,170,192,42,
+  128,40,128,41,64,50,64,36,32,11,11,22,12,0,255,127,
+  224,128,0,63,192,32,64,63,192,16,0,31,192,16,64,40,
+  128,71,0,56,224,11,11,22,12,0,255,127,224,64,0,64,
+  0,64,0,64,0,64,0,64,0,64,0,64,0,64,0,128,
+  0,11,11,22,12,0,255,255,224,4,0,127,192,68,64,127,
+  192,68,64,127,192,68,0,36,0,24,0,231,224,11,11,22,
+  12,0,255,17,224,253,0,69,0,41,224,253,64,17,64,125,
+  64,17,64,85,64,146,64,52,64,11,11,22,12,0,255,33,
+  0,95,224,64,0,207,192,64,0,79,192,64,0,79,192,72,
+  64,79,192,72,64,11,11,22,12,0,255,4,0,127,192,64,
+  64,127,192,64,64,127,192,64,64,127,192,4,64,82,32,191,
+  160,11,11,22,12,0,255,127,192,68,64,127,192,68,64,127,
+  192,4,0,27,0,224,224,17,0,17,0,97,0,11,11,22,
+  12,0,255,255,224,4,0,8,0,127,224,73,32,79,32,73,
+  32,79,32,73,32,73,32,127,224,11,11,22,12,0,255,253,
+  224,86,64,121,64,56,128,85,64,146,32,255,224,4,0,39,
+  192,36,0,255,224,11,11,22,12,0,255,251,128,82,0,123,
+  224,18,64,250,64,20,64,63,128,32,128,63,128,32,128,63,
+  128,11,11,22,12,0,255,31,224,32,0,39,192,100,64,167,
+  192,32,0,47,224,40,32,39,192,33,0,35,0,11,11,22,
+  12,0,255,243,224,130,32,130,32,250,32,130,32,130,32,138,
+  32,178,32,194,224,2,0,2,0,11,11,22,12,0,255,36,
+  128,70,160,149,192,228,128,39,224,68,128,245,192,6,160,52,
+  128,196,128,7,224,11,11,22,12,0,255,39,192,65,0,135,
+  224,224,32,34,128,69,128,242,128,15,224,48,128,193,64,2,
+  32,11,11,22,12,0,255,2,0,2,0,34,0,35,192,34,
+  0,34,0,34,0,34,0,34,0,34,0,255,224,9,11,22,
+  12,0,255,8,0,8,0,255,128,136,128,136,128,136,128,255,
+  128,136,128,136,128,136,128,255,128,11,11,22,12,0,255,33,
+  0,83,160,65,0,247,224,81,0,83,192,86,64,83,192,90,
+  64,83,192,66,64,11,11,22,12,0,255,127,192,4,0,4,
+  0,4,0,255,224,10,0,10,0,18,0,34,32,66,32,129,
+  224,11,11,22,12,0,255,17,0,33,0,47,224,97,0,163,
+  128,35,128,37,64,37,64,41,32,33,0,33,0,11,11,22,
+  12,0,255,247,224,148,32,244,32,151,224,148,128,244,128,151,
+  224,148,128,244,160,150,96,4,32,11,11,22,12,0,255,123,
+  224,148,128,4,0,127,192,4,0,255,224,1,0,255,224,33,
+  0,17,0,7,0,11,11,22,12,0,255,33,0,71,192,145,
+  0,47,224,96,128,175,224,32,128,36,128,34,128,32,128,35,
+  128,11,11,22,12,0,255,39,192,36,64,247,192,46,224,42,
+  160,62,224,225,0,47,224,35,128,37,64,105,32,11,11,22,
+  12,0,255,20,0,39,224,42,0,98,0,163,192,34,0,34,
+  0,35,224,34,0,34,0,34,0};
diff --git a/Marlin/dogm_lcd_implementation.h b/Marlin/dogm_lcd_implementation.h
index 2a6d229b7f3c4dd2592fe5457aca945a402a4583..ff7cc999061af128c656587203bd5e83946c3cf5 100644
--- a/Marlin/dogm_lcd_implementation.h
+++ b/Marlin/dogm_lcd_implementation.h
@@ -29,7 +29,7 @@
 #endif
 
 #include <U8glib.h>
-#include "DOGMbitmaps.h"
+#include "dogm_bitmaps.h"
 
 #include "ultralcd.h"
 #include "ultralcd_st7920_u8glib_rrd.h"
@@ -64,6 +64,10 @@
   #elif defined( DISPLAY_CHARSET_ISO10646_KANA )
     #include "dogm_font_data_ISO10646_Kana.h"
     #define FONT_MENU_NAME ISO10646_Kana_5x7
+  #elif defined( DISPLAY_CHARSET_ISO10646_CN )
+    #include "dogm_font_data_ISO10646_CN.h"
+    #define FONT_MENU_NAME ISO10646_CN
+    #define TALL_FONT_CORRECTION 1
   #else // fall-back
     #include "dogm_font_data_ISO10646_1.h"
     #define FONT_MENU_NAME ISO10646_1_5x7
@@ -106,23 +110,11 @@
   #define LCD_WIDTH_EDIT       22
 #endif
 
-#define START_ROW              0
+#ifndef TALL_FONT_CORRECTION
+  #define TALL_FONT_CORRECTION 0
+#endif
 
-/* Custom characters defined in font font_6x10_marlin_symbols */
-// \x00 intentionally skipped to avoid problems in strings
-#define LCD_STR_REFRESH     "\x01"
-#define LCD_STR_FOLDER      "\x02"
-#define LCD_STR_ARROW_RIGHT "\x03"
-#define LCD_STR_UPLEVEL     "\x04"
-#define LCD_STR_CLOCK       "\x05"
-#define LCD_STR_FEEDRATE    "\x06"
-#define LCD_STR_BEDTEMP     "\x07"
-#define LCD_STR_THERMOMETER "\x08"
-#define LCD_STR_DEGREE      "\x09"
-
-#define LCD_STR_SPECIAL_MAX '\x09'
-// Maximum here is 0x1f because 0x20 is ' ' (space) and the normal charsets begin.
-// Better stay below 0x10 because DISPLAY_CHARSET_HD44780_WESTERN begins here.
+#define START_ROW              0
 
 // LCD selection
 #ifdef U8GLIB_ST7920
@@ -139,6 +131,13 @@
   U8GLIB_DOGM128 u8g(DOGLCD_CS, DOGLCD_A0);  // HW-SPI Com: CS, A0
 #endif
 
+#ifndef LCD_PIXEL_WIDTH
+  #define LCD_PIXEL_WIDTH 128
+#endif
+#ifndef LCD_PIXEL_HEIGHT
+  #define LCD_PIXEL_HEIGHT 64
+#endif
+
 #include "utf_mapper.h"
 
 int lcd_contrast;
@@ -188,6 +187,7 @@ char lcd_printPGM(const char* str) {
 
 static bool show_splashscreen = true;
 
+/* Warning: This function is called from interrupt context */
 static void lcd_implementation_init() {
 
   #ifdef LCD_PIN_BL // Enable LCD backlight
@@ -268,25 +268,26 @@ static void lcd_implementation_status_screen() {
  
   #ifdef SDSUPPORT
     // SD Card Symbol
-    u8g.drawBox(42,42,8,7);
-    u8g.drawBox(50,44,2,5);
-    u8g.drawFrame(42,49,10,4);
-    u8g.drawPixel(50,43);
+    u8g.drawBox(42, 42 - TALL_FONT_CORRECTION, 8, 7);
+    u8g.drawBox(50, 44 - TALL_FONT_CORRECTION, 2, 5);
+    u8g.drawFrame(42, 49 - TALL_FONT_CORRECTION, 10, 4);
+    u8g.drawPixel(50, 43 - TALL_FONT_CORRECTION);
+
 
     // Progress bar frame
-    u8g.drawFrame(54,49,73,4);
+    u8g.drawFrame(54, 49, 73, 4 - TALL_FONT_CORRECTION);
 
     // SD Card Progress bar and clock
     lcd_setFont(FONT_STATUSMENU);
  
     if (IS_SD_PRINTING) {
       // Progress bar solid part
-      u8g.drawBox(55, 50, (unsigned int)(71.f * card.percentDone() / 100.f), 2);
+      u8g.drawBox(55, 50, (unsigned int)(71.f * card.percentDone() / 100.f), 2 - TALL_FONT_CORRECTION);
     }
 
     u8g.setPrintPos(80,48);
-    if (starttime != 0) {
-      uint16_t time = (millis() - starttime) / 60000;
+    if (print_job_start_ms != 0) {
+      uint16_t time = (millis() - print_job_start_ms) / 60000;
       lcd_print(itostr2(time/60));
       lcd_print(':');
       lcd_print(itostr2(time%60));
@@ -322,9 +323,9 @@ static void lcd_implementation_status_screen() {
   lcd_setFont(FONT_STATUSMENU);
 
   #ifdef USE_SMALL_INFOFONT
-    u8g.drawBox(0,30,128,10);
+    u8g.drawBox(0,30,LCD_PIXEL_WIDTH,10);
   #else
-    u8g.drawBox(0,30,128,9);
+    u8g.drawBox(0,30,LCD_PIXEL_WIDTH,9);
   #endif
   u8g.setColorIndex(0); // white on black
   u8g.setPrintPos(2,XYZ_BASELINE);
@@ -353,7 +354,7 @@ static void lcd_implementation_status_screen() {
   lcd_print(LCD_STR_FEEDRATE[0]);
   lcd_setFont(FONT_STATUSMENU);
   u8g.setPrintPos(12,49);
-  lcd_print(itostr3(feedmultiply));
+  lcd_print(itostr3(feedrate_multiplier));
   lcd_print('%');
 
   // Status line
@@ -366,7 +367,7 @@ static void lcd_implementation_status_screen() {
   #ifndef FILAMENT_LCD_DISPLAY
     lcd_print(lcd_status_message);
   #else
-    if (millis() < message_millis + 5000) {  //Display both Status message line and Filament display on the last line
+    if (millis() < previous_lcd_status_ms + 5000) {  //Display both Status message line and Filament display on the last line
       lcd_print(lcd_status_message);
     }
     else {
@@ -382,7 +383,7 @@ static void lcd_implementation_status_screen() {
 static void lcd_implementation_mark_as_selected(uint8_t row, bool isSelected) {
   if (isSelected) {
     u8g.setColorIndex(1);  // black on white
-    u8g.drawBox(0, row * DOG_CHAR_HEIGHT + 3, 128, DOG_CHAR_HEIGHT);
+    u8g.drawBox(0, row * DOG_CHAR_HEIGHT + 3 - TALL_FONT_CORRECTION, LCD_PIXEL_WIDTH, DOG_CHAR_HEIGHT);
     u8g.setColorIndex(0);  // following text must be white on black
   }
   else {
@@ -402,13 +403,15 @@ static void lcd_implementation_drawmenu_generic(bool isSelected, uint8_t row, co
     pstr++;
   }
   while (n--) lcd_print(' ');
+    u8g.setPrintPos(LCD_PIXEL_WIDTH - DOG_CHAR_WIDTH, (row + 1) * DOG_CHAR_HEIGHT);
   lcd_print(post_char);
   lcd_print(' ');
 }
 
 static void _drawmenu_setting_edit_generic(bool isSelected, uint8_t row, const char* pstr, const char* data, bool pgm) {
   char c;
-  uint8_t n = LCD_WIDTH - 2 - (pgm ? lcd_strlen_P(data) : (lcd_strlen((char*)data)));
+  uint8_t vallen = (pgm ? lcd_strlen_P(data) : (lcd_strlen((char*)data)));
+  uint8_t n = LCD_WIDTH - 2 - vallen;
 
   lcd_implementation_mark_as_selected(row, isSelected);
 
@@ -418,6 +421,7 @@ static void _drawmenu_setting_edit_generic(bool isSelected, uint8_t row, const c
   }
   lcd_print(':');
   while (n--) lcd_print(' ');
+  u8g.setPrintPos(LCD_PIXEL_WIDTH - DOG_CHAR_WIDTH * vallen, (row + 1) * DOG_CHAR_HEIGHT);
   if (pgm) { lcd_printPGM(data); } else { lcd_print((char *)data); }
 }
 
diff --git a/Marlin/example_configurations/Felix/Configuration.h b/Marlin/example_configurations/Felix/Configuration.h
index f5d839a3e37dc8b605dfad92acaf332334c1d384..41e0955a442e1fc7503920a80af97fc27ef0c9cd 100644
--- a/Marlin/example_configurations/Felix/Configuration.h
+++ b/Marlin/example_configurations/Felix/Configuration.h
@@ -39,7 +39,6 @@ Here are some standard links for getting your machine calibrated:
 // startup. Implementation of an idea by Prof Braino to inform user that any changes made to this
 // build by the user have been successfully uploaded into firmware.
 #define STRING_VERSION "1.0.3 dev"
-#define STRING_URL "reprap.org"
 #define STRING_VERSION_CONFIG_H __DATE__ " " __TIME__ // build date and time
 #define STRING_CONFIG_H_AUTHOR "(none, default config)" // Who made the changes.
 #define STRING_SPLASH_LINE1 "v" STRING_VERSION // will be shown during bootup in line 1
@@ -62,8 +61,9 @@ Here are some standard links for getting your machine calibrated:
   #define MOTHERBOARD BOARD_FELIX2
 #endif
 
-// Define this to set a custom name for your generic Mendel,
-// #define CUSTOM_MENDEL_NAME "This Mendel"
+// Optional custom name for your RepStrap or other custom machine
+// Displayed in the LCD "Ready" message
+#define CUSTOM_MACHINE_NAME "Felix"
 
 // Define this to set a unique identifier for this printer, (Used by some programs to differentiate between machines)
 // You can use an online service to generate a random UUID. (eg http://www.uuidgenerator.net/version4)
@@ -305,6 +305,7 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 const bool X_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop.
 const bool Y_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop.
 const bool Z_MAX_ENDSTOP_INVERTING = true; // 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
 
@@ -315,6 +316,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 #define E_ENABLE_ON 0 // For all extruders
 
 // Disables axis when it's not being used.
+// WARNING: When motors turn off there is a chance of losing position accuracy!
 #define DISABLE_X false
 #define DISABLE_Y false
 #define DISABLE_Z false
@@ -572,7 +574,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //==============================LCD and SD support=============================
 
 // Define your display language below. Replace (en) with your language code and uncomment.
-// en, pl, fr, de, es, ru, bg, it, pt, pt-br, fi, an, nl, ca, eu, kana, kana_utf8, test
+// en, pl, fr, de, es, ru, bg, it, pt, pt-br, fi, an, nl, ca, eu, kana, kana_utf8, cn, test
 // See also language.h
 //#define LANGUAGE_INCLUDE GENERATE_LANGUAGE_INCLUDE(en)
 
@@ -592,9 +594,9 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //#define ENCODER_STEPS_PER_MENU_ITEM 5 // Set according to ENCODER_PULSES_PER_STEP or your liking
 //#define ULTIMAKERCONTROLLER //as available from the Ultimaker online store.
 //#define ULTIPANEL  //the UltiPanel as on Thingiverse
-//#define LCD_FEEDBACK_FREQUENCY_HZ 1000	// this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
 //#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click
-                                               // 0 to disable buzzer feedback  
+//#define LCD_FEEDBACK_FREQUENCY_HZ 1000         // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
+                                                 // 0 to disable buzzer feedback. Test with M300 S<frequency Hz> P<duration ms>
 
 // PanelOne from T3P3 (via RAMPS 1.4 AUX2/AUX3)
 // http://reprap.org/wiki/PanelOne
diff --git a/Marlin/example_configurations/Felix/Configuration_DUAL.h b/Marlin/example_configurations/Felix/Configuration_DUAL.h
index 93eb47985b38996fcd2ee93b301ee814122edba9..3d485c89bc7b5b0f6a0bf6fa10a83c66f832c2e0 100644
--- a/Marlin/example_configurations/Felix/Configuration_DUAL.h
+++ b/Marlin/example_configurations/Felix/Configuration_DUAL.h
@@ -1,4 +1,4 @@
-#ifndef CONFIGURATION_H
+#ifndef CONFIGURATION_H
 #define CONFIGURATION_H
 
 #include "boards.h"
@@ -39,7 +39,6 @@ Here are some standard links for getting your machine calibrated:
 // startup. Implementation of an idea by Prof Braino to inform user that any changes made to this
 // build by the user have been successfully uploaded into firmware.
 #define STRING_VERSION "1.0.3 dev"
-#define STRING_URL "reprap.org"
 #define STRING_VERSION_CONFIG_H __DATE__ " " __TIME__ // build date and time
 #define STRING_CONFIG_H_AUTHOR "(none, default config)" // Who made the changes.
 #define STRING_SPLASH_LINE1 "v" STRING_VERSION // will be shown during bootup in line 1
@@ -62,8 +61,9 @@ Here are some standard links for getting your machine calibrated:
   #define MOTHERBOARD BOARD_FELIX2
 #endif
 
-// Define this to set a custom name for your generic Mendel,
-// #define CUSTOM_MENDEL_NAME "This Mendel"
+// Optional custom name for your RepStrap or other custom machine
+// Displayed in the LCD "Ready" message
+#define CUSTOM_MACHINE_NAME "Felix Dual"
 
 // Define this to set a unique identifier for this printer, (Used by some programs to differentiate between machines)
 // You can use an online service to generate a random UUID. (eg http://www.uuidgenerator.net/version4)
@@ -305,6 +305,7 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 const bool X_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop.
 const bool Y_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop.
 const bool Z_MAX_ENDSTOP_INVERTING = true; // 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
 
@@ -315,6 +316,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 #define E_ENABLE_ON 0 // For all extruders
 
 // Disables axis when it's not being used.
+// WARNING: When motors turn off there is a chance of losing position accuracy!
 #define DISABLE_X false
 #define DISABLE_Y false
 #define DISABLE_Z false
@@ -568,7 +570,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //==============================LCD and SD support=============================
 
 // Define your display language below. Replace (en) with your language code and uncomment.
-// en, pl, fr, de, es, ru, bg, it, pt, pt-br, fi, an, nl, ca, eu, kana, kana_utf8, test
+// en, pl, fr, de, es, ru, bg, it, pt, pt-br, fi, an, nl, ca, eu, kana, kana_utf8, cn, test
 // See also language.h
 //#define LANGUAGE_INCLUDE GENERATE_LANGUAGE_INCLUDE(en)
 
@@ -588,9 +590,9 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //#define ENCODER_STEPS_PER_MENU_ITEM 5 // Set according to ENCODER_PULSES_PER_STEP or your liking
 //#define ULTIMAKERCONTROLLER //as available from the Ultimaker online store.
 //#define ULTIPANEL  //the UltiPanel as on Thingiverse
-//#define LCD_FEEDBACK_FREQUENCY_HZ 1000	// this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
 //#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click
-                                               // 0 to disable buzzer feedback  
+//#define LCD_FEEDBACK_FREQUENCY_HZ 1000         // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
+                                                 // 0 to disable buzzer feedback. Test with M300 S<frequency Hz> P<duration ms>
 
 // PanelOne from T3P3 (via RAMPS 1.4 AUX2/AUX3)
 // http://reprap.org/wiki/PanelOne
diff --git a/Marlin/example_configurations/Felix/Configuration_adv.h b/Marlin/example_configurations/Felix/Configuration_adv.h
index a80790b95782ef31d5416af8c495c074b791729a..672b3ff028fc6af40ccf23327d17deadf3208210 100644
--- a/Marlin/example_configurations/Felix/Configuration_adv.h
+++ b/Marlin/example_configurations/Felix/Configuration_adv.h
@@ -195,6 +195,9 @@
 #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.
 
+// When G28 is called, this option will make Y home before X
+// #define HOME_Y_BEFORE_X
+
 // @section machine
 
 #define AXIS_RELATIVE_MODES {false, false, false, false}
@@ -258,7 +261,6 @@
 #define ENCODER_RATE_MULTIPLIER         // If defined, certain menu edit operations automatically multiply the steps when the encoder is moved quickly
 #define ENCODER_10X_STEPS_PER_SEC 75    // If the encoder steps per sec exceeds this value, multiply steps moved x10 to quickly advance the value
 #define ENCODER_100X_STEPS_PER_SEC 160  // If the encoder steps per sec exceeds this value, multiply steps moved x100 to really quickly advance the value
-//#define ENCODER_RATE_MULTIPLIER_DEBUG  // If defined, output the encoder steps per second value
 
 //#define CHDK 4        //Pin for triggering CHDK to take a picture see how to use it here http://captain-slow.dk/2014/03/09/3d-printing-timelapses/
 #define CHDK_DELAY 50 //How long in ms the pin should stay HIGH before going LOW again
@@ -355,7 +357,7 @@ const unsigned int dropsegments=5; //everything with less than this number of st
 //#define HEATERS_PARALLEL
 
 //===========================================================================
-//=============================Buffers           ============================
+//================================= Buffers =================================
 //===========================================================================
 
 // @section hidden
diff --git a/Marlin/example_configurations/Hephestos/Configuration.h b/Marlin/example_configurations/Hephestos/Configuration.h
index e4df70ba23cc38d7011f79d04dd663c806591782..f53dc9d9347b47177433b6f252a5d1be0baea202 100644
--- a/Marlin/example_configurations/Hephestos/Configuration.h
+++ b/Marlin/example_configurations/Hephestos/Configuration.h
@@ -1,4 +1,4 @@
-#ifndef CONFIGURATION_H
+#ifndef CONFIGURATION_H
 #define CONFIGURATION_H
 
 #include "boards.h"
@@ -39,7 +39,6 @@ Here are some standard links for getting your machine calibrated:
 // startup. Implementation of an idea by Prof Braino to inform user that any changes made to this
 // build by the user have been successfully uploaded into firmware.
 #define STRING_VERSION "1.0.3 dev"
-#define STRING_URL "reprap.org"
 #define STRING_VERSION_CONFIG_H __DATE__ " " __TIME__ // build date and time
 #define STRING_CONFIG_H_AUTHOR "(bq Hephestos)" // Who made the changes.
 #define STRING_SPLASH_LINE1 "v" STRING_VERSION // will be shown during bootup in line 1
@@ -58,13 +57,12 @@ Here are some standard links for getting your machine calibrated:
 
 // The following define selects which electronics board you have.
 // Please choose the name from boards.h that matches your setup
-#ifndef MOTHERBOARD
-  #define MOTHERBOARD BOARD_HEPHESTOS
-#endif
-
-// Define this to set a custom name for your generic Mendel,
-// #define CUSTOM_MENDEL_NAME "This Mendel"
+#define MOTHERBOARD BOARD_HEPHESTOS
 
+// Optional custom name for your RepStrap or other custom machine
+// Displayed in the LCD "Ready" message
+// #define CUSTOM_MACHINE_NAME "3D Printer"
+ 
 // Define this to set a unique identifier for this printer, (Used by some programs to differentiate between machines)
 // You can use an online service to generate a random UUID. (eg http://www.uuidgenerator.net/version4)
 // #define MACHINE_UUID "00000000-0000-0000-0000-000000000000"
@@ -328,6 +326,7 @@ const bool Z_MIN_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 const bool X_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop.
 const bool Y_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop.
 const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop.
+const bool Z_PROBE_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop.
 //#define DISABLE_MAX_ENDSTOPS
 //#define DISABLE_MIN_ENDSTOPS
 
@@ -338,6 +337,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 #define E_ENABLE_ON 0 // For all extruders
 
 // Disables axis when it's not being used.
+// WARNING: When motors turn off there is a chance of losing position accuracy!
 #define DISABLE_X false
 #define DISABLE_Y false
 #define DISABLE_Z false
@@ -594,7 +594,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //==============================LCD and SD support=============================
 
 // Define your display language below. Replace (en) with your language code and uncomment.
-// en, pl, fr, de, es, ru, bg, it, pt, pt-br, fi, an, nl, ca, eu, kana, kana_utf8, test
+// en, pl, fr, de, es, ru, bg, it, pt, pt-br, fi, an, nl, ca, eu, kana, kana_utf8, cn, test
 // See also language.h
 //#define LANGUAGE_INCLUDE GENERATE_LANGUAGE_INCLUDE(en)
 
@@ -614,9 +614,9 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //#define ENCODER_STEPS_PER_MENU_ITEM 5 // Set according to ENCODER_PULSES_PER_STEP or your liking
 //#define ULTIMAKERCONTROLLER //as available from the Ultimaker online store.
 //#define ULTIPANEL  //the UltiPanel as on Thingiverse
-//#define LCD_FEEDBACK_FREQUENCY_HZ 1000	// this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
 //#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click
-                                               // 0 to disable buzzer feedback  
+//#define LCD_FEEDBACK_FREQUENCY_HZ 1000         // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
+                                                 // 0 to disable buzzer feedback. Test with M300 S<frequency Hz> P<duration ms>
 
 // PanelOne from T3P3 (via RAMPS 1.4 AUX2/AUX3)
 // http://reprap.org/wiki/PanelOne
diff --git a/Marlin/example_configurations/Hephestos/Configuration_adv.h b/Marlin/example_configurations/Hephestos/Configuration_adv.h
index 84536d044e4ed35bf643cf765ec0cd46fa6ff5a8..5713373d165d80b48e28ae4dfd8f8e18f5829c0f 100644
--- a/Marlin/example_configurations/Hephestos/Configuration_adv.h
+++ b/Marlin/example_configurations/Hephestos/Configuration_adv.h
@@ -195,6 +195,9 @@
 #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.
 
+// When G28 is called, this option will make Y home before X
+// #define HOME_Y_BEFORE_X
+
 // @section machine
 
 #define AXIS_RELATIVE_MODES {false, false, false, false}
@@ -258,7 +261,6 @@
 #define ENCODER_RATE_MULTIPLIER         // If defined, certain menu edit operations automatically multiply the steps when the encoder is moved quickly
 #define ENCODER_10X_STEPS_PER_SEC 75    // If the encoder steps per sec exceeds this value, multiply steps moved x10 to quickly advance the value
 #define ENCODER_100X_STEPS_PER_SEC 160  // If the encoder steps per sec exceeds this value, multiply steps moved x100 to really quickly advance the value
-//#define ENCODER_RATE_MULTIPLIER_DEBUG  // If defined, output the encoder steps per second value
 
 //#define CHDK 4        //Pin for triggering CHDK to take a picture see how to use it here http://captain-slow.dk/2014/03/09/3d-printing-timelapses/
 #define CHDK_DELAY 50 //How long in ms the pin should stay HIGH before going LOW again
@@ -355,7 +357,7 @@ const unsigned int dropsegments=5; //everything with less than this number of st
 //#define HEATERS_PARALLEL
 
 //===========================================================================
-//=============================Buffers           ============================
+//================================= Buffers =================================
 //===========================================================================
 
 // @section hidden
diff --git a/Marlin/example_configurations/K8200/Configuration.h b/Marlin/example_configurations/K8200/Configuration.h
index 860f04da2e16994fcef970c9d3d5b4b1f531a04a..ac5ff0c71f05b6d3cade460b8ba9dbafce02e099 100644
--- a/Marlin/example_configurations/K8200/Configuration.h
+++ b/Marlin/example_configurations/K8200/Configuration.h
@@ -1,4 +1,9 @@
-#ifndef CONFIGURATION_H
+// Example configuration file for Vellemann K8200
+// tested on K8200 with VM8201 (Display)
+// and Arduino 1.6.1 (Win) by @CONSULitAS, 2015-04-14
+// https://github.com/CONSULitAS/Marlin-K8200/archive/K8200_stable_2015-04-14.zip
+
+#ifndef CONFIGURATION_H
 #define CONFIGURATION_H
 
 #include "boards.h"
@@ -31,26 +36,31 @@ 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.
 //
 
+// @section info
+
 // User-specified version info of this build to display in [Pronterface, etc] terminal window during
 // startup. Implementation of an idea by Prof Braino to inform user that any changes made to this
 // build by the user have been successfully uploaded into firmware.
 #define STRING_VERSION "1.0.3 dev"
-#define STRING_URL "reprap.org"
 #define STRING_VERSION_CONFIG_H __DATE__ " " __TIME__ // build date and time
 #define STRING_CONFIG_H_AUTHOR "(K8200, CONSULitAS)" // Who made the changes.
 #define STRING_SPLASH_LINE1 "v" STRING_VERSION // will be shown during bootup in line 1
 //#define STRING_SPLASH_LINE2 STRING_VERSION_CONFIG_H // will be shown during bootup in line2
 
+// @section machine
+
 // SERIAL_PORT selects which serial port should be used for communication with the host.
 // This allows the connection of wireless adapters (for instance) to non-default port pins.
 // Serial port 0 is still used by the Arduino bootloader regardless of this setting.
+// :[0,1,2,3,4,5,6,7]
 #define SERIAL_PORT 0
 
 // This determines the communication speed of the printer
+// :[2400,9600,19200,38400,57600,115200,250000]
 #define BAUDRATE 250000
 
 // This enables the serial port associated to the Bluetooth interface
@@ -62,25 +72,36 @@ Here are some standard links for getting your machine calibrated:
   #define MOTHERBOARD BOARD_K8200
 #endif
 
-// Define this to set a custom name for your generic Mendel,
-// #define CUSTOM_MENDEL_NAME "This Mendel"
+// Optional custom name for your RepStrap or other custom machine
+// Displayed in the LCD "Ready" message
+// #define CUSTOM_MACHINE_NAME "3D Printer"
 
 // Define this to set a unique identifier for this printer, (Used by some programs to differentiate between machines)
 // You can use an online service to generate a random UUID. (eg http://www.uuidgenerator.net/version4)
 // #define MACHINE_UUID "00000000-0000-0000-0000-000000000000"
 
 // This defines the number of extruders
+// :[1,2,3,4]
 #define EXTRUDERS 1
 
+// Offset of the extruders (uncomment if using more than one and relying on firmware to position when changing).
+// The offset has to be X=0, Y=0 for the extruder 0 hotend (default extruder).
+// For the other hotends it is their distance from the extruder 0 hotend.
+//#define EXTRUDER_OFFSET_X {0.0, 20.00} // (in mm) for each extruder, offset of the hotend on the X axis
+//#define EXTRUDER_OFFSET_Y {0.0, 5.00}  // (in mm) for each extruder, offset of the hotend on the Y axis
+
 //// The following define selects which power supply you have. Please choose the one that matches your setup
 // 1 = ATX
 // 2 = X-Box 360 203Watts (the blue wire connected to PS_ON and the red wire to VCC)
+// :{1:'ATX',2:'X-Box 360'}
 
 #define POWER_SUPPLY 1
 
 // Define this to have the electronics keep the power supply off on startup. If you don't know what this is leave it.
 // #define PS_DEFAULT_OFF
 
+// @section temperature
+
 //===========================================================================
 //============================= Thermal Settings ============================
 //===========================================================================
@@ -244,7 +265,7 @@ Here are some standard links for getting your machine calibrated:
 //    #define  DEFAULT_bedKi 1.41
 //    #define  DEFAULT_bedKd 1675.16
 
-//Vellemann K8200 PCB heatbed with standard PCU - calculated with PID Autotune and tested
+//Vellemann K8200 PCB heatbed with standard PCU at 60 degreesC - calculated with PID Autotune and tested
 //from pidautotune
    #define  DEFAULT_bedKp 341.88
    #define  DEFAULT_bedKi 25.32
@@ -253,6 +274,7 @@ Here are some standard links for getting your machine calibrated:
    // FIND YOUR OWN: "M303 E-1 C8 S90" to run autotune on the bed at 90 degreesC for 8 cycles.
 #endif // PIDTEMPBED
 
+// @section extruder
 
 //this prevents dangerous Extruder moves, i.e. if the temperature is under the limit
 //can be software-disabled for whatever purposes by
@@ -307,12 +329,16 @@ your extruder heater takes 2 minutes to hit the target on heating.
 //============================= Mechanical Settings =========================
 //===========================================================================
 
+// @section machine
+
 // Uncomment this option to enable CoreXY kinematics
 // #define COREXY
 
 // Enable this option for Toshiba steppers
 // #define CONFIG_STEPPERS_TOSHIBA
 
+// @section homing
+
 // coarse Endstop Settings
 #define ENDSTOPPULLUPS // Comment this out (using // at the start of the line) to disable the endstop pullup resistors
 
@@ -324,6 +350,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).
@@ -333,33 +360,55 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 const bool X_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop.
 const bool Y_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop.
 const bool Z_MAX_ENDSTOP_INVERTING = true; // 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
 
+// @section machine
+// 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
+// :{0:'Low',1:'High'}
 #define X_ENABLE_ON 0
 #define Y_ENABLE_ON 0
 #define Z_ENABLE_ON 0
 #define E_ENABLE_ON 0 // For all extruders
 
 // Disables axis when it's not being used.
+// WARNING: When motors turn off there is a chance of losing position accuracy!
 #define DISABLE_X false
 #define DISABLE_Y false
 #define DISABLE_Z true
+
+// @section extruder
+
 #define DISABLE_E false // For all extruders
 #define DISABLE_INACTIVE_EXTRUDER true //disable only inactive extruders and keep active extruder enabled
 
+// @section machine
+
 // Invert the stepper direction. Change (or reverse the motor connector) if an axis goes the wrong way.
 #define INVERT_X_DIR false
 #define INVERT_Y_DIR false
 #define INVERT_Z_DIR false
-#define INVERT_E0_DIR true
+
+// @section extruder
+
+// For direct drive extruder v9 set to true, for geared extruder set to false.
+#define INVERT_E0_DIR true // K8200: true for geared default extruder!
 #define INVERT_E1_DIR true
 #define INVERT_E2_DIR true
 #define INVERT_E3_DIR true
 
+// @section homing
+
 // ENDSTOP SETTINGS:
 // Sets direction of endstops when homing; 1=MAX, -1=MIN
+// :[-1,1]
 #define X_HOME_DIR -1
 #define Y_HOME_DIR -1
 #define Z_HOME_DIR -1
@@ -367,6 +416,8 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 #define min_software_endstops true // If true, axis won't move to coordinates less than HOME_POS.
 #define max_software_endstops true  // If true, axis won't move to coordinates greater than the defined lengths below.
 
+// @section machine
+
 // Travel limits after homing (units are in mm)
 #define X_MIN_POS 0
 #define Y_MIN_POS 0
@@ -386,14 +437,14 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined.
 
 //===========================================================================
-//============================ Manual Bed Leveling ==========================
+//============================ Mesh Bed Leveling ============================
 //===========================================================================
 
 // #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
 // #define MESH_BED_LEVELING    // Enable mesh bed leveling
 
 #ifdef MANUAL_BED_LEVELING
-  #define MBL_Z_STEP 0.025
+  #define MBL_Z_STEP 0.025  // Step size while manually probing Z axis
 #endif  // MANUAL_BED_LEVELING
 
 #ifdef MESH_BED_LEVELING
@@ -410,6 +461,8 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //============================= Bed Auto Leveling ===========================
 //===========================================================================
 
+// @section bedlevel
+
 //#define ENABLE_AUTO_BED_LEVELING // Delete the comment to enable (remove // at the start of the line)
 #define Z_PROBE_REPEATABILITY_TEST  // If not commented out, Z-Probe Repeatability test will be included if Auto Bed Leveling is Enabled.
 
@@ -518,6 +571,8 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 #endif // ENABLE_AUTO_BED_LEVELING
 
 
+// @section homing
+
 // The position of the homing switches
 //#define MANUAL_HOME_POSITIONS  // If defined, MANUAL_*_HOME_POS below will be used
 //#define BED_CENTER_AT_0_0  // If defined, the center of the bed is at (X=0, Y=0)
@@ -531,6 +586,8 @@ 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
 
+// @section movement
+
 /**
  * MOVEMENT SETTINGS
  */
@@ -547,13 +604,6 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 #define DEFAULT_RETRACT_ACCELERATION  1000   // E acceleration in mm/s^2 for retracts
 #define DEFAULT_TRAVEL_ACCELERATION   1000    // X, Y, Z acceleration in mm/s^2 for travel (non printing) moves
 
-
-// Offset of the extruders (uncomment if using more than one and relying on firmware to position when changing).
-// The offset has to be X=0, Y=0 for the extruder 0 hotend (default extruder).
-// For the other hotends it is their distance from the extruder 0 hotend.
-// #define EXTRUDER_OFFSET_X {0.0, 20.00} // (in mm) for each extruder, offset of the hotend on the X axis
-// #define EXTRUDER_OFFSET_Y {0.0, 5.00}  // (in mm) for each extruder, offset of the hotend on the Y axis
-
 // The speed change that does not require acceleration (i.e. the software might assume it can be done instantaneously)
 #define DEFAULT_XYJERK                20.0    // (mm/sec)
 #define DEFAULT_ZJERK                 0.4     // (mm/sec)
@@ -564,6 +614,8 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //============================= Additional Features ===========================
 //=============================================================================
 
+// @section more
+
 // Custom M code points
 #define CUSTOM_M_CODES
 #ifdef CUSTOM_M_CODES
@@ -574,6 +626,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
   #endif
 #endif
 
+// @section extras
 
 // EEPROM
 // The microcontroller can store settings in the EEPROM, e.g. max velocity...
@@ -588,26 +641,30 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
   #define EEPROM_CHITCHAT // please keep turned on if you can.
 #endif
 
+
+// @section temperature
+
 // Preheat Constants
 #define PLA_PREHEAT_HOTEND_TEMP 190
-#define PLA_PREHEAT_HPB_TEMP 50
+#define PLA_PREHEAT_HPB_TEMP 50 // K8200: set back to 70 if you have an upgraded heatbed power supply
 #define PLA_PREHEAT_FAN_SPEED 0   // Insert Value between 0 and 255
 
 #define ABS_PREHEAT_HOTEND_TEMP 240
-#define ABS_PREHEAT_HPB_TEMP 60
+#define ABS_PREHEAT_HPB_TEMP 60 // K8200: set back to 110 if you have an upgraded heatbed power supply
 #define ABS_PREHEAT_FAN_SPEED 0   // Insert Value between 0 and 255
 
 //==============================LCD and SD support=============================
+// @section lcd
 
 // Define your display language below. Replace (en) with your language code and uncomment.
-// en, pl, fr, de, es, ru, bg, it, pt, pt-br, fi, an, nl, ca, eu, kana, kana_utf8, test
+// en, pl, fr, de, es, ru, bg, it, pt, pt-br, fi, an, nl, ca, eu, kana, kana_utf8, cn, test
 // See also language.h
-//#define LANGUAGE_INCLUDE GENERATE_LANGUAGE_INCLUDE(en)
+#define LANGUAGE_INCLUDE GENERATE_LANGUAGE_INCLUDE(en)
 
 // Choose ONE of these 3 charsets. This has to match your hardware. Ignored for full graphic display.
 // To find out what type you have - compile with (test) - upload - click to get the menu. You'll see two typical lines from the upper half of the charset.
 // See also documentation/LCDLanguageFont.md
-  #define DISPLAY_CHARSET_HD44780_JAPAN        // this is the most common hardware
+  #define DISPLAY_CHARSET_HD44780_JAPAN        // K8200: for Display VM8201 // this is the most common hardware
   //#define DISPLAY_CHARSET_HD44780_WESTERN
   //#define DISPLAY_CHARSET_HD44780_CYRILLIC
 
@@ -618,11 +675,11 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //#define SD_CHECK_AND_RETRY // Use CRC checks and retries on the SD communication
 //#define ENCODER_PULSES_PER_STEP 1 // Increase if you have a high resolution encoder
 //#define ENCODER_STEPS_PER_MENU_ITEM 5 // Set according to ENCODER_PULSES_PER_STEP or your liking
-#define ULTIMAKERCONTROLLER //as available from the Ultimaker online store.
+#define ULTIMAKERCONTROLLER // K8200: for Display VM8201 // as available from the Ultimaker online store.
 //#define ULTIPANEL  //the UltiPanel as on Thingiverse
-//#define LCD_FEEDBACK_FREQUENCY_HZ 1000	// this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
 //#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click
-                                               // 0 to disable buzzer feedback  
+//#define LCD_FEEDBACK_FREQUENCY_HZ 1000         // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
+                                                 // 0 to disable buzzer feedback. Test with M300 S<frequency Hz> P<duration ms>
 
 // PanelOne from T3P3 (via RAMPS 1.4 AUX2/AUX3)
 // http://reprap.org/wiki/PanelOne
@@ -681,6 +738,8 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 
 //#define SAV_3DLCD
 
+// @section extras
+
 // Increase the FAN pwm frequency. Removes the PWM noise but increases heating in the FET/Arduino
 //#define FAST_PWM_FAN
 
@@ -704,7 +763,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 // 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.
@@ -758,7 +817,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 #define MAX_MEASUREMENT_DELAY			20  //delay buffer size in bytes (1 byte = 1cm)- limits maximum measurement delay allowable (must be larger than MEASUREMENT_DELAY_CM  and lower number saves RAM)
 
 //defines used in the code
-#define DEFAULT_MEASURED_FILAMENT_DIA  DEFAULT_NOMINAL_FILAMENT_DIA  //set measured to nominal initially 
+#define DEFAULT_MEASURED_FILAMENT_DIA  DEFAULT_NOMINAL_FILAMENT_DIA  //set measured to nominal initially
 
 //When using an LCD, uncomment the line below to display the Filament sensor data on the last line instead of status.  Status will appear for 5 sec.
 //#define FILAMENT_LCD_DISPLAY
diff --git a/Marlin/example_configurations/K8200/Configuration_adv.h b/Marlin/example_configurations/K8200/Configuration_adv.h
index a80790b95782ef31d5416af8c495c074b791729a..672b3ff028fc6af40ccf23327d17deadf3208210 100644
--- a/Marlin/example_configurations/K8200/Configuration_adv.h
+++ b/Marlin/example_configurations/K8200/Configuration_adv.h
@@ -195,6 +195,9 @@
 #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.
 
+// When G28 is called, this option will make Y home before X
+// #define HOME_Y_BEFORE_X
+
 // @section machine
 
 #define AXIS_RELATIVE_MODES {false, false, false, false}
@@ -258,7 +261,6 @@
 #define ENCODER_RATE_MULTIPLIER         // If defined, certain menu edit operations automatically multiply the steps when the encoder is moved quickly
 #define ENCODER_10X_STEPS_PER_SEC 75    // If the encoder steps per sec exceeds this value, multiply steps moved x10 to quickly advance the value
 #define ENCODER_100X_STEPS_PER_SEC 160  // If the encoder steps per sec exceeds this value, multiply steps moved x100 to really quickly advance the value
-//#define ENCODER_RATE_MULTIPLIER_DEBUG  // If defined, output the encoder steps per second value
 
 //#define CHDK 4        //Pin for triggering CHDK to take a picture see how to use it here http://captain-slow.dk/2014/03/09/3d-printing-timelapses/
 #define CHDK_DELAY 50 //How long in ms the pin should stay HIGH before going LOW again
@@ -355,7 +357,7 @@ const unsigned int dropsegments=5; //everything with less than this number of st
 //#define HEATERS_PARALLEL
 
 //===========================================================================
-//=============================Buffers           ============================
+//================================= Buffers =================================
 //===========================================================================
 
 // @section hidden
diff --git a/Marlin/example_configurations/K8200/readme.md b/Marlin/example_configurations/K8200/readme.md
index 79cb6e039e18dd0fcf6b49b144196a7a35f14f8c..bb29f51c1d47f1320ae8087311d36e9bc7143919 100644
--- a/Marlin/example_configurations/K8200/readme.md
+++ b/Marlin/example_configurations/K8200/readme.md
@@ -1,13 +1,20 @@
-# Example Configuration for Vellemann K8200
-* Configuration files for **Vellemann K8200** (with VM8201 - LCD Option for K8200)
+# Example Configuration for Vellemann [K8200](http://www.k8200.eu/)
+* Configuration files for **Vellemann K8200** (with [VM8201](http://www.vellemanprojects.eu/products/view/?id=416158) - LCD Option for K8200)
 * K8200 is a 3Drag clone - configuration should work with 3Drag http://reprap.org/wiki/3drag, too. Please report.
 
-* updated manually with parameters form genuine Vellemann Firmware "firmware_k8200_marlinv2" based on the recent development branch
+* updated manually with parameters from genuine Vellemann Firmware "firmware_k8200_marlinv2" based on the recent development branch
 
 * VM8201 uses "DISPLAY_CHARSET_HD44870_JAPAN" and "ULTIMAKERCONTROLLER"
 * german (de) translation with umlaut is supported now - thanks to @AnHardt for the great hardware based umlaut support
 
-I (@CONSULitAS) tested the changes on my K8200 with 20x4-LCD and Arduino 1.0.5 for Windows (SD library added to IDE manually) - everything works well.
+I [@CONSULitAS](https://github.com/CONSULitAS) tested the changes on my K8200 with 20x4-LCD and Arduino 1.6.1 for Windows (SD library added to IDE manually) - everything works well.
 
-**Source for genuine Vellemann Firmware V2 (with LCD/SD-Support):**
-* [firmware_k8200_marlinv2.zip](http://www.k8200.eu/downloads/files/downloads/firmware_k8200_marlinv2.zip)
\ No newline at end of file
+**Source for genuine [Vellemann Firmware](http://www.k8200.eu/support/downloads/)**
+* V2.1.1 (for z axis upgrade, date branched: 2013-06-05): [firmware_k8200_v2.1.1.zip](http://www.k8200.eu/downloads/files/downloads/firmware_k8200_v2.1.1.zip)
+  * see also https://github.com/CONSULitAS/Marlin-K8200/tree/Vellemann_firmware_k8200_v2.1.1.zip
+
+* V2 (with LCD/SD-Support, date branched: 2013-06-05): [firmware_k8200_marlinv2.zip](http://www.k8200.eu/downloads/files/downloads/firmware_k8200_marlinv2.zip)
+  * see also https://github.com/CONSULitAS/Marlin-K8200/tree/Vellemann_firmware_k8200_marlinv2.zip
+
+* V1 (without LCD/SD-Support, date branched: 2012-10-02): [firmware_k8200_marlinv1.zip](http://www.k8200.eu/downloads/files/downloads/firmware_k8200_marlinv1.zip)
+  * see also https://github.com/CONSULitAS/Marlin-K8200/tree/Vellemann_firmware_k8200_marlinv1.zip
diff --git a/Marlin/example_configurations/SCARA/Configuration.h b/Marlin/example_configurations/SCARA/Configuration.h
index a4812f648a1d6a5cd7043f6a70520977820ea8fe..f988eb7c69426284ab461ec67b33dfc79f599975 100644
--- a/Marlin/example_configurations/SCARA/Configuration.h
+++ b/Marlin/example_configurations/SCARA/Configuration.h
@@ -1,4 +1,4 @@
-#ifndef CONFIGURATION_H
+#ifndef CONFIGURATION_H
 #define CONFIGURATION_H
 
 #include "boards.h"
@@ -57,7 +57,6 @@ Here are some standard links for getting your machine calibrated:
 // startup. Implementation of an idea by Prof Braino to inform user that any changes made to this
 // build by the user have been successfully uploaded into firmware.
 #define STRING_VERSION "1.0.3 dev"
-#define STRING_URL "reprap.org"
 #define STRING_VERSION_CONFIG_H __DATE__ " " __TIME__ // build date and time
 #define STRING_CONFIG_H_AUTHOR "(none, default config)" // Who made the changes.
 #define STRING_SPLASH_LINE1 "v" STRING_VERSION // will be shown during bootup in line 1
@@ -80,8 +79,9 @@ Here are some standard links for getting your machine calibrated:
   #define MOTHERBOARD BOARD_RAMPS_13_EFB
 #endif
 
-// Define this to set a custom name for your generic Mendel,
-// #define CUSTOM_MENDEL_NAME "This Mendel"
+// Optional custom name for your RepStrap or other custom machine
+// Displayed in the LCD "Ready" message
+// #define CUSTOM_MACHINE_NAME "3D Printer"
 
 // Define this to set a unique identifier for this printer, (Used by some programs to differentiate between machines)
 // You can use an online service to generate a random UUID. (eg http://www.uuidgenerator.net/version4)
@@ -357,6 +357,7 @@ const bool Z_MIN_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 const bool X_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop.
 const bool Y_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop.
 const bool Z_MAX_ENDSTOP_INVERTING = true; // 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
 
@@ -367,6 +368,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 #define E_ENABLE_ON 0 // For all extruders
 
 // Disables axis when it's not being used.
+// WARNING: When motors turn off there is a chance of losing position accuracy!
 #define DISABLE_X false
 #define DISABLE_Y false
 #define DISABLE_Z false
@@ -623,7 +625,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //==============================LCD and SD support=============================
 
 // Define your display language below. Replace (en) with your language code and uncomment.
-// en, pl, fr, de, es, ru, bg, it, pt, pt-br, fi, an, nl, ca, eu, kana, kana_utf8, test
+// en, pl, fr, de, es, ru, bg, it, pt, pt-br, fi, an, nl, ca, eu, kana, kana_utf8, cn, test
 // See also language.h
 //#define LANGUAGE_INCLUDE GENERATE_LANGUAGE_INCLUDE(en)
 
@@ -643,9 +645,9 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //#define ENCODER_STEPS_PER_MENU_ITEM 5 // Set according to ENCODER_PULSES_PER_STEP or your liking
 //#define ULTIMAKERCONTROLLER //as available from the Ultimaker online store.
 //#define ULTIPANEL  //the UltiPanel as on Thingiverse
-//#define LCD_FEEDBACK_FREQUENCY_HZ 1000	// this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
 //#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click
-                                               // 0 to disable buzzer feedback  
+//#define LCD_FEEDBACK_FREQUENCY_HZ 1000         // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
+                                                 // 0 to disable buzzer feedback. Test with M300 S<frequency Hz> P<duration ms>
 
 // PanelOne from T3P3 (via RAMPS 1.4 AUX2/AUX3)
 // http://reprap.org/wiki/PanelOne
diff --git a/Marlin/example_configurations/SCARA/Configuration_adv.h b/Marlin/example_configurations/SCARA/Configuration_adv.h
index edc2445af6ef7e3dfb671242d66bb06c2cf37adf..a706696aad9ba7ca049f136c64968db9e8e56827 100644
--- a/Marlin/example_configurations/SCARA/Configuration_adv.h
+++ b/Marlin/example_configurations/SCARA/Configuration_adv.h
@@ -195,6 +195,9 @@
 #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.
 
+// When G28 is called, this option will make Y home before X
+// #define HOME_Y_BEFORE_X
+
 // @section machine
 
 #define AXIS_RELATIVE_MODES {false, false, false, false}
@@ -258,7 +261,6 @@
 #define ENCODER_RATE_MULTIPLIER         // If defined, certain menu edit operations automatically multiply the steps when the encoder is moved quickly
 #define ENCODER_10X_STEPS_PER_SEC 75    // If the encoder steps per sec exceeds this value, multiply steps moved x10 to quickly advance the value
 #define ENCODER_100X_STEPS_PER_SEC 160  // If the encoder steps per sec exceeds this value, multiply steps moved x100 to really quickly advance the value
-//#define ENCODER_RATE_MULTIPLIER_DEBUG  // If defined, output the encoder steps per second value
 
 //#define CHDK 4        //Pin for triggering CHDK to take a picture see how to use it here http://captain-slow.dk/2014/03/09/3d-printing-timelapses/
 #define CHDK_DELAY 50 //How long in ms the pin should stay HIGH before going LOW again
@@ -355,7 +357,7 @@ const unsigned int dropsegments=5; //everything with less than this number of st
 //#define HEATERS_PARALLEL
 
 //===========================================================================
-//=============================Buffers           ============================
+//================================= Buffers =================================
 //===========================================================================
 
 // @section hidden
diff --git a/Marlin/example_configurations/WITBOX/Configuration.h b/Marlin/example_configurations/WITBOX/Configuration.h
index fe198852495aa737802cd1c74907e2955f0a660b..308a05a6e4569bc371c6997a48b675a1784e6eb8 100644
--- a/Marlin/example_configurations/WITBOX/Configuration.h
+++ b/Marlin/example_configurations/WITBOX/Configuration.h
@@ -1,4 +1,4 @@
-#ifndef CONFIGURATION_H
+#ifndef CONFIGURATION_H
 #define CONFIGURATION_H
 
 #include "boards.h"
@@ -39,7 +39,6 @@ Here are some standard links for getting your machine calibrated:
 // startup. Implementation of an idea by Prof Braino to inform user that any changes made to this
 // build by the user have been successfully uploaded into firmware.
 #define STRING_VERSION "1.0.3 dev"
-#define STRING_URL "reprap.org"
 #define STRING_VERSION_CONFIG_H __DATE__ " " __TIME__ // build date and time
 #define STRING_CONFIG_H_AUTHOR "(bq Witbox)" // Who made the changes.
 #define STRING_SPLASH_LINE1 "v" STRING_VERSION // will be shown during bootup in line 1
@@ -58,12 +57,11 @@ Here are some standard links for getting your machine calibrated:
 
 // The following define selects which electronics board you have.
 // Please choose the name from boards.h that matches your setup
-#ifndef MOTHERBOARD
-  #define MOTHERBOARD BOARD_WITBOX
-#endif
+#define MOTHERBOARD BOARD_WITBOX
 
-// Define this to set a custom name for your generic Mendel,
-// #define CUSTOM_MENDEL_NAME "This Mendel"
+// Optional custom name for your RepStrap or other custom machine
+// Displayed in the LCD "Ready" message
+// #define CUSTOM_MACHINE_NAME "3D Printer"
 
 // Define this to set a unique identifier for this printer, (Used by some programs to differentiate between machines)
 // You can use an online service to generate a random UUID. (eg http://www.uuidgenerator.net/version4)
@@ -327,6 +325,7 @@ const bool Z_MIN_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 const bool X_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop.
 const bool Y_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop.
 const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop.
+const bool Z_PROBE_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop.
 //#define DISABLE_MAX_ENDSTOPS
 //#define DISABLE_MIN_ENDSTOPS
 
@@ -337,6 +336,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 #define E_ENABLE_ON 0 // For all extruders
 
 // Disables axis when it's not being used.
+// WARNING: When motors turn off there is a chance of losing position accuracy!
 #define DISABLE_X false
 #define DISABLE_Y false
 #define DISABLE_Z true
@@ -593,7 +593,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //==============================LCD and SD support=============================
 
 // Define your display language below. Replace (en) with your language code and uncomment.
-// en, pl, fr, de, es, ru, bg, it, pt, pt-br, fi, an, nl, ca, eu, kana, kana_utf8, test
+// en, pl, fr, de, es, ru, bg, it, pt, pt-br, fi, an, nl, ca, eu, kana, kana_utf8, cn, test
 // See also language.h
 //#define LANGUAGE_INCLUDE GENERATE_LANGUAGE_INCLUDE(en)
 
@@ -613,9 +613,9 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //#define ENCODER_STEPS_PER_MENU_ITEM 5 // Set according to ENCODER_PULSES_PER_STEP or your liking
 //#define ULTIMAKERCONTROLLER //as available from the Ultimaker online store.
 //#define ULTIPANEL  //the UltiPanel as on Thingiverse
-//#define LCD_FEEDBACK_FREQUENCY_HZ 1000	// this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
 //#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click
-                                               // 0 to disable buzzer feedback  
+//#define LCD_FEEDBACK_FREQUENCY_HZ 1000         // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
+                                                 // 0 to disable buzzer feedback. Test with M300 S<frequency Hz> P<duration ms>
 
 // PanelOne from T3P3 (via RAMPS 1.4 AUX2/AUX3)
 // http://reprap.org/wiki/PanelOne
diff --git a/Marlin/example_configurations/WITBOX/Configuration_adv.h b/Marlin/example_configurations/WITBOX/Configuration_adv.h
index 15c52328487ae7a8356da2222792e0a614dd96f8..25abd17c2ceba2509c3dca6a59ac1ed049df17e0 100644
--- a/Marlin/example_configurations/WITBOX/Configuration_adv.h
+++ b/Marlin/example_configurations/WITBOX/Configuration_adv.h
@@ -195,6 +195,9 @@
 #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.
 
+// When G28 is called, this option will make Y home before X
+// #define HOME_Y_BEFORE_X
+
 // @section machine
 
 #define AXIS_RELATIVE_MODES {false, false, false, false}
@@ -258,7 +261,6 @@
 #define ENCODER_RATE_MULTIPLIER         // If defined, certain menu edit operations automatically multiply the steps when the encoder is moved quickly
 #define ENCODER_10X_STEPS_PER_SEC 75    // If the encoder steps per sec exceeds this value, multiply steps moved x10 to quickly advance the value
 #define ENCODER_100X_STEPS_PER_SEC 160  // If the encoder steps per sec exceeds this value, multiply steps moved x100 to really quickly advance the value
-//#define ENCODER_RATE_MULTIPLIER_DEBUG  // If defined, output the encoder steps per second value
 
 //#define CHDK 4        //Pin for triggering CHDK to take a picture see how to use it here http://captain-slow.dk/2014/03/09/3d-printing-timelapses/
 #define CHDK_DELAY 50 //How long in ms the pin should stay HIGH before going LOW again
@@ -355,7 +357,7 @@ const unsigned int dropsegments=5; //everything with less than this number of st
 //#define HEATERS_PARALLEL
 
 //===========================================================================
-//=============================Buffers           ============================
+//================================= Buffers =================================
 //===========================================================================
 
 // @section hidden
diff --git a/Marlin/example_configurations/delta/generic/Configuration.h b/Marlin/example_configurations/delta/generic/Configuration.h
index d7162c151b6d3006847d2b6b08035650d972d191..edf8a13f1e4f681346d7fbb8f35b29d073c45ec5 100644
--- a/Marlin/example_configurations/delta/generic/Configuration.h
+++ b/Marlin/example_configurations/delta/generic/Configuration.h
@@ -39,7 +39,6 @@ Here are some standard links for getting your machine calibrated:
 // startup. Implementation of an idea by Prof Braino to inform user that any changes made to this
 // build by the user have been successfully uploaded into firmware.
 #define STRING_VERSION "1.0.3 dev"
-#define STRING_URL "reprap.org"
 #define STRING_VERSION_CONFIG_H __DATE__ " " __TIME__ // build date and time
 #define STRING_CONFIG_H_AUTHOR "(none, default config)" // Who made the changes.
 #define STRING_SPLASH_LINE1 "v" STRING_VERSION // will be shown during bootup in line 1
@@ -62,8 +61,9 @@ Here are some standard links for getting your machine calibrated:
   #define MOTHERBOARD BOARD_RAMPS_13_EFB
 #endif
 
-// Define this to set a custom name for your generic Mendel,
-// #define CUSTOM_MENDEL_NAME "This Mendel"
+// Optional custom name for your RepStrap or other custom machine
+// Displayed in the LCD "Ready" message
+#define CUSTOM_MACHINE_NAME "Deltabot"
 
 // Define this to set a unique identifier for this printer, (Used by some programs to differentiate between machines)
 // You can use an online service to generate a random UUID. (eg http://www.uuidgenerator.net/version4)
@@ -355,6 +355,7 @@ const bool Z_MIN_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 const bool X_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop.
 const bool Y_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop.
 const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop.
+const bool Z_PROBE_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop.
 //#define DISABLE_MAX_ENDSTOPS
 #define DISABLE_MIN_ENDSTOPS // Deltas only use min endstops for probing
 
@@ -365,6 +366,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 #define E_ENABLE_ON 0 // For all extruders
 
 // Disables axis when it's not being used.
+// WARNING: When motors turn off there is a chance of losing position accuracy!
 #define DISABLE_X false
 #define DISABLE_Y false
 #define DISABLE_Z false
@@ -640,7 +642,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //==============================LCD and SD support=============================
 
 // Define your display language below. Replace (en) with your language code and uncomment.
-// en, pl, fr, de, es, ru, bg, it, pt, pt-br, fi, an, nl, ca, eu, kana, kana_utf8, test
+// en, pl, fr, de, es, ru, bg, it, pt, pt-br, fi, an, nl, ca, eu, kana, kana_utf8, cn, test
 // See also language.h
 #define LANGUAGE_INCLUDE GENERATE_LANGUAGE_INCLUDE(en)
 
@@ -660,9 +662,9 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //#define ENCODER_STEPS_PER_MENU_ITEM 5 // Set according to ENCODER_PULSES_PER_STEP or your liking
 //#define ULTIMAKERCONTROLLER //as available from the Ultimaker online store.
 //#define ULTIPANEL  //the UltiPanel as on Thingiverse
-//#define LCD_FEEDBACK_FREQUENCY_HZ 1000	// this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
 //#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click
-                                               // 0 to disable buzzer feedback  
+//#define LCD_FEEDBACK_FREQUENCY_HZ 1000         // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
+                                                 // 0 to disable buzzer feedback. Test with M300 S<frequency Hz> P<duration ms>
 
 // PanelOne from T3P3 (via RAMPS 1.4 AUX2/AUX3)
 // http://reprap.org/wiki/PanelOne
diff --git a/Marlin/example_configurations/delta/generic/Configuration_adv.h b/Marlin/example_configurations/delta/generic/Configuration_adv.h
index 88b76c66e18232f71bcc5169c6264f934143490c..cdfa7c8f935fd19363cbf268675e973279fd6cce 100644
--- a/Marlin/example_configurations/delta/generic/Configuration_adv.h
+++ b/Marlin/example_configurations/delta/generic/Configuration_adv.h
@@ -195,6 +195,9 @@
 #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.
 
+// When G28 is called, this option will make Y home before X
+// #define HOME_Y_BEFORE_X
+
 // @section machine
 
 #define AXIS_RELATIVE_MODES {false, false, false, false}
@@ -259,7 +262,6 @@
 #define ENCODER_RATE_MULTIPLIER         // If defined, certain menu edit operations automatically multiply the steps when the encoder is moved quickly
 #define ENCODER_10X_STEPS_PER_SEC 75    // If the encoder steps per sec exceeds this value, multiply steps moved x10 to quickly advance the value
 #define ENCODER_100X_STEPS_PER_SEC 160  // If the encoder steps per sec exceeds this value, multiply steps moved x100 to really quickly advance the value
-//#define ENCODER_RATE_MULTIPLIER_DEBUG  // If defined, output the encoder steps per second value
 
 //#define CHDK 4        //Pin for triggering CHDK to take a picture see how to use it here http://captain-slow.dk/2014/03/09/3d-printing-timelapses/
 #define CHDK_DELAY 50 //How long in ms the pin should stay HIGH before going LOW again
@@ -356,7 +358,7 @@ const unsigned int dropsegments=5; //everything with less than this number of st
 //#define HEATERS_PARALLEL
 
 //===========================================================================
-//=============================Buffers           ============================
+//================================= Buffers =================================
 //===========================================================================
 
 // @section hidden
diff --git a/Marlin/example_configurations/delta/kossel_mini/Configuration.h b/Marlin/example_configurations/delta/kossel_mini/Configuration.h
index 657a653cec9c61f5c05ebecaa7d8e1250f6f02cc..2cc7d4c6630900d3c27f476665357fc57454fec2 100644
--- a/Marlin/example_configurations/delta/kossel_mini/Configuration.h
+++ b/Marlin/example_configurations/delta/kossel_mini/Configuration.h
@@ -39,7 +39,6 @@ Here are some standard links for getting your machine calibrated:
 // startup. Implementation of an idea by Prof Braino to inform user that any changes made to this
 // build by the user have been successfully uploaded into firmware.
 #define STRING_VERSION "1.0.3 dev"
-#define STRING_URL "reprap.org"
 #define STRING_VERSION_CONFIG_H __DATE__ " " __TIME__ // build date and time
 #define STRING_CONFIG_H_AUTHOR "(none, default config)" // Who made the changes.
 #define STRING_SPLASH_LINE1 "v" STRING_VERSION // will be shown during bootup in line 1
@@ -62,8 +61,9 @@ Here are some standard links for getting your machine calibrated:
   #define MOTHERBOARD BOARD_RAMPS_13_EFB
 #endif
 
-// Define this to set a custom name for your generic Mendel,
-#define CUSTOM_MENDEL_NAME "Mini Kossel"
+// Optional custom name for your RepStrap or other custom machine
+// Displayed in the LCD "Ready" message
+#define CUSTOM_MACHINE_NAME "Mini Kossel"
 
 // Define this to set a unique identifier for this printer, (Used by some programs to differentiate between machines)
 // You can use an online service to generate a random UUID. (eg http://www.uuidgenerator.net/version4)
@@ -355,6 +355,7 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 const bool X_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic of the endstop.
 const bool Y_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic of the endstop.
 const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic of the endstop.
+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 // Deltas only use min endstops for probing
 
@@ -365,6 +366,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 #define E_ENABLE_ON 0 // For all extruders
 
 // Disables axis when it's not being used.
+// WARNING: When motors turn off there is a chance of losing position accuracy!
 #define DISABLE_X false
 #define DISABLE_Y false
 #define DISABLE_Z false
@@ -644,7 +646,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 //==============================LCD and SD support=============================
 
 // Define your display language below. Replace (en) with your language code and uncomment.
-// en, pl, fr, de, es, ru, bg, it, pt, pt-br, fi, an, nl, ca, eu, kana, kana_utf8, test
+// en, pl, fr, de, es, ru, bg, it, pt, pt-br, fi, an, nl, ca, eu, kana, kana_utf8, cn, test
 // See also language.h
 #define LANGUAGE_INCLUDE GENERATE_LANGUAGE_INCLUDE(en)
 
@@ -664,9 +666,9 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 //#define ENCODER_STEPS_PER_MENU_ITEM 5 // Set according to ENCODER_PULSES_PER_STEP or your liking
 //#define ULTIMAKERCONTROLLER //as available from the Ultimaker online store.
 //#define ULTIPANEL  //the UltiPanel as on Thingiverse
-//#define LCD_FEEDBACK_FREQUENCY_HZ 1000	// this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
 //#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click
-                                               // 0 to disable buzzer feedback  
+//#define LCD_FEEDBACK_FREQUENCY_HZ 1000         // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
+                                                 // 0 to disable buzzer feedback. Test with M300 S<frequency Hz> P<duration ms>
 
 // PanelOne from T3P3 (via RAMPS 1.4 AUX2/AUX3)
 // http://reprap.org/wiki/PanelOne
diff --git a/Marlin/example_configurations/delta/kossel_mini/Configuration_adv.h b/Marlin/example_configurations/delta/kossel_mini/Configuration_adv.h
index 9b52ede7b7591d14d134ead9f9306388574c7669..943022c819a9b1c59606ff4d62b00d10b9c4c588 100644
--- a/Marlin/example_configurations/delta/kossel_mini/Configuration_adv.h
+++ b/Marlin/example_configurations/delta/kossel_mini/Configuration_adv.h
@@ -195,6 +195,9 @@
 #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.
 
+// When G28 is called, this option will make Y home before X
+// #define HOME_Y_BEFORE_X
+
 // @section machine
 
 #define AXIS_RELATIVE_MODES {false, false, false, false}
@@ -258,7 +261,6 @@
 #define ENCODER_RATE_MULTIPLIER         // If defined, certain menu edit operations automatically multiply the steps when the encoder is moved quickly
 #define ENCODER_10X_STEPS_PER_SEC 75    // If the encoder steps per sec exceeds this value, multiply steps moved x10 to quickly advance the value
 #define ENCODER_100X_STEPS_PER_SEC 160  // If the encoder steps per sec exceeds this value, multiply steps moved x100 to really quickly advance the value
-//#define ENCODER_RATE_MULTIPLIER_DEBUG  // If defined, output the encoder steps per second value
 
 //#define CHDK 4        //Pin for triggering CHDK to take a picture see how to use it here http://captain-slow.dk/2014/03/09/3d-printing-timelapses/
 #define CHDK_DELAY 50 //How long in ms the pin should stay HIGH before going LOW again
@@ -355,7 +357,7 @@ const unsigned int dropsegments=5; //everything with less than this number of st
 //#define HEATERS_PARALLEL
 
 //===========================================================================
-//=============================Buffers           ============================
+//================================= Buffers =================================
 //===========================================================================
 
 // @section hidden
diff --git a/Marlin/example_configurations/makibox/Configuration.h b/Marlin/example_configurations/makibox/Configuration.h
index 857731fa2fd034fa7b8dda4d244c8b34389f2d3c..7223febd2893cc158dfb6298177c54851b99e131 100644
--- a/Marlin/example_configurations/makibox/Configuration.h
+++ b/Marlin/example_configurations/makibox/Configuration.h
@@ -1,4 +1,4 @@
-#ifndef CONFIGURATION_H
+#ifndef CONFIGURATION_H
 #define CONFIGURATION_H
 
 #include "boards.h"
@@ -39,7 +39,6 @@ Here are some standard links for getting your machine calibrated:
 // startup. Implementation of an idea by Prof Braino to inform user that any changes made to this
 // build by the user have been successfully uploaded into firmware.
 #define STRING_VERSION "1.0.3 dev"
-#define STRING_URL "reprap.org"
 #define STRING_VERSION_CONFIG_H __DATE__ " " __TIME__ // build date and time
 #define STRING_CONFIG_H_AUTHOR "(none, default config)" // Who made the changes.
 #define STRING_SPLASH_LINE1 "v" STRING_VERSION // will be shown during bootup in line 1
@@ -62,8 +61,9 @@ Here are some standard links for getting your machine calibrated:
   #define MOTHERBOARD BOARD_5DPRINT
 #endif
 
-// Define this to set a custom name for your generic Mendel,
-// #define CUSTOM_MENDEL_NAME "This Mendel"
+// Optional custom name for your RepStrap or other custom machine
+// Displayed in the LCD "Ready" message
+// #define CUSTOM_MACHINE_NAME "3D Printer"
 
 // Define this to set a unique identifier for this printer, (Used by some programs to differentiate between machines)
 // You can use an online service to generate a random UUID. (eg http://www.uuidgenerator.net/version4)
@@ -325,6 +325,7 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 const bool X_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop.
 const bool Y_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop.
 const bool Z_MAX_ENDSTOP_INVERTING = true; // 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
 
@@ -335,6 +336,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 #define E_ENABLE_ON 0 // For all extruders
 
 // Disables axis when it's not being used.
+// WARNING: When motors turn off there is a chance of losing position accuracy!
 #define DISABLE_X false
 #define DISABLE_Y false
 #define DISABLE_Z false
@@ -591,7 +593,8 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //==============================LCD and SD support=============================
 
 // Define your display language below. Replace (en) with your language code and uncomment.
-// en, pl, fr, de, es, ru, bg, it, pt, pt-br, fi, an, nl, ca, eu, kana, kana_utf8, test
+// en, pl, fr, de, es, ru, bg, it, pt, pt-br, fi, an, nl, ca, eu, kana, kana_utf8, cn, test
+
 // See also language.h
 //#define LANGUAGE_INCLUDE GENERATE_LANGUAGE_INCLUDE(en)
 
@@ -611,9 +614,9 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //#define ENCODER_STEPS_PER_MENU_ITEM 5 // Set according to ENCODER_PULSES_PER_STEP or your liking
 //#define ULTIMAKERCONTROLLER //as available from the Ultimaker online store.
 //#define ULTIPANEL  //the UltiPanel as on Thingiverse
-//#define LCD_FEEDBACK_FREQUENCY_HZ 1000	// this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
 //#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click
-                                               // 0 to disable buzzer feedback  
+//#define LCD_FEEDBACK_FREQUENCY_HZ 1000         // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
+                                                 // 0 to disable buzzer feedback. Test with M300 S<frequency Hz> P<duration ms>
 
 // PanelOne from T3P3 (via RAMPS 1.4 AUX2/AUX3)
 // http://reprap.org/wiki/PanelOne
diff --git a/Marlin/example_configurations/makibox/Configuration_adv.h b/Marlin/example_configurations/makibox/Configuration_adv.h
index 7b63732a69db33c2b12f290d7b02ec1a65e6ebc9..8d451fd28277f3323850d6a41668d1c963686af3 100644
--- a/Marlin/example_configurations/makibox/Configuration_adv.h
+++ b/Marlin/example_configurations/makibox/Configuration_adv.h
@@ -195,6 +195,9 @@
 #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.
 
+// When G28 is called, this option will make Y home before X
+// #define HOME_Y_BEFORE_X
+
 // @section machine
 
 #define AXIS_RELATIVE_MODES {false, false, false, false}
@@ -258,7 +261,6 @@
 #define ENCODER_RATE_MULTIPLIER         // If defined, certain menu edit operations automatically multiply the steps when the encoder is moved quickly
 #define ENCODER_10X_STEPS_PER_SEC 75    // If the encoder steps per sec exceeds this value, multiply steps moved x10 to quickly advance the value
 #define ENCODER_100X_STEPS_PER_SEC 160  // If the encoder steps per sec exceeds this value, multiply steps moved x100 to really quickly advance the value
-//#define ENCODER_RATE_MULTIPLIER_DEBUG  // If defined, output the encoder steps per second value
 
 //#define CHDK 4        //Pin for triggering CHDK to take a picture see how to use it here http://captain-slow.dk/2014/03/09/3d-printing-timelapses/
 #define CHDK_DELAY 50 //How long in ms the pin should stay HIGH before going LOW again
@@ -355,7 +357,7 @@ const unsigned int dropsegments=5; //everything with less than this number of st
 //#define HEATERS_PARALLEL
 
 //===========================================================================
-//=============================Buffers           ============================
+//================================= Buffers =================================
 //===========================================================================
 
 // @section hidden
diff --git a/Marlin/example_configurations/tvrrug/Round2/Configuration.h b/Marlin/example_configurations/tvrrug/Round2/Configuration.h
index 0ef31bc8e6bc5f0944a8245b2b06bbd8db42cce5..e7602a6d0bb90d92a9315faae081251ac2811f7f 100644
--- a/Marlin/example_configurations/tvrrug/Round2/Configuration.h
+++ b/Marlin/example_configurations/tvrrug/Round2/Configuration.h
@@ -1,4 +1,4 @@
-#ifndef CONFIGURATION_H
+#ifndef CONFIGURATION_H
 #define CONFIGURATION_H
 
 #include "boards.h"
@@ -39,7 +39,6 @@ Here are some standard links for getting your machine calibrated:
 // startup. Implementation of an idea by Prof Braino to inform user that any changes made to this
 // build by the user have been successfully uploaded into firmware.
 #define STRING_VERSION "1.0.3 dev"
-#define STRING_URL "reprap.org"
 #define STRING_VERSION_CONFIG_H __DATE__ " " __TIME__ // build date and time
 #define STRING_CONFIG_H_AUTHOR "(none, default config)" // Who made the changes.
 #define STRING_SPLASH_LINE1 "v" STRING_VERSION // will be shown during bootup in line 1
@@ -62,8 +61,9 @@ Here are some standard links for getting your machine calibrated:
   #define MOTHERBOARD BOARD_OMCA
 #endif
 
-// Define this to set a custom name for your generic Mendel,
-// #define CUSTOM_MENDEL_NAME "This Mendel"
+// Optional custom name for your RepStrap or other custom machine
+// Displayed in the LCD "Ready" message
+// #define CUSTOM_MACHINE_NAME "3D Printer"
 
 // Define this to set a unique identifier for this printer, (Used by some programs to differentiate between machines)
 // You can use an online service to generate a random UUID. (eg http://www.uuidgenerator.net/version4)
@@ -327,6 +327,7 @@ const bool Z_MIN_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 const bool X_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop.
 const bool Y_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop.
 const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop.
+const bool Z_PROBE_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop.
 //#define DISABLE_MAX_ENDSTOPS
 //#define DISABLE_MIN_ENDSTOPS
 
@@ -337,6 +338,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 #define E_ENABLE_ON 1 // For all extruders
 
 // Disables axis when it's not being used.
+// WARNING: When motors turn off there is a chance of losing position accuracy!
 #define DISABLE_X false
 #define DISABLE_Y false
 #define DISABLE_Z false
@@ -597,7 +599,8 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //==============================LCD and SD support=============================
 
 // Define your display language below. Replace (en) with your language code and uncomment.
-// en, pl, fr, de, es, ru, bg, it, pt, pt-br, fi, an, nl, ca, eu, kana, kana_utf8, test
+// en, pl, fr, de, es, ru, bg, it, pt, pt-br, fi, an, nl, ca, eu, kana, kana_utf8, cn, test
+
 // See also language.h
 //#define LANGUAGE_INCLUDE GENERATE_LANGUAGE_INCLUDE(en)
 
@@ -617,9 +620,9 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //#define ENCODER_STEPS_PER_MENU_ITEM 5 // Set according to ENCODER_PULSES_PER_STEP or your liking
 //#define ULTIMAKERCONTROLLER //as available from the Ultimaker online store.
 //#define ULTIPANEL  //the UltiPanel as on Thingiverse
-//#define LCD_FEEDBACK_FREQUENCY_HZ 1000	// this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
 //#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click
-                                               // 0 to disable buzzer feedback  
+//#define LCD_FEEDBACK_FREQUENCY_HZ 1000         // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
+                                                 // 0 to disable buzzer feedback. Test with M300 S<frequency Hz> P<duration ms>
 
 // PanelOne from T3P3 (via RAMPS 1.4 AUX2/AUX3)
 // http://reprap.org/wiki/PanelOne
diff --git a/Marlin/example_configurations/tvrrug/Round2/Configuration_adv.h b/Marlin/example_configurations/tvrrug/Round2/Configuration_adv.h
index d221f080b4ac225457f949df56ac40e2d7807d4c..3187150ba9b7421d546cf2aa507415dfd9a75350 100644
--- a/Marlin/example_configurations/tvrrug/Round2/Configuration_adv.h
+++ b/Marlin/example_configurations/tvrrug/Round2/Configuration_adv.h
@@ -195,6 +195,9 @@
 #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.
 
+// When G28 is called, this option will make Y home before X
+// #define HOME_Y_BEFORE_X
+
 // @section machine
 
 #define AXIS_RELATIVE_MODES {false, false, false, false}
@@ -258,7 +261,6 @@
 #define ENCODER_RATE_MULTIPLIER         // If defined, certain menu edit operations automatically multiply the steps when the encoder is moved quickly
 #define ENCODER_10X_STEPS_PER_SEC 75    // If the encoder steps per sec exceeds this value, multiply steps moved x10 to quickly advance the value
 #define ENCODER_100X_STEPS_PER_SEC 160  // If the encoder steps per sec exceeds this value, multiply steps moved x100 to really quickly advance the value
-//#define ENCODER_RATE_MULTIPLIER_DEBUG  // If defined, output the encoder steps per second value
 
 //#define CHDK 4        //Pin for triggering CHDK to take a picture see how to use it here http://captain-slow.dk/2014/03/09/3d-printing-timelapses/
 #define CHDK_DELAY 50 //How long in ms the pin should stay HIGH before going LOW again
@@ -355,7 +357,7 @@ const unsigned int dropsegments=5; //everything with less than this number of st
 //#define HEATERS_PARALLEL
 
 //===========================================================================
-//=============================Buffers           ============================
+//================================= Buffers =================================
 //===========================================================================
 
 // @section hidden
diff --git a/Marlin/fonts/ISO10646_CN.fon b/Marlin/fonts/ISO10646_CN.fon
new file mode 100644
index 0000000000000000000000000000000000000000..6c77eeccd8dea480f2759b0b987b777c34b48083
Binary files /dev/null and b/Marlin/fonts/ISO10646_CN.fon differ
diff --git a/Marlin/fonts/README.fonts b/Marlin/fonts/README.fonts
index f227c079ae49963b0bfe7c8eee13a7919cb10785..f7d41fa982acb26ba6e99ef27fe8441d4e4b6127 100644
--- a/Marlin/fonts/README.fonts
+++ b/Marlin/fonts/README.fonts
@@ -3,4 +3,24 @@ In Fony export the fonts to bdf-format. Maybe another one can edit them with Fon
 Then run make_fonts.bat what calls bdf2u8g.exe with the needed parameters to produce the .h files.
 The .h files must be edited to replace '#include "u8g.h"' with '#include <utility/u8g.h>', replace 'U8G_FONT_SECTION' with 'U8G_SECTION', insert '.progmem.' right behind the first '"' and moved to the main directory.
 
-Especially the Kana and Cyrillic fonts should be revised by someone who knows what he/she does. I am only a west-European with very little knowledge about this scripts.
+How to integrate a new font:
+Currently we are limited to 256 symbols per font. We use a menu system with 5 lines, on a display with 64 pixel height. That means we have 12 pixel for a line. To have some space in between the lines we can't use more then 10 pixel height for the symbols. For up to 11 pixel set TALL_FONT_CORRECTION 1 when loading the font.
+To fit 22 Symbols on the 128 pixel wide screen, the symbols can't be wider than 5 pixel, for the first 128 symbols.
+For the second half of the font we now support up to 11x11 pixel. 
+
+ * Get 'Fony.exe'
+ * Copy one of the existing *.fon files and work with this.
+ * Change the pixels. Don't change width or height.
+ * Export as *.bdf
+ * Use 'bdf2u8g.exe' to produce the *.h file. Examples for the existing fonts are in 'make_fonts.bat'
+ * Edit the produced .h file to match our needs. See hints in 'README.fonts' or the other 'dogm_font_data_.h' files.
+ * Make a new entry in the font list in 'dogm_lcd_implementation.h' before the '#else // fall back'
+    #elif defined( DISPLAY_CHARSET_NEWNAME )
+      #include "dogm_font_data_yourfont.h"
+      #define FONT_MENU_NAME YOURFONTNAME
+    #else // fall-back
+ * Add your font to the list of permitted fonts in 'language_en.h'
+    ... || defined(DISPLAY_CHARSET_YOUR_NEW_FONT) ... )
+
+
+Especially the Kana font should be revised by someone who knows what he/she does. I am only a west-European with very little knowledge about this script.
diff --git a/Marlin/fonts/make_fonts.bat b/Marlin/fonts/make_fonts.bat
index ca50def7137c91988a93307b1282e0c1a412882b..5ab975bf47b14e8c1b5275cc0f7770f9c57c674a 100644
--- a/Marlin/fonts/make_fonts.bat
+++ b/Marlin/fonts/make_fonts.bat
@@ -5,3 +5,4 @@
 .\bdf2u8g.exe -b 32 -e 255 ISO10646-1.bdf ISO10646_1_5x7 dogm_font_data_ISO10646_1.h
 .\bdf2u8g.exe -b 32 -e 255 ISO10646_5_Cyrillic.bdf ISO10646_5_Cyrillic_5x7 dogm_font_data_ISO10646_5_Cyrillic.h
 .\bdf2u8g.exe -b 32 -e 255 ISO10646_Kana.bdf ISO10646_Kana_5x7 dogm_font_data_ISO10646_Kana.h
+.\bdf2u8g.exe -b 32 -e 255 ISO10646_CN.bdf ISO10646_CN dogm_font_data_ISO10646_CN.h
diff --git a/Marlin/language.h b/Marlin/language.h
index f352e4bb70a1cf7657a55a351a16460eab35ed37..acd773e9a74ebcd4f0484a635fa1847e203c0eb2 100644
--- a/Marlin/language.h
+++ b/Marlin/language.h
@@ -31,6 +31,7 @@
 // eu       Basque-Euskera
 // kana     Japanese
 // kana_utf Japanese
+// cn       Chinese
 
 #ifndef LANGUAGE_INCLUDE
   // pick your language from the list above
@@ -71,12 +72,12 @@
   #endif
 #else
   #ifndef MACHINE_NAME
-    #define MACHINE_NAME "Mendel"
+    #define MACHINE_NAME "3D Printer"
   #endif
 #endif
 
 #ifdef CUSTOM_MENDEL_NAME
-  #warning CUSTOM_MENDEL_NAME deprecated - use CUSTOM_MACHINE_NAME
+  #error CUSTOM_MENDEL_NAME deprecated - use CUSTOM_MACHINE_NAME
   #define CUSTOM_MACHINE_NAME CUSTOM_MENDEL_NAME
 #endif
 
@@ -111,7 +112,7 @@
 
 // Serial Console Messages (do not translate those!)
 
-#define MSG_Enqueing                        "enqueing \""
+#define MSG_Enqueueing                      "enqueueing \""
 #define MSG_POWERUP                         "PowerUp"
 #define MSG_EXTERNAL_RESET                  " External Reset"
 #define MSG_BROWNOUT_RESET                  " Brown out Reset"
@@ -123,7 +124,8 @@
 #define MSG_PLANNER_BUFFER_BYTES            "  PlannerBufferBytes: "
 #define MSG_OK                              "ok"
 #define MSG_FILE_SAVED                      "Done saving file."
-#define MSG_ERR_LINE_NO                     "Line Number is not Last Line Number+1, Last Line: "
+#define MSG_ERR_LINE_NO1                    "Line Number out of sequence. Expected: "
+#define MSG_ERR_LINE_NO2                    " Got: "
 #define MSG_ERR_CHECKSUM_MISMATCH           "checksum mismatch, Last Line: "
 #define MSG_ERR_NO_CHECKSUM                 "No Checksum with line number, Last Line: "
 #define MSG_ERR_NO_LINENUMBER_WITH_CHECKSUM "No Line Number with checksum, Last Line: "
@@ -158,6 +160,9 @@
 #define MSG_Z_MAX                           "z_max: "
 #define MSG_Z2_MAX                          "z2_max: "
 #define MSG_Z_PROBE                         "z_probe: "
+#define MSG_ERR_MATERIAL_INDEX              "M145 S<index> out of range (0-1)"
+#define MSG_ERR_M421_REQUIRES_XYZ           "M421 requires XYZ parameters"
+#define MSG_ERR_MESH_INDEX_OOB              "Mesh XY index is out of bounds"
 #define MSG_M119_REPORT                     "Reporting endstop status"
 #define MSG_ENDSTOP_HIT                     "TRIGGERED"
 #define MSG_ENDSTOP_OPEN                    "open"
@@ -210,7 +215,7 @@
 #define MSG_OK_B                            "ok B:"
 #define MSG_OK_T                            "ok T:"
 #define MSG_AT                              " @:"
-#define MSG_PID_AUTOTUNE_FINISHED           MSG_PID_AUTOTUNE " finished! Put the last Kp, Ki and Kd constants from above into Configuration.h"
+#define MSG_PID_AUTOTUNE_FINISHED           MSG_PID_AUTOTUNE " finished! Put the last Kp, Ki and Kd constants from below into Configuration.h"
 #define MSG_PID_DEBUG                       " PID_DEBUG "
 #define MSG_PID_DEBUG_INPUT                 ": Input "
 #define MSG_PID_DEBUG_OUTPUT                " Output "
diff --git a/Marlin/language_cn.h b/Marlin/language_cn.h
new file mode 100644
index 0000000000000000000000000000000000000000..25ab251f8425f070481064b9320ba1ecb10e7871
--- /dev/null
+++ b/Marlin/language_cn.h
@@ -0,0 +1,159 @@
+/**
+ * Chinese
+ *
+ * LCD Menu Messages
+ * Se also documentation/LCDLanguageFont.md
+ *
+ */
+#ifndef LANGUAGE_CN_H
+#define LANGUAGE_CN_H
+
+  #define MAPPER_NON         // For direct asci codes
+  #define DISPLAY_CHARSET_ISO10646_CN
+
+  #define WELCOME_MSG                         "\xa4\xa5\xa6\xa7"
+  #define MSG_SD_INSERTED                     "\xa8\xa9\xaa\xab"
+  #define MSG_SD_REMOVED                      "\xa8\xa9\xac\xad"
+  #define MSG_MAIN                            "\xae\xaf\xb0"
+  #define MSG_AUTOSTART                       "\xb1\xb2\xb3\xb4"
+  #define MSG_DISABLE_STEPPERS                "\xb5\xb6\xb7\xb8\xb9\xba"
+  #define MSG_AUTO_HOME                       "\xbb\xbc\xbd"
+  #define MSG_SET_HOME_OFFSETS                "\xbe\xbf\xbb\xbc\xbd\xc0\xc1"
+  #define MSG_SET_ORIGIN                      "\xbe\xbf\xbc\xbd"
+  #define MSG_PREHEAT_PLA                     "\xc3\xc4 PLA"
+  #define MSG_PREHEAT_PLA_N                   MSG_PREHEAT_PLA " "
+  #define MSG_PREHEAT_PLA_ALL                 MSG_PREHEAT_PLA " \xc5\xc6"
+  #define MSG_PREHEAT_PLA_BEDONLY             MSG_PREHEAT_PLA " \xc4\xc7"
+  #define MSG_PREHEAT_PLA_SETTINGS            MSG_PREHEAT_PLA " \xbe\xbf"
+  #define MSG_PREHEAT_ABS                     "\xc3\xc4 ABS"
+  #define MSG_PREHEAT_ABS_N                   MSG_PREHEAT_ABS " "
+  #define MSG_PREHEAT_ABS_ALL                 MSG_PREHEAT_ABS " \xc5\xc6"
+  #define MSG_PREHEAT_ABS_BEDONLY             MSG_PREHEAT_ABS " \xbe\xc6"
+  #define MSG_PREHEAT_ABS_SETTINGS            MSG_PREHEAT_ABS " \xbe\xbf"
+  #define MSG_H1                              "1"
+  #define MSG_H2                              "2"
+  #define MSG_H3                              "3"
+  #define MSG_H4                              "4"
+  #define MSG_COOLDOWN                        "\xc8\xc9"
+  #define MSG_SWITCH_PS_ON                    "\xb9\xcb\xca\xb3"
+  #define MSG_SWITCH_PS_OFF                   "\xb9\xcb\xb5\xb6"
+  #define MSG_EXTRUDE                         "\xcc\xad"
+  #define MSG_RETRACT                         "\xbb\xcd"
+  #define MSG_MOVE_AXIS                       "\xc1\xb2\xce"
+  #define MSG_LEVEL_BED                       "\xcf\xe0\xc4\xc7"
+  #define MSG_MOVE_X                          "\xc1\xb2 X"
+  #define MSG_MOVE_Y                          "\xc1\xb2 Y"
+  #define MSG_MOVE_Z                          "\xc1\xb2 Z"
+  #define MSG_MOVE_E                          "\xcc\xad\xba"
+  #define MSG_MOVE_01MM                       "\xc1\xb2 0.1mm"
+  #define MSG_MOVE_1MM                        "\xc1\xb2 1mm"
+  #define MSG_MOVE_10MM                       "\xc1\xb2 10mm"
+  #define MSG_SPEED                           "\xd1\xd2"
+  #define MSG_NOZZLE                          "\xd3\xd4"
+  #define MSG_N2                              " 2"
+  #define MSG_N3                              " 3"
+  #define MSG_N4                              " 4"
+  #define MSG_BED                             "\xc4\xc7"
+  #define MSG_FAN_SPEED                       "\xd5\xd6\xd1\xd2"
+  #define MSG_FLOW                            "\xcc\xad\xd1\xd2"
+  #define MSG_F0                              " 0"
+  #define MSG_F1                              " 1"
+  #define MSG_F2                              " 2"
+  #define MSG_F3                              " 3"
+  #define MSG_CONTROL                         "\xd8\xd9"
+  #define MSG_MIN                             LCD_STR_THERMOMETER " \xda\xdb"
+  #define MSG_MAX                             LCD_STR_THERMOMETER " \xda\xdc"
+  #define MSG_FACTOR                          LCD_STR_THERMOMETER " \xdd\xde"
+  #define MSG_AUTOTEMP                        "\xb1\xb2\xd8\xc9"
+  #define MSG_ON                              "\xb3 "  // intentional space to shift wide symbol to the left
+  #define MSG_OFF                             "\xb5 "  // intentional space to shift wide symbol to the left
+  #define MSG_PID_P                           "PID-P"
+  #define MSG_PID_I                           "PID-I"
+  #define MSG_PID_D                           "PID-D"
+  #define MSG_PID_C                           "PID-C"
+  #define MSG_E2                              " E2"
+  #define MSG_E3                              " E3"
+  #define MSG_E4                              " E4"
+  #define MSG_ACC                             "Accel"
+  #define MSG_VXY_JERK                        "Vxy-jerk"
+  #define MSG_VZ_JERK                         "Vz-jerk"
+  #define MSG_VE_JERK                         "Ve-jerk"
+  #define MSG_VMAX                            "Vmax "
+  #define MSG_X                               "x"
+  #define MSG_Y                               "y"
+  #define MSG_Z                               "z"
+  #define MSG_E                               "e"
+  #define MSG_VMIN                            "Vmin"
+  #define MSG_VTRAV_MIN                       "VTrav min"
+  #define MSG_AMAX                            "Amax "
+  #define MSG_A_RETRACT                       "A-retract"
+  #define MSG_A_TRAVEL                        "A-travel"
+  #define MSG_XSTEPS                          "Xsteps/mm"
+  #define MSG_YSTEPS                          "Ysteps/mm"
+  #define MSG_ZSTEPS                          "Zsteps/mm"
+  #define MSG_ESTEPS                          "Esteps/mm"
+  #define MSG_TEMPERATURE                     "\xc9\xd2"
+  #define MSG_MOTION                          "\xdf\xb2"
+  #define MSG_VOLUMETRIC                      "Filament"
+  #define MSG_VOLUMETRIC_ENABLED              "E in mm3"
+  #define MSG_FILAMENT_SIZE_EXTRUDER_0        "Fil. Dia. 1"
+  #define MSG_FILAMENT_SIZE_EXTRUDER_1        "Fil. Dia. 2"
+  #define MSG_FILAMENT_SIZE_EXTRUDER_2        "Fil. Dia. 3"
+  #define MSG_FILAMENT_SIZE_EXTRUDER_3        "Fil. Dia. 4"
+  #define MSG_CONTRAST                        "LCD contrast"
+  #define MSG_STORE_EPROM                     "Store memory"
+  #define MSG_LOAD_EPROM                      "Load memory"
+  #define MSG_RESTORE_FAILSAFE                "Restore failsafe"
+  #define MSG_REFRESH                         "Refresh"
+  #define MSG_WATCH                           "\xec\xed\xee\xef"
+  #define MSG_PREPARE                         "\xa4\xa5"
+  #define MSG_TUNE                            "\xcf\xf0"
+  #define MSG_PAUSE_PRINT                     "\xf1\xf2\xca\xf3"
+  #define MSG_RESUME_PRINT                    "\xf4\xf5\xca\xf3"
+  #define MSG_STOP_PRINT                      "\xf2\xf6\xca\xf3"
+  #define MSG_CARD_MENU                       "\xaf\xb0"
+  #define MSG_NO_CARD                         "\xf9\xa8"
+  #define MSG_DWELL                           "Sleep..."
+  #define MSG_USERWAIT                        "Wait for user..."
+  #define MSG_RESUMING                        "Resuming print"
+  #define MSG_PRINT_ABORTED                   "Print aborted"
+  #define MSG_NO_MOVE                         "No move."
+  #define MSG_KILLED                          "KILLED. "
+  #define MSG_STOPPED                         "STOPPED. "
+  #define MSG_CONTROL_RETRACT                 "Retract mm"
+  #define MSG_CONTROL_RETRACT_SWAP            "Swap Re.mm"
+  #define MSG_CONTROL_RETRACTF                "Retract  V"
+  #define MSG_CONTROL_RETRACT_ZLIFT           "Hop mm"
+  #define MSG_CONTROL_RETRACT_RECOVER         "UnRet +mm"
+  #define MSG_CONTROL_RETRACT_RECOVER_SWAP    "S UnRet+mm"
+  #define MSG_CONTROL_RETRACT_RECOVERF        "UnRet  V"
+  #define MSG_AUTORETRACT                     "AutoRetr."
+  #define MSG_FILAMENTCHANGE                  "Change filament"
+  #define MSG_INIT_SDCARD                     "Init. SD card"
+  #define MSG_CNG_SDCARD                      "Change SD card"
+  #define MSG_ZPROBE_OUT                      "Z probe out. bed"
+  #define MSG_POSITION_UNKNOWN                "Home X/Y before Z"
+  #define MSG_ZPROBE_ZOFFSET                  "Z Offset"
+  #define MSG_BABYSTEP_X                      "Babystep X"
+  #define MSG_BABYSTEP_Y                      "Babystep Y"
+  #define MSG_BABYSTEP_Z                      "Babystep Z"
+  #define MSG_ENDSTOP_ABORT                   "Endstop abort"
+  #define MSG_HEATING_FAILED_LCD              "Heating failed"
+  #define MSG_ERR_REDUNDANT_TEMP              "Err: REDUNDANT TEMP ERROR"
+  #define MSG_THERMAL_RUNAWAY                 "THERMAL RUNAWAY"
+  #define MSG_ERR_MAXTEMP                     "Err: MAXTEMP"
+  #define MSG_ERR_MINTEMP                     "Err: MINTEMP"
+  #define MSG_ERR_MAXTEMP_BED                 "Err: MAXTEMP BED"
+  #define MSG_END_HOUR                        "hours"
+  #define MSG_END_MINUTE                      "minutes"
+
+  #ifdef DELTA_CALIBRATION_MENU
+    #define MSG_DELTA_CALIBRATE             "Delta Calibration"
+    #define MSG_DELTA_CALIBRATE_X           "Calibrate X"
+    #define MSG_DELTA_CALIBRATE_Y           "Calibrate Y"
+    #define MSG_DELTA_CALIBRATE_Z           "Calibrate Z"
+    #define MSG_DELTA_CALIBRATE_CENTER      "Calibrate Center"
+  #endif // DELTA_CALIBRATION_MENU
+
+#endif // LANGUAGE_CN_H
+
diff --git a/Marlin/language_de.h b/Marlin/language_de.h
index 8c8496f02c125cd779e62f301f480e2db24fe053..57db43f2451d55ecd8ac131b19c89f37036b8689 100644
--- a/Marlin/language_de.h
+++ b/Marlin/language_de.h
@@ -13,7 +13,7 @@
 //#define SIMULATE_ROMFONT
 #define DISPLAY_CHARSET_ISO10646_1
 
-#define WELCOME_MSG                         MACHINE_NAME " Bereit."
+#define WELCOME_MSG                         MACHINE_NAME " bereit."
 #define MSG_SD_INSERTED                     "SDKarte erkannt."
 #define MSG_SD_REMOVED                      "SDKarte entfernt."
 #define MSG_MAIN                            "Hauptmenü"
@@ -61,9 +61,9 @@
 #define MSG_PID_D                           "PID D"
 #define MSG_PID_C                           "PID C"
 #define MSG_ACC                             "A"
-#define MSG_VXY_JERK                        "V xy jerk"
-#define MSG_VZ_JERK                         "V z  jerk"
-#define MSG_VE_JERK                         "V e  jerk"
+#define MSG_VXY_JERK                        "V xy Ruck"
+#define MSG_VZ_JERK                         "V z  Ruck"
+#define MSG_VE_JERK                         "V e  Ruck"
 #define MSG_VMAX                            "V max " // space by purpose
 #define MSG_X                               "x"
 #define MSG_Y                               "y"
@@ -84,7 +84,7 @@
 #define MSG_FILAMENT_SIZE_EXTRUDER_0        "Filament D 1"
 #define MSG_FILAMENT_SIZE_EXTRUDER_1        "Filament D 2"
 #define MSG_FILAMENT_SIZE_EXTRUDER_2        "Filament D 3"
-#define MSG_CONTRAST                        "LCD contrast"
+#define MSG_CONTRAST                        "LCD Kontrast"
 #define MSG_STORE_EPROM                     "EPROM speichern"
 #define MSG_LOAD_EPROM                      "EPROM laden"
 #define MSG_RESTORE_FAILSAFE                "Standardkonfig."
@@ -116,7 +116,7 @@
 #define MSG_INIT_SDCARD                     "SDKarte erkennen"// Manually initialize the SD-card via user interface
 #define MSG_CNG_SDCARD                      "SDKarte erkennen"// SD-card changed by user. For machines with no autocarddetect. Both send "M21"
 #define MSG_ZPROBE_OUT                      "Sensor ausserhalb"
-#define MSG_POSITION_UNKNOWN                "X/Y vor Z Homen."
+#define MSG_POSITION_UNKNOWN                "X/Y vor Z homen."
 #define MSG_ZPROBE_ZOFFSET                  "Z Offset"
 #define MSG_BABYSTEP_X                      "Babystep X"
 #define MSG_BABYSTEP_Y                      "Babystep Y"
@@ -126,7 +126,7 @@
 #define MSG_END_MINUTE                      "Minuten"
 
 #ifdef DELTA_CALIBRATION_MENU
-    #define MSG_DELTA_CALIBRATE             "Delta Kalibrieren"
+    #define MSG_DELTA_CALIBRATE             "Delta kalibrieren"
     #define MSG_DELTA_CALIBRATE_X           "Kalibriere X"
     #define MSG_DELTA_CALIBRATE_Y           "Kalibriere Y"
     #define MSG_DELTA_CALIBRATE_Z           "Kalibriere Z"
diff --git a/Marlin/language_en.h b/Marlin/language_en.h
index 9d61167cc7cd11c38c670c3054c1b2bd1313288c..94e0614748d25eaad5e9240dddf94edc0be3a304 100644
--- a/Marlin/language_en.h
+++ b/Marlin/language_en.h
@@ -13,7 +13,7 @@
 #endif
 
 //#define SIMULATE_ROMFONT //Comment in to see what is seen on the character based displays
-#if !( defined(SIMULATE_ROMFONT)|| defined(DISPLAY_CHARSET_ISO10646_1)|| defined(DISPLAY_CHARSET_ISO10646_5)|| defined(DISPLAY_CHARSET_ISO10646_KANA) )
+#if !( defined(SIMULATE_ROMFONT)|| defined(DISPLAY_CHARSET_ISO10646_1)|| defined(DISPLAY_CHARSET_ISO10646_5)|| defined(DISPLAY_CHARSET_ISO10646_KANA)|| defined(DISPLAY_CHARSET_ISO10646_CN) )
   #define DISPLAY_CHARSET_ISO10646_1 // use the better font on full graphic displays.
 #endif
 
diff --git a/Marlin/language_es.h b/Marlin/language_es.h
index 92f903cebc18fcbddfd07b5488eb25cd07ebd074..168dd0a658fb06e85d465c3e210add8966e37d08 100644
--- a/Marlin/language_es.h
+++ b/Marlin/language_es.h
@@ -17,7 +17,7 @@
 #define MSG_SD_INSERTED                     "Tarjeta colocada"
 #define MSG_SD_REMOVED                      "Tarjeta retirada"
 #define MSG_MAIN                            "Menu principal"
-#define MSG_AUTOSTART                       " Autostart"
+#define MSG_AUTOSTART                       "Autostart"
 #define MSG_DISABLE_STEPPERS                "Apagar motores"
 #define MSG_AUTO_HOME                       "Llevar al origen"
 #define MSG_SET_HOME_OFFSETS                "Ajustar offsets"
@@ -46,7 +46,7 @@
 #define MSG_MOVE_1MM                        "Mover 1mm"
 #define MSG_MOVE_10MM                       "Mover 10mm"
 #define MSG_SPEED                           "Velocidad"
-#define MSG_NOZZLE                          "Nozzle"
+#define MSG_NOZZLE                          "Fusor"
 #define MSG_BED                             "Base"
 #define MSG_FAN_SPEED                       "Ventilador"
 #define MSG_FLOW                            "Flujo"
@@ -80,7 +80,7 @@
 #define MSG_ESTEPS                          "E pasos/mm"
 #define MSG_TEMPERATURE                     "Temperatura"
 #define MSG_MOTION                          "Movimiento"
-#define MSG_VOLUMETRIC                      "Filament"
+#define MSG_VOLUMETRIC                      "Filamento"
 #define MSG_VOLUMETRIC_ENABLED		        "E in mm3"
 #define MSG_FILAMENT_SIZE_EXTRUDER_0        "Fil. Dia. 1"
 #define MSG_FILAMENT_SIZE_EXTRUDER_1        "Fil. Dia. 2"
@@ -116,22 +116,22 @@
 #define MSG_FILAMENTCHANGE                  "Cambiar filamento"
 #define MSG_INIT_SDCARD                     "Iniciando tarjeta"
 #define MSG_CNG_SDCARD                      "Cambiar tarjeta"
-#define MSG_ZPROBE_OUT                      "sonda Z fuera"
+#define MSG_ZPROBE_OUT                      "Sonda Z fuera"
 #define MSG_POSITION_UNKNOWN                "Reiniciar X/Y y Z"
 #define MSG_ZPROBE_ZOFFSET                  "Offset Z"
-#define MSG_BABYSTEP_X                      "Babystep X"
-#define MSG_BABYSTEP_Y                      "Babystep Y"
-#define MSG_BABYSTEP_Z                      "Babystep Z"
+#define MSG_BABYSTEP_X                      "Micropaso X"
+#define MSG_BABYSTEP_Y                      "Micropaso Y"
+#define MSG_BABYSTEP_Z                      "Micropaso Z"
 #define MSG_ENDSTOP_ABORT                   "Endstop abort"
 #define MSG_END_HOUR                        "horas"
 #define MSG_END_MINUTE                      "minutos"
 
 #ifdef DELTA_CALIBRATION_MENU
-    #define MSG_DELTA_CALIBRATE             "Delta Calibration"
-    #define MSG_DELTA_CALIBRATE_X           "Calibrate X"
-    #define MSG_DELTA_CALIBRATE_Y           "Calibrate Y"
-    #define MSG_DELTA_CALIBRATE_Z           "Calibrate Z"
-    #define MSG_DELTA_CALIBRATE_CENTER      "Calibrate Center"
+    #define MSG_DELTA_CALIBRATE             "Calibracion Delta"
+    #define MSG_DELTA_CALIBRATE_X           "Calibrar X"
+    #define MSG_DELTA_CALIBRATE_Y           "Calibrar Y"
+    #define MSG_DELTA_CALIBRATE_Z           "Calibrar Z"
+    #define MSG_DELTA_CALIBRATE_CENTER      "Calibrar Centro"
 #endif // DELTA_CALIBRATION_MENU
 
 #endif // LANGUAGE_ES_H
diff --git a/Marlin/pins.h b/Marlin/pins.h
index e5af0af1797dd6740a97ebd02cd0c46e8c372390..e45ba09dff0be1269ac3e07254ca7225dc123f5b 100644
--- a/Marlin/pins.h
+++ b/Marlin/pins.h
@@ -76,6 +76,8 @@
   #include "pins_PRINTRBOARD.h"
 #elif MB(BRAINWAVE)
   #include "pins_BRAINWAVE.h"
+#elif MB(BRAINWAVE_PRO)
+  #include "pins_BRAINWAVE_PRO.h"
 #elif MB(SAV_MKI)
   #include "pins_SAV_MKI.h"
 #elif MB(TEENSY2)
diff --git a/Marlin/pins_AZTEEG_X3.h b/Marlin/pins_AZTEEG_X3.h
index ea98f95c595b659e9cd0b80a994fdafe9e61e934..7c5dca3397e414e9e6675a260725227e236af373 100644
--- a/Marlin/pins_AZTEEG_X3.h
+++ b/Marlin/pins_AZTEEG_X3.h
@@ -28,9 +28,7 @@
    #define STAT_LED_RED 64
    #define STAT_LED_BLUE 63
   #endif
-#endif
-
-#elif define TEMP_STAT_LEDS
+#elif defined(TEMP_STAT_LEDS)
   #define STAT_LED_RED       6
   #define STAT_LED_BLUE     11
 #endif
diff --git a/Marlin/pins_BRAINWAVE_PRO.h b/Marlin/pins_BRAINWAVE_PRO.h
new file mode 100644
index 0000000000000000000000000000000000000000..374487046732a59a8b6f6ae4de738c7051ce888c
--- /dev/null
+++ b/Marlin/pins_BRAINWAVE_PRO.h
@@ -0,0 +1,63 @@
+/**
+ * Brainwave Pro pin assignments (AT90USB186)
+ *
+ * Requires hardware bundle for Arduino:
+ * https://github.com/unrepentantgeek/brainwave-arduino
+ */
+
+#ifndef __AVR_AT90USB1286__
+  #error Oops!  Make sure you have 'Brainwave Pro' selected from the 'Tools -> Boards' menu.
+#endif
+
+#ifndef AT90USBxx_TEENSYPP_ASSIGNMENTS  // use Teensyduino Teensy++2.0 pin assignments instead of Marlin alphabetical.
+  #error  Uncomment #define AT90USBxx_TEENSYPP_ASSIGNMENTS in fastio.h for this config
+#endif
+
+#define AT90USB 1286  // Disable MarlinSerial etc.
+
+#define X_STEP_PIN         33
+#define X_DIR_PIN          32
+#define X_ENABLE_PIN       11
+#define X_STOP_PIN         47
+
+#define Y_STEP_PIN         31
+#define Y_DIR_PIN          30
+#define Y_ENABLE_PIN        8
+#define Y_STOP_PIN         18
+
+#define Z_STEP_PIN         29
+#define Z_DIR_PIN          28
+#define Z_ENABLE_PIN       37
+#define Z_MAX_PIN          36
+#define Z_MIN_PIN          17  // Bed probe
+
+#define E0_STEP_PIN        35
+#define E0_DIR_PIN         34
+#define E0_ENABLE_PIN      13
+
+#define HEATER_0_PIN       15
+#define HEATER_1_PIN       -1
+#define HEATER_2_PIN       -1
+#define HEATER_BED_PIN     14  // Bed
+#define FAN_PIN            16  // Fan, PWM
+
+#define TEMP_0_PIN          2  // Extruder / Analog pin numbering
+#define TEMP_1_PIN          1  // Spare / Analog pin numbering
+#define TEMP_2_PIN         -1
+#define TEMP_BED_PIN        0  // Bed / Analog pin numbering
+
+#define SDPOWER            -1
+#define SDSS               20
+#define LED_PIN            19
+#define PS_ON_PIN          -1
+#define KILL_PIN           -1
+#define ALARM_PIN          -1
+#define SDCARDDETECT       12
+
+#ifndef SDSUPPORT
+// these pins are defined in the SD library if building with SD support
+  #define SCK_PIN          21
+  #define MISO_PIN         23
+  #define MOSI_PIN         22
+#endif
+
diff --git a/Marlin/pins_SAV_MKI.h b/Marlin/pins_SAV_MKI.h
index e898c9f28522a52a044e818c143172ff9193bdf2..e853b63b958ffa260c75e28a69257d78e010bea8 100644
--- a/Marlin/pins_SAV_MKI.h
+++ b/Marlin/pins_SAV_MKI.h
@@ -82,7 +82,7 @@
   #define HOME_PIN          -1 // A4 = marlin 44 - teensy = 42
 
   #ifdef NUM_SERVOS
-    #define SERVO0_PIN       41 // In teensy's pin definition for pinMode (in Servo.cpp)
+    #define SERVO0_PIN       41 // In teensy's pin definition for pinMode (in servo.cpp)
   #endif
 
 #endif // SAV_3DLCD
diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index 76ec037887a34de84848893d48581fd954f1f4c8..a8558d6c491dc85365720cb4c189c7f014412267 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -1,54 +1,51 @@
-/*
-  planner.c - buffers movement commands and manages the acceleration profile plan
- Part of Grbl
- 
- Copyright (c) 2009-2011 Simen Svale Skogsrud
- 
- Grbl 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.
- 
- Grbl 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 Grbl.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-/* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */
-
-/*  
- Reasoning behind the mathematics in this module (in the key of 'Mathematica'):
- 
- s == speed, a == acceleration, t == time, d == distance
- 
- Basic definitions:
- 
- Speed[s_, a_, t_] := s + (a*t) 
- Travel[s_, a_, t_] := Integrate[Speed[s, a, t], t]
- 
- Distance to reach a specific speed with a constant acceleration:
- 
- Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, d, t]
- d -> (m^2 - s^2)/(2 a) --> estimate_acceleration_distance()
- 
- Speed after a given distance of travel with constant acceleration:
- 
- Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, m, t]
- m -> Sqrt[2 a d + s^2]    
- 
- DestinationSpeed[s_, a_, d_] := Sqrt[2 a d + s^2]
- 
- When to start braking (di) to reach a specified destionation speed (s2) after accelerating
- from initial speed s1 without ever stopping at a plateau:
- 
- Solve[{DestinationSpeed[s1, a, di] == DestinationSpeed[s2, a, d - di]}, di]
- di -> (2 a d - s1^2 + s2^2)/(4 a) --> intersection_distance()
- 
- IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a)
+/**
+ * planner.cpp - Buffer movement commands and manage the acceleration profile plan
+ * Part of Grbl
+ * 
+ * Copyright (c) 2009-2011 Simen Svale Skogsrud
+ *
+ * Grbl 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.
+ *
+ * Grbl 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 Grbl.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *
+ * The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis.
+ *
+ *
+ * Reasoning behind the mathematics in this module (in the key of 'Mathematica'):
+ *
+ * s == speed, a == acceleration, t == time, d == distance
+ *
+ * Basic definitions:
+ *   Speed[s_, a_, t_] := s + (a*t)
+ *   Travel[s_, a_, t_] := Integrate[Speed[s, a, t], t]
+ *
+ * Distance to reach a specific speed with a constant acceleration:
+ *   Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, d, t]
+ *   d -> (m^2 - s^2)/(2 a) --> estimate_acceleration_distance()
+ *
+ * Speed after a given distance of travel with constant acceleration:
+ *   Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, m, t]
+ *   m -> Sqrt[2 a d + s^2]
+ *
+ * DestinationSpeed[s_, a_, d_] := Sqrt[2 a d + s^2]
+ *
+ * When to start braking (di) to reach a specified destination speed (s2) after accelerating
+ * from initial speed s1 without ever stopping at a plateau:
+ *   Solve[{DestinationSpeed[s1, a, di] == DestinationSpeed[s2, a, d - di]}, di]
+ *   di -> (2 a d - s1^2 + s2^2)/(4 a) --> intersection_distance()
+ *
+ * IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a)
+ *
  */
 
 #include "Marlin.h"
@@ -60,28 +57,28 @@
 
 #ifdef MESH_BED_LEVELING
   #include "mesh_bed_leveling.h"
-#endif  // MESH_BED_LEVELING
+#endif
 
 //===========================================================================
 //============================= public variables ============================
 //===========================================================================
 
-unsigned long minsegmenttime;
+millis_t minsegmenttime;
 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;
-float acceleration;         // Normal acceleration mm/s^2  THIS IS THE DEFAULT ACCELERATION for all printing moves. M204 SXXXX
-float retract_acceleration; //  mm/s^2   filament pull-pack and push-forward  while standing still in the other axis M204 TXXXX
-float travel_acceleration;  // Travel acceleration mm/s^2  THIS IS THE DEFAULT ACCELERATION for all NON printing moves. M204 MXXXX
-float max_xy_jerk; //speed than can be stopped at once, if i understand correctly.
+float acceleration;         // Normal acceleration mm/s^2  DEFAULT ACCELERATION for all printing moves. M204 SXXXX
+float retract_acceleration; // Retract acceleration mm/s^2 filament pull-back and push-forward while standing still in the other axes M204 TXXXX
+float travel_acceleration;  // Travel acceleration mm/s^2  DEFAULT ACCELERATION for all NON printing moves. M204 MXXXX
+float max_xy_jerk;          // The largest speed change requiring no acceleration
 float max_z_jerk;
 float max_e_jerk;
 float mintravelfeedrate;
 unsigned long axis_steps_per_sqr_second[NUM_AXIS];
 
 #ifdef ENABLE_AUTO_BED_LEVELING
-  // this holds the required transform to compensate for bed level
+  // Transform required to compensate for bed level
   matrix_3x3 plan_bed_level_matrix = {
     1.0, 0.0, 0.0,
     0.0, 1.0, 0.0,
@@ -89,11 +86,6 @@ unsigned long axis_steps_per_sqr_second[NUM_AXIS];
   };
 #endif // ENABLE_AUTO_BED_LEVELING
 
-// The current position of the tool in absolute steps
-long position[NUM_AXIS];   //rescaled from extern when axis_steps_per_unit are changed by gcode
-static float previous_speed[NUM_AXIS]; // Speed of previous path line segment
-static float previous_nominal_speed; // Nominal speed of previous path line segment
-
 #ifdef AUTOTEMP
   float autotemp_max = 250;
   float autotemp_min = 210;
@@ -101,21 +93,25 @@ static float previous_nominal_speed; // Nominal speed of previous path line segm
   bool autotemp_enabled = false;
 #endif
 
-unsigned char g_uc_extruder_last_move[4] = {0,0,0,0};
-
 //===========================================================================
-//=================semi-private variables, used in inline  functions    =====
+//============ semi-private variables, used in inline functions =============
 //===========================================================================
+
 block_t block_buffer[BLOCK_BUFFER_SIZE];            // A ring buffer for motion instfructions
 volatile unsigned char block_buffer_head;           // Index of the next block to be pushed
 volatile unsigned char block_buffer_tail;           // Index of the block to process now
 
 //===========================================================================
-//=============================private variables ============================
+//============================ private variables ============================
 //===========================================================================
-#ifdef PREVENT_DANGEROUS_EXTRUDE
-  float extrude_min_temp = EXTRUDE_MINTEMP;
-#endif
+
+// The current position of the tool in absolute steps
+long position[NUM_AXIS];               // Rescaled from extern when axis_steps_per_unit are changed by gcode
+static float previous_speed[NUM_AXIS]; // Speed of previous path line segment
+static float previous_nominal_speed;   // Nominal speed of previous path line segment
+
+unsigned char g_uc_extruder_last_move[4] = {0,0,0,0};
+
 #ifdef XY_FREQUENCY_LIMIT
   // Used for the frequency limit
   #define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT)
@@ -129,15 +125,15 @@ volatile unsigned char block_buffer_tail;           // Index of the block to pro
   static char meas_sample; //temporary variable to hold filament measurement sample
 #endif
 
+//===========================================================================
+//================================ functions ================================
+//===========================================================================
+
 // Get the next / previous index of the next block in the ring buffer
 // NOTE: Using & here (not %) because BLOCK_BUFFER_SIZE is always a power of 2
 FORCE_INLINE int8_t next_block_index(int8_t block_index) { return BLOCK_MOD(block_index + 1); }
 FORCE_INLINE int8_t prev_block_index(int8_t block_index) { return BLOCK_MOD(block_index - 1); }
 
-//===========================================================================
-//================================ Functions ================================
-//===========================================================================
-
 // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the 
 // given acceleration:
 FORCE_INLINE float estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration) {
@@ -162,8 +158,8 @@ void calculate_trapezoid_for_block(block_t *block, float entry_factor, float exi
   unsigned long final_rate = ceil(block->nominal_rate * exit_factor); // (step/min)
 
   // Limit minimal step rate (Otherwise the timer will overflow.)
-  if (initial_rate < 120) initial_rate = 120;
-  if (final_rate < 120) final_rate = 120;
+  NOLESS(initial_rate, 120);
+  NOLESS(final_rate, 120);
 
   long acceleration = block->acceleration_st;
   int32_t accelerate_steps = ceil(estimate_acceleration_distance(initial_rate, block->nominal_rate, acceleration));
@@ -385,16 +381,18 @@ void plan_init() {
     }
 
     float t = autotemp_min + high * autotemp_factor;
-    if (t < autotemp_min) t = autotemp_min;
-    if (t > autotemp_max) t = autotemp_max;
-    if (oldt > t) t = AUTOTEMP_OLDWEIGHT * oldt + (1 - AUTOTEMP_OLDWEIGHT) * t;
+    t = constrain(t, autotemp_min, autotemp_max);
+    if (oldt > t) {
+      t *= (1 - AUTOTEMP_OLDWEIGHT);
+      t += AUTOTEMP_OLDWEIGHT * oldt;
+    }
     oldt = t;
     setTargetHotend0(t);
   }
 #endif
 
 void check_axes_activity() {
-  unsigned char axis_active[NUM_AXIS],
+  unsigned char axis_active[NUM_AXIS] = { 0 },
                 tail_fan_speed = fanSpeed;
   #ifdef BARICUDA
     unsigned char tail_valve_pressure = ValvePressure,
@@ -429,7 +427,7 @@ void check_axes_activity() {
 
   #if HAS_FAN
     #ifdef FAN_KICKSTART_TIME
-      static unsigned long fan_kick_end;
+      static millis_t fan_kick_end;
       if (tail_fan_speed) {
         if (fan_kick_end == 0) {
           // Just starting up fan - run at full power.
@@ -507,8 +505,8 @@ float junction_deviation = 0.1;
 
   #ifdef PREVENT_DANGEROUS_EXTRUDE
     if (de) {
-      if (degHotend(active_extruder) < extrude_min_temp) {
-        position[E_AXIS] = target[E_AXIS]; //behave as if the move really took place, but ignore E part
+      if (degHotend(extruder) < extrude_min_temp) {
+        position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
         de = 0; // no difference
         SERIAL_ECHO_START;
         SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
@@ -544,8 +542,8 @@ float junction_deviation = 0.1;
 
   block->steps[Z_AXIS] = labs(dz);
   block->steps[E_AXIS] = labs(de);
-  block->steps[E_AXIS] *= volumetric_multiplier[active_extruder];
-  block->steps[E_AXIS] *= extruder_multiply[active_extruder];
+  block->steps[E_AXIS] *= volumetric_multiplier[extruder];
+  block->steps[E_AXIS] *= extruder_multiply[extruder];
   block->steps[E_AXIS] /= 100;
   block->step_event_count = max(block->steps[X_AXIS], max(block->steps[Y_AXIS], max(block->steps[Z_AXIS], block->steps[E_AXIS])));
 
@@ -654,10 +652,10 @@ float junction_deviation = 0.1;
     }
   }
 
-  if (block->steps[E_AXIS]) {
-    if (feed_rate < minimumfeedrate) feed_rate = minimumfeedrate;
-  }
-  else if (feed_rate < mintravelfeedrate) feed_rate = mintravelfeedrate;
+  if (block->steps[E_AXIS])
+    NOLESS(feed_rate, minimumfeedrate);
+  else
+    NOLESS(feed_rate, mintravelfeedrate);
 
   /**
    * This part of the code calculates the total length of the movement. 
@@ -679,7 +677,7 @@ float junction_deviation = 0.1;
     delta_mm[Y_AXIS] = dy / axis_steps_per_unit[Y_AXIS];
   #endif
   delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS];
-  delta_mm[E_AXIS] = (de / axis_steps_per_unit[E_AXIS]) * volumetric_multiplier[active_extruder] * extruder_multiply[active_extruder] / 100.0;
+  delta_mm[E_AXIS] = (de / axis_steps_per_unit[E_AXIS]) * volumetric_multiplier[extruder] * extruder_multiply[extruder] / 100.0;
 
   if (block->steps[X_AXIS] <= dropsegments && block->steps[Y_AXIS] <= dropsegments && block->steps[Z_AXIS] <= dropsegments) {
     block->millimeters = fabs(delta_mm[E_AXIS]);
@@ -998,10 +996,6 @@ void plan_set_e_position(const float &e) {
   st_set_e_position(position[E_AXIS]);
 }
 
-#ifdef PREVENT_DANGEROUS_EXTRUDE
-  void set_extrude_min_temp(float temp) { extrude_min_temp = temp; }
-#endif
-
 // Calculate the steps/s^2 acceleration rates, based on the mm/s^s
 void reset_acceleration_rates() {
   for (int i = 0; i < NUM_AXIS; i++)
diff --git a/Marlin/planner.h b/Marlin/planner.h
index 41471a2b0f6f70824881264adae765729b9b7bdb..c54ed4411aeb4f181237e2724c6f8f7febcc13d8 100644
--- a/Marlin/planner.h
+++ b/Marlin/planner.h
@@ -115,15 +115,19 @@ FORCE_INLINE uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block
 
 void plan_set_e_position(const float &e);
 
-extern unsigned long minsegmenttime;
-extern float max_feedrate[NUM_AXIS]; // set the max speeds
+//===========================================================================
+//============================= public variables ============================
+//===========================================================================
+
+extern millis_t minsegmenttime;
+extern float max_feedrate[NUM_AXIS]; // Max speeds in mm per minute
 extern float axis_steps_per_unit[NUM_AXIS];
 extern unsigned long max_acceleration_units_per_sq_second[NUM_AXIS]; // Use M201 to override by software
 extern float minimumfeedrate;
-extern float acceleration;         // Normal acceleration mm/s^2  THIS IS THE DEFAULT ACCELERATION for all moves. M204 SXXXX
-extern float retract_acceleration; //  mm/s^2   filament pull-pack and push-forward  while standing still in the other axis M204 TXXXX
-extern float travel_acceleration;  // Travel acceleration mm/s^2  THIS IS THE DEFAULT ACCELERATION for all NON printing moves. M204 MXXXX
-extern float max_xy_jerk; //speed than can be stopped at once, if i understand correctly.
+extern float acceleration;         // Normal acceleration mm/s^2  DEFAULT ACCELERATION for all printing moves. M204 SXXXX
+extern float retract_acceleration; // Retract acceleration mm/s^2 filament pull-back and push-forward while standing still in the other axes M204 TXXXX
+extern float travel_acceleration;  // Travel acceleration mm/s^2  DEFAULT ACCELERATION for all NON printing moves. M204 MXXXX
+extern float max_xy_jerk;          // The largest speed change requiring no acceleration
 extern float max_z_jerk;
 extern float max_e_jerk;
 extern float mintravelfeedrate;
@@ -161,10 +165,6 @@ FORCE_INLINE block_t *plan_get_current_block() {
     return NULL;
 }
 
-#ifdef PREVENT_DANGEROUS_EXTRUDE
-  void set_extrude_min_temp(float temp);
-#endif
-
 void reset_acceleration_rates();
 
-#endif //PLANNER_H
+#endif // PLANNER_H
diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index c53d7c0c926f09fda9a28496b0351f5360d07514..ca17b3f9f2cb4830d3ea16908d3209184872b74d 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -46,7 +46,7 @@ block_t *current_block;  // A pointer to the block currently being traced
 
 // Variables used by The Stepper Driver Interrupt
 static unsigned char out_bits;        // The next stepping-bits to be output
-static unsigned int cleaning_buffer_counter;  
+static unsigned int cleaning_buffer_counter;
 
 #ifdef Z_DUAL_ENDSTOPS
   static bool performing_homing = false, 
@@ -54,7 +54,7 @@ static unsigned int cleaning_buffer_counter;
               locked_z2_motor = false;
 #endif
 
-// Counter variables for the bresenham line tracer
+// Counter variables for the Bresenham line tracer
 static long counter_x, counter_y, counter_z, counter_e;
 volatile static unsigned long step_events_completed; // The number of step events executed in the current block
 
@@ -66,7 +66,7 @@ volatile static unsigned long step_events_completed; // The number of step event
 
 static long acceleration_time, deceleration_time;
 //static unsigned long accelerate_until, decelerate_after, acceleration_rate, initial_rate, final_rate, nominal_rate;
-static unsigned short acc_step_rate; // needed for deccelaration start point
+static unsigned short acc_step_rate; // needed for deceleration start point
 static char step_loops;
 static unsigned short OCR1A_nominal;
 static unsigned short step_loops_nominal;
@@ -98,19 +98,13 @@ static volatile bool endstop_z_probe_hit = false; // Leaving this in even if Z_P
 #if HAS_Y_MAX
   static bool old_y_max_endstop = false;
 #endif
-#if HAS_Z_MIN
-  static bool old_z_min_endstop = false;
-#endif
-#if HAS_Z_MAX
-  static bool old_z_max_endstop = false;
-#endif
+
+static bool old_z_min_endstop = false;
+static bool old_z_max_endstop = false;
+
 #ifdef Z_DUAL_ENDSTOPS
-  // #if HAS_Z2_MIN
-    static bool old_z2_min_endstop = false;
-  // #endif
-  // #if HAS_Z2_MAX
-    static bool old_z2_max_endstop = false;
-  // #endif
+  static bool old_z2_min_endstop = false;
+  static bool old_z2_max_endstop = false;
 #endif
 
 #ifdef Z_PROBE_ENDSTOP // No need to check for valid pin, SanityCheck.h already does this.
@@ -211,8 +205,14 @@ volatile signed char count_direction[NUM_AXIS] = { 1, 1, 1, 1 };
 // intRes = longIn1 * longIn2 >> 24
 // uses:
 // r26 to store 0
-// r27 to store the byte 1 of the 48bit result
-#define MultiU24X24toH16(intRes, longIn1, longIn2) \
+// r27 to store bits 16-23 of the 48bit result. The top bit is used to round the two byte result.
+// note that the lower two bytes and the upper byte of the 48bit result are not calculated.
+// this can cause the result to be out by one as the lower bytes may cause carries into the upper ones.
+// B0 A0 are bits 24-39 and are the returned value
+// C1 B1 A1 is longIn1
+// D2 C2 B2 A2 is longIn2
+//
+#define MultiU24X32toH16(intRes, longIn1, longIn2) \
   asm volatile ( \
     "clr r26 \n\t" \
     "mul %A1, %B2 \n\t" \
@@ -243,6 +243,11 @@ volatile signed char count_direction[NUM_AXIS] = { 1, 1, 1, 1 };
     "lsr r27 \n\t" \
     "adc %A0, r26 \n\t" \
     "adc %B0, r26 \n\t" \
+    "mul %D2, %A1 \n\t" \
+    "add %A0, r0 \n\t" \
+    "adc %B0, r1 \n\t" \
+    "mul %D2, %B1 \n\t" \
+    "add %B0, r0 \n\t" \
     "clr r1 \n\t" \
     : \
     "=&r" (intRes) \
@@ -280,8 +285,8 @@ void checkHitEndstops() {
     }
     #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");
+      SERIAL_ECHOPAIR(" Z_PROBE:", (float)endstops_trigsteps[Z_AXIS] / axis_steps_per_unit[Z_AXIS]);
+      LCD_MESSAGEPGM(MSG_ENDSTOPS_HIT "ZP");
     }
     #endif
     SERIAL_EOL;
@@ -319,7 +324,7 @@ void enable_endstops(bool check) { check_endstops = check; }
 //  The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates
 //  first block->accelerate_until step_events_completed, then keeps going at constant speed until
 //  step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset.
-//  The slope of acceleration is calculated with the leib ramp alghorithm.
+//  The slope of acceleration is calculated using v = u + at where t is the accumulated timer values of the steps so far.
 
 void st_wake_up() {
   //  TCNT1 = 0;
@@ -400,13 +405,13 @@ ISR(TIMER1_COMPA_vect) {
     current_block = NULL;
     plan_discard_current_block();
     #ifdef SD_FINISHED_RELEASECOMMAND
-      if ((cleaning_buffer_counter == 1) && (SD_FINISHED_STEPPERRELEASE)) enquecommands_P(PSTR(SD_FINISHED_RELEASECOMMAND));
+      if ((cleaning_buffer_counter == 1) && (SD_FINISHED_STEPPERRELEASE)) enqueuecommands_P(PSTR(SD_FINISHED_RELEASECOMMAND));
     #endif
     cleaning_buffer_counter--;
     OCR1A = 200;
     return;
   }
-  
+
   // If there is no current block, attempt to pop one from the buffer
   if (!current_block) {
     // Anything in the buffer?
@@ -458,14 +463,22 @@ ISR(TIMER1_COMPA_vect) {
       count_direction[Y_AXIS] = 1;
     }
 
+    #define _ENDSTOP(axis, minmax) axis ##_## minmax ##_endstop
+    #define _ENDSTOP_PIN(AXIS, MINMAX) AXIS ##_## MINMAX ##_PIN
+    #define _ENDSTOP_INVERTING(AXIS, MINMAX) AXIS ##_## MINMAX ##_ENDSTOP_INVERTING
+    #define _OLD_ENDSTOP(axis, minmax) old_## axis ##_## minmax ##_endstop
+    #define _AXIS(AXIS) AXIS ##_AXIS
+    #define _ENDSTOP_HIT(axis) endstop_## axis ##_hit
+
     #define UPDATE_ENDSTOP(axis,AXIS,minmax,MINMAX) \
-      bool axis ##_## minmax ##_endstop = (READ(AXIS ##_## MINMAX ##_PIN) != AXIS ##_## MINMAX ##_ENDSTOP_INVERTING); \
-      if (axis ##_## minmax ##_endstop && old_## axis ##_## minmax ##_endstop && (current_block->steps[AXIS ##_AXIS] > 0)) { \
-        endstops_trigsteps[AXIS ##_AXIS] = count_position[AXIS ##_AXIS]; \
-        endstop_## axis ##_hit = true; \
+      bool _ENDSTOP(axis, minmax) = (READ(_ENDSTOP_PIN(AXIS, MINMAX)) != _ENDSTOP_INVERTING(AXIS, MINMAX)); \
+      if (_ENDSTOP(axis, minmax) && _OLD_ENDSTOP(axis, minmax) && (current_block->steps[_AXIS(AXIS)] > 0)) { \
+        endstops_trigsteps[_AXIS(AXIS)] = count_position[_AXIS(AXIS)]; \
+        _ENDSTOP_HIT(axis) = true; \
         step_events_completed = current_block->step_event_count; \
       } \
-      old_## axis ##_## minmax ##_endstop = axis ##_## minmax ##_endstop;
+      _OLD_ENDSTOP(axis, minmax) = _ENDSTOP(axis, minmax);
+
 
     // Check X and Y endstops
     if (check_endstops) {
@@ -475,13 +488,13 @@ ISR(TIMER1_COMPA_vect) {
         if ((current_block->steps[A_AXIS] != current_block->steps[B_AXIS]) || (TEST(out_bits, A_AXIS) == TEST(out_bits, B_AXIS))) {
           if (TEST(out_bits, X_HEAD))
       #else
-          if (TEST(out_bits, X_AXIS))   // stepping along -X axis (regular cartesians bot)
+          if (TEST(out_bits, X_AXIS))   // stepping along -X axis (regular Cartesian bot)
       #endif
           { // -direction
             #ifdef DUAL_X_CARRIAGE
               // with 2 x-carriages, endstops are only checked in the homing direction for the active extruder
               if ((current_block->active_extruder == 0 && X_HOME_DIR == -1) || (current_block->active_extruder != 0 && X2_HOME_DIR == -1))
-            #endif          
+            #endif
               {
                 #if HAS_X_MIN
                   UPDATE_ENDSTOP(x, X, min, MIN);
@@ -567,14 +580,14 @@ ISR(TIMER1_COMPA_vect) {
           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;
+            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");
+//            if (z_probe_endstop && old_z_probe_endstop) SERIAL_ECHOLN("z_probe_endstop = true");
           }
           old_z_probe_endstop = z_probe_endstop;
         #endif
-        
+
       } // check_endstops
 
     }
@@ -620,15 +633,15 @@ 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");
+            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
@@ -662,6 +675,11 @@ ISR(TIMER1_COMPA_vect) {
         }
       #endif //ADVANCE
 
+      #define _COUNTER(axis) counter_## axis
+      #define _WRITE_STEP(AXIS, HIGHLOW) AXIS ##_STEP_WRITE(HIGHLOW)
+      #define _APPLY_STEP(AXIS) AXIS ##_APPLY_STEP
+      #define _INVERT_STEP_PIN(AXIS) INVERT_## AXIS ##_STEP_PIN
+
       #ifdef CONFIG_STEPPERS_TOSHIBA
         /**
          * The Toshiba stepper controller require much longer pulses.
@@ -670,8 +688,8 @@ ISR(TIMER1_COMPA_vect) {
          * lag to allow it work with without needing NOPs
          */
         #define STEP_ADD(axis, AXIS) \
-         counter_## axis += current_block->steps[AXIS ##_AXIS]; \
-         if (counter_## axis > 0) { AXIS ##_STEP_WRITE(HIGH); }
+         _COUNTER(axis) += current_block->steps[_AXIS(AXIS)]; \
+         if (_COUNTER(axis) > 0) { _WRITE_STEP(AXIS, HIGH); }
         STEP_ADD(x,X);
         STEP_ADD(y,Y);
         STEP_ADD(z,Z);
@@ -680,10 +698,10 @@ ISR(TIMER1_COMPA_vect) {
         #endif
 
         #define STEP_IF_COUNTER(axis, AXIS) \
-          if (counter_## axis > 0) { \
-            counter_## axis -= current_block->step_event_count; \
-            count_position[AXIS ##_AXIS] += count_direction[AXIS ##_AXIS]; \
-            AXIS ##_STEP_WRITE(LOW); \
+          if (_COUNTER(axis) > 0) { \
+            _COUNTER(axis) -= current_block->step_event_count; \
+            count_position[_AXIS(AXIS)] += count_direction[_AXIS(AXIS)]; \
+            _WRITE_STEP(AXIS, LOW); \
           }
 
         STEP_IF_COUNTER(x, X);
@@ -696,12 +714,12 @@ ISR(TIMER1_COMPA_vect) {
       #else // !CONFIG_STEPPERS_TOSHIBA
 
         #define APPLY_MOVEMENT(axis, AXIS) \
-          counter_## axis += current_block->steps[AXIS ##_AXIS]; \
-          if (counter_## axis > 0) { \
-            AXIS ##_APPLY_STEP(!INVERT_## AXIS ##_STEP_PIN,0); \
-            counter_## axis -= current_block->step_event_count; \
-            count_position[AXIS ##_AXIS] += count_direction[AXIS ##_AXIS]; \
-            AXIS ##_APPLY_STEP(INVERT_## AXIS ##_STEP_PIN,0); \
+          _COUNTER(axis) += current_block->steps[_AXIS(AXIS)]; \
+          if (_COUNTER(axis) > 0) { \
+            _APPLY_STEP(AXIS)(!_INVERT_STEP_PIN(AXIS),0); \
+            _COUNTER(axis) -= current_block->step_event_count; \
+            count_position[_AXIS(AXIS)] += count_direction[_AXIS(AXIS)]; \
+            _APPLY_STEP(AXIS)(_INVERT_STEP_PIN(AXIS),0); \
           }
 
         APPLY_MOVEMENT(x, X);
@@ -718,9 +736,9 @@ ISR(TIMER1_COMPA_vect) {
     // Calculate new timer value
     unsigned short timer;
     unsigned short step_rate;
-    if (step_events_completed <= (unsigned long int)current_block->accelerate_until) {
+    if (step_events_completed <= (unsigned long)current_block->accelerate_until) {
 
-      MultiU24X24toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
+      MultiU24X32toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
       acc_step_rate += current_block->initial_rate;
 
       // upper limit
@@ -742,8 +760,8 @@ ISR(TIMER1_COMPA_vect) {
 
       #endif
     }
-    else if (step_events_completed > (unsigned long int)current_block->decelerate_after) {
-      MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate);
+    else if (step_events_completed > (unsigned long)current_block->decelerate_after) {
+      MultiU24X32toH16(step_rate, deceleration_time, current_block->acceleration_rate);
 
       if (step_rate > acc_step_rate) { // Check step_rate stays positive
         step_rate = current_block->final_rate;
@@ -869,7 +887,7 @@ void st_init() {
   #ifdef HAVE_L6470DRIVER
     L6470_init();
   #endif
-  
+
   // Initialize Dir Pins
   #if HAS_X_DIR
     X_DIR_INIT;
@@ -915,11 +933,11 @@ void st_init() {
   #if HAS_Y_ENABLE
     Y_ENABLE_INIT;
     if (!Y_ENABLE_ON) Y_ENABLE_WRITE(HIGH);
-	
-	#if defined(Y_DUAL_STEPPER_DRIVERS) && HAS_Y2_ENABLE
-	  Y2_ENABLE_INIT;
-	  if (!Y_ENABLE_ON) Y2_ENABLE_WRITE(HIGH);
-	#endif
+
+  #if defined(Y_DUAL_STEPPER_DRIVERS) && HAS_Y2_ENABLE
+    Y2_ENABLE_INIT;
+    if (!Y_ENABLE_ON) Y2_ENABLE_WRITE(HIGH);
+  #endif
   #endif
   #if HAS_Z_ENABLE
     Z_ENABLE_INIT;
@@ -996,8 +1014,8 @@ void st_init() {
     #ifdef ENDSTOPPULLUP_ZMAX
       WRITE(Z2_MAX_PIN,HIGH);
     #endif
-  #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
@@ -1005,10 +1023,13 @@ void st_init() {
   #endif
 #endif
 
+  #define _STEP_INIT(AXIS) AXIS ##_STEP_INIT
+  #define _DISABLE(axis) disable_## axis()
+
   #define AXIS_INIT(axis, AXIS, PIN) \
-    AXIS ##_STEP_INIT; \
-    AXIS ##_STEP_WRITE(INVERT_## PIN ##_STEP_PIN); \
-    disable_## axis()
+    _STEP_INIT(AXIS); \
+    _WRITE_STEP(AXIS, _INVERT_STEP_PIN(PIN)); \
+    _DISABLE(axis)
 
   #define E_AXIS_INIT(NUM) AXIS_INIT(e## NUM, E## NUM, E)
 
@@ -1072,10 +1093,7 @@ void st_init() {
       TCCR0A &= ~BIT(WGM01);
       TCCR0A &= ~BIT(WGM00);
     #endif
-    e_steps[0] = 0;
-    e_steps[1] = 0;
-    e_steps[2] = 0;
-    e_steps[3] = 0;
+    e_steps[0] = e_steps[1] = e_steps[2] = e_steps[3] = 0;
     TIMSK0 |= BIT(OCIE0A);
   #endif //ADVANCE
 
@@ -1144,14 +1162,19 @@ void quickStop() {
   // No other ISR should ever interrupt this!
   void babystep(const uint8_t axis, const bool direction) {
 
+    #define _ENABLE(axis) enable_## axis()
+    #define _READ_DIR(AXIS) AXIS ##_DIR_READ
+    #define _INVERT_DIR(AXIS) INVERT_## AXIS ##_DIR
+    #define _APPLY_DIR(AXIS, INVERT) AXIS ##_APPLY_DIR(INVERT, true)
+
     #define BABYSTEP_AXIS(axis, AXIS, INVERT) { \
-        enable_## axis(); \
-        uint8_t old_pin = AXIS ##_DIR_READ; \
-        AXIS ##_APPLY_DIR(INVERT_## AXIS ##_DIR^direction^INVERT, true); \
-        AXIS ##_APPLY_STEP(!INVERT_## AXIS ##_STEP_PIN, true); \
-        _delay_us(1U); \
-        AXIS ##_APPLY_STEP(INVERT_## AXIS ##_STEP_PIN, true); \
-        AXIS ##_APPLY_DIR(old_pin, true); \
+        _ENABLE(axis); \
+        uint8_t old_pin = _READ_DIR(AXIS); \
+        _APPLY_DIR(AXIS, _INVERT_DIR(AXIS)^direction^INVERT); \
+        _APPLY_STEP(AXIS)(!_INVERT_STEP_PIN(AXIS), true); \
+        delayMicroseconds(2); \
+        _APPLY_STEP(AXIS)(_INVERT_STEP_PIN(AXIS), true); \
+        _APPLY_DIR(AXIS, old_pin); \
       }
 
     switch(axis) {
@@ -1163,7 +1186,7 @@ void quickStop() {
       case Y_AXIS:
         BABYSTEP_AXIS(y, Y, false);
         break;
-     
+ 
       case Z_AXIS: {
 
         #ifndef DELTA
@@ -1188,7 +1211,7 @@ void quickStop() {
           X_STEP_WRITE(!INVERT_X_STEP_PIN);
           Y_STEP_WRITE(!INVERT_Y_STEP_PIN);
           Z_STEP_WRITE(!INVERT_Z_STEP_PIN);
-          _delay_us(1U);
+          delayMicroseconds(2);
           X_STEP_WRITE(INVERT_X_STEP_PIN); 
           Y_STEP_WRITE(INVERT_Y_STEP_PIN); 
           Z_STEP_WRITE(INVERT_Z_STEP_PIN);
@@ -1200,7 +1223,7 @@ void quickStop() {
         #endif
 
       } break;
-     
+ 
       default: break;
     }
   }
@@ -1264,7 +1287,7 @@ void microstep_init() {
 
   #if HAS_MICROSTEPS
     pinMode(X_MS1_PIN,OUTPUT);
-    pinMode(X_MS2_PIN,OUTPUT);  
+    pinMode(X_MS2_PIN,OUTPUT);
     pinMode(Y_MS1_PIN,OUTPUT);
     pinMode(Y_MS2_PIN,OUTPUT);
     pinMode(Z_MS1_PIN,OUTPUT);
diff --git a/Marlin/temperature.cpp b/Marlin/temperature.cpp
index b4dffbfe16bf6c6e48f910d0f51d0e942b3ab2fd..7bf0b6e53cdef8db81ca2df18fa246033cd6ab6f 100644
--- a/Marlin/temperature.cpp
+++ b/Marlin/temperature.cpp
@@ -77,20 +77,21 @@ unsigned char soft_pwm_bed;
 #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
   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);
+  void thermal_runaway_protection(TRState *state, millis_t *timer, float temperature, float target_temperature, int heater_id, int period_seconds, int hysteresis_degc);
   #if HAS_HEATER_THERMAL_PROTECTION
     static TRState thermal_runaway_state_machine[4] = { TRReset, TRReset, TRReset, TRReset };
-    static unsigned long thermal_runaway_timer[4]; // = {0,0,0,0};
+    static millis_t thermal_runaway_timer[4]; // = {0,0,0,0};
   #endif
   #if HAS_BED_THERMAL_PROTECTION
     static TRState thermal_runaway_bed_state_machine = TRReset;
-    static unsigned long thermal_runaway_bed_timer;
+    static millis_t thermal_runaway_bed_timer;
   #endif
 #endif
 
 //===========================================================================
-//=============================private variables============================
+//============================ private variables ============================
 //===========================================================================
+
 static volatile bool temp_meas_ready = false;
 
 #ifdef PIDTEMP
@@ -118,7 +119,7 @@ static volatile bool temp_meas_ready = false;
   static float temp_iState_min_bed;
   static float temp_iState_max_bed;
 #else //PIDTEMPBED
-  static unsigned long  previous_millis_bed_heater;
+  static millis_t  next_bed_check_ms;
 #endif //PIDTEMPBED
   static unsigned char soft_pwm[EXTRUDERS];
 
@@ -126,7 +127,7 @@ static volatile bool temp_meas_ready = false;
   static unsigned char soft_pwm_fan;
 #endif
 #if HAS_AUTO_FAN
-  static unsigned long extruder_autofan_last_check;
+  static millis_t next_auto_fan_check_ms;
 #endif  
 
 #ifdef PIDTEMP
@@ -171,7 +172,7 @@ static void updateTemperaturesFromRawValues();
 
 #ifdef WATCH_TEMP_PERIOD
   int watch_start_temp[EXTRUDERS] = { 0 };
-  unsigned long watchmillis[EXTRUDERS] = { 0 };
+  millis_t watchmillis[EXTRUDERS] = { 0 };
 #endif //WATCH_TEMP_PERIOD
 
 #ifndef SOFT_PWM_SCALE
@@ -187,7 +188,7 @@ static void updateTemperaturesFromRawValues();
 #endif
 
 //===========================================================================
-//=============================   functions      ============================
+//================================ Functions ================================
 //===========================================================================
 
 void PID_autotune(float temp, int extruder, int ncycles)
@@ -196,7 +197,7 @@ void PID_autotune(float temp, int extruder, int ncycles)
   int cycles = 0;
   bool heating = true;
 
-  unsigned long temp_millis = millis(), t1 = temp_millis, t2 = temp_millis;
+  millis_t temp_ms = millis(), t1 = temp_ms, t2 = temp_ms;
   long t_high = 0, t_low = 0;
 
   long bias, d;
@@ -205,7 +206,7 @@ void PID_autotune(float temp, int extruder, int ncycles)
   float max = 0, min = 10000;
 
   #if HAS_AUTO_FAN
-        unsigned long extruder_autofan_last_check = temp_millis;
+    millis_t next_auto_fan_check_ms = temp_ms + 2500;
   #endif
 
   if (extruder >= EXTRUDERS
@@ -219,7 +220,7 @@ void PID_autotune(float temp, int extruder, int ncycles)
   
   SERIAL_ECHOLN(MSG_PID_AUTOTUNE_START);
 
-  disable_heater(); // switch off all heaters.
+  disable_all_heaters(); // switch off all heaters.
 
   if (extruder < 0)
     soft_pwm_bed = bias = d = MAX_BED_POWER / 2;
@@ -229,7 +230,7 @@ void PID_autotune(float temp, int extruder, int ncycles)
   // PID Tuning loop
   for (;;) {
 
-    unsigned long ms = millis();
+    millis_t ms = millis();
 
     if (temp_meas_ready) { // temp sample ready
       updateTemperaturesFromRawValues();
@@ -240,9 +241,9 @@ void PID_autotune(float temp, int extruder, int ncycles)
       min = min(min, input);
 
       #if HAS_AUTO_FAN
-        if (ms > extruder_autofan_last_check + 2500) {
+        if (ms > next_auto_fan_check_ms) {
           checkExtruderAutoFans();
-          extruder_autofan_last_check = ms;
+          next_auto_fan_check_ms = ms + 2500;
         }
       #endif
 
@@ -317,7 +318,7 @@ void PID_autotune(float temp, int extruder, int ncycles)
       return;
     }
     // Every 2 seconds...
-    if (ms > temp_millis + 2000) {
+    if (ms > temp_ms + 2000) {
       int p;
       if (extruder < 0) {
         p = soft_pwm_bed;
@@ -332,7 +333,7 @@ void PID_autotune(float temp, int extruder, int ncycles)
       SERIAL_PROTOCOLPGM(MSG_AT);
       SERIAL_PROTOCOLLN(p);
 
-      temp_millis = ms;
+      temp_ms = ms;
     } // every 2 seconds
     // Over 2 minutes?
     if (((ms - t1) + (ms - t2)) > (10L*60L*1000L*2L)) {
@@ -341,6 +342,10 @@ void PID_autotune(float temp, int extruder, int ncycles)
     }
     if (cycles > ncycles) {
       SERIAL_PROTOCOLLNPGM(MSG_PID_AUTOTUNE_FINISHED);
+      const char *estring = extruder < 0 ? "bed" : "";
+      SERIAL_PROTOCOLPGM("#define  DEFAULT_"); SERIAL_PROTOCOL(estring); SERIAL_PROTOCOLPGM("Kp "); SERIAL_PROTOCOLLN(Kp);
+      SERIAL_PROTOCOLPGM("#define  DEFAULT_"); SERIAL_PROTOCOL(estring); SERIAL_PROTOCOLPGM("Ki "); SERIAL_PROTOCOLLN(Ki);
+      SERIAL_PROTOCOLPGM("#define  DEFAULT_"); SERIAL_PROTOCOL(estring); SERIAL_PROTOCOLPGM("Kd "); SERIAL_PROTOCOLLN(Kd);
       return;
     }
     lcd_update();
@@ -458,11 +463,11 @@ inline void _temp_error(int e, const char *msg1, const char *msg2) {
 }
 
 void max_temp_error(uint8_t e) {
-  disable_heater();
+  disable_all_heaters();
   _temp_error(e, PSTR(MSG_MAXTEMP_EXTRUDER_OFF), PSTR(MSG_ERR_MAXTEMP));
 }
 void min_temp_error(uint8_t e) {
-  disable_heater();
+  disable_all_heaters();
   _temp_error(e, PSTR(MSG_MINTEMP_EXTRUDER_OFF), PSTR(MSG_ERR_MINTEMP));
 }
 void bed_max_temp_error(void) {
@@ -579,6 +584,14 @@ float get_pid_output(int e) {
   }
 #endif
 
+/**
+ * Manage heating activities for extruder hot-ends and a heated bed
+ *  - Acquire updated temperature readings
+ *  - Invoke thermal runaway protection
+ *  - Manage extruder auto-fan
+ *  - Apply filament width to the extrusion rate (may move)
+ *  - Update the heated bed PID output value
+ */
 void manage_heater() {
 
   if (!temp_meas_ready) return;
@@ -592,7 +605,7 @@ void manage_heater() {
   #endif //HEATER_0_USES_MAX6675
 
   #if defined(WATCH_TEMP_PERIOD) || !defined(PIDTEMPBED) || HAS_AUTO_FAN
-    unsigned long ms = millis();
+    millis_t ms = millis();
   #endif
 
   // Loop through all extruders
@@ -623,7 +636,7 @@ void manage_heater() {
 
     #ifdef TEMP_SENSOR_1_AS_REDUNDANT
       if (fabs(current_temperature[0] - redundant_temperature) > MAX_REDUNDANT_TEMP_SENSOR_DIFF) {
-        disable_heater();
+        disable_all_heaters();
         _temp_error(0, PSTR(MSG_EXTRUDER_SWITCHED_OFF), PSTR(MSG_ERR_REDUNDANT_TEMP));
       }
     #endif // TEMP_SENSOR_1_AS_REDUNDANT
@@ -631,16 +644,31 @@ void manage_heater() {
   } // Extruders Loop
 
   #if HAS_AUTO_FAN
-    if (ms > extruder_autofan_last_check + 2500) { // only need to check fan state very infrequently
+    if (ms > next_auto_fan_check_ms) { // only need to check fan state very infrequently
       checkExtruderAutoFans();
-      extruder_autofan_last_check = ms;
+      next_auto_fan_check_ms = ms + 2500;
     }
   #endif       
-  
+
+  // Control the extruder rate based on the width sensor
+  #ifdef FILAMENT_SENSOR
+    if (filament_sensor) {
+      meas_shift_index = delay_index1 - meas_delay_cm;
+      if (meas_shift_index < 0) meas_shift_index += MAX_MEASUREMENT_DELAY + 1;  //loop around buffer if needed
+      
+      // Get the delayed info and add 100 to reconstitute to a percent of
+      // the nominal filament diameter then square it to get an area
+      meas_shift_index = constrain(meas_shift_index, 0, MAX_MEASUREMENT_DELAY);
+      float vm = pow((measurement_delay[meas_shift_index] + 100.0) / 100.0, 2);
+      if (vm < 0.01) vm = 0.01;
+      volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM] = vm;
+    }
+  #endif //FILAMENT_SENSOR
+
   #ifndef PIDTEMPBED
-    if (ms < previous_millis_bed_heater + BED_CHECK_INTERVAL) return;
-    previous_millis_bed_heater = ms;
-  #endif //PIDTEMPBED
+    if (ms < next_bed_check_ms) return;
+    next_bed_check_ms = ms + BED_CHECK_INTERVAL;
+  #endif
 
   #if TEMP_SENSOR_BED != 0
   
@@ -653,22 +681,22 @@ void manage_heater() {
 
       soft_pwm_bed = current_temperature_bed > BED_MINTEMP && current_temperature_bed < BED_MAXTEMP ? (int)pid_output >> 1 : 0;
 
-    #elif !defined(BED_LIMIT_SWITCHING)
-      // Check if temperature is within the correct range
+    #elif defined(BED_LIMIT_SWITCHING)
+      // Check if temperature is within the correct band
       if (current_temperature_bed > BED_MINTEMP && current_temperature_bed < BED_MAXTEMP) {
-        soft_pwm_bed = current_temperature_bed < target_temperature_bed ? MAX_BED_POWER >> 1 : 0;
+        if (current_temperature_bed >= target_temperature_bed + BED_HYSTERESIS)
+          soft_pwm_bed = 0;
+        else if (current_temperature_bed <= target_temperature_bed - BED_HYSTERESIS)
+          soft_pwm_bed = MAX_BED_POWER >> 1;
       }
       else {
         soft_pwm_bed = 0;
         WRITE_HEATER_BED(LOW);
       }
-    #else //#ifdef BED_LIMIT_SWITCHING
-      // Check if temperature is within the correct band
+    #else // BED_LIMIT_SWITCHING
+      // Check if temperature is within the correct range
       if (current_temperature_bed > BED_MINTEMP && current_temperature_bed < BED_MAXTEMP) {
-        if (current_temperature_bed >= target_temperature_bed + BED_HYSTERESIS)
-          soft_pwm_bed = 0;
-        else if (current_temperature_bed <= target_temperature_bed - BED_HYSTERESIS)
-          soft_pwm_bed = MAX_BED_POWER >> 1;
+        soft_pwm_bed = current_temperature_bed < target_temperature_bed ? MAX_BED_POWER >> 1 : 0;
       }
       else {
         soft_pwm_bed = 0;
@@ -676,56 +704,36 @@ void manage_heater() {
       }
     #endif
   #endif //TEMP_SENSOR_BED != 0
-  
-  // Control the extruder rate based on the width sensor
-  #ifdef FILAMENT_SENSOR
-    if (filament_sensor) {
-      meas_shift_index = delay_index1 - meas_delay_cm;
-      if (meas_shift_index < 0) meas_shift_index += MAX_MEASUREMENT_DELAY + 1;  //loop around buffer if needed
-      
-      // Get the delayed info and add 100 to reconstitute to a percent of
-      // the nominal filament diameter then square it to get an area
-      meas_shift_index = constrain(meas_shift_index, 0, MAX_MEASUREMENT_DELAY);
-      float vm = pow((measurement_delay[meas_shift_index] + 100.0) / 100.0, 2);
-      if (vm < 0.01) vm = 0.01;
-      volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM] = vm;
-    }
-  #endif //FILAMENT_SENSOR
 }
 
 #define PGM_RD_W(x)   (short)pgm_read_word(&x)
 // Derived from RepRap FiveD extruder::getTemperature()
 // For hot end temperature measurement.
 static float analog2temp(int raw, uint8_t e) {
-#ifdef TEMP_SENSOR_1_AS_REDUNDANT
-  if (e > EXTRUDERS)
-#else
-  if (e >= EXTRUDERS)
-#endif
-  {
+  #ifdef TEMP_SENSOR_1_AS_REDUNDANT
+    if (e > EXTRUDERS)
+  #else
+    if (e >= EXTRUDERS)
+  #endif
+    {
       SERIAL_ERROR_START;
       SERIAL_ERROR((int)e);
       SERIAL_ERRORLNPGM(MSG_INVALID_EXTRUDER_NUM);
       kill();
       return 0.0;
-  } 
+    } 
+
   #ifdef HEATER_0_USES_MAX6675
-    if (e == 0)
-    {
-      return 0.25 * raw;
-    }
+    if (e == 0) return 0.25 * raw;
   #endif
 
-  if(heater_ttbl_map[e] != NULL)
-  {
+  if (heater_ttbl_map[e] != NULL) {
     float celsius = 0;
     uint8_t i;
     short (*tt)[][2] = (short (*)[][2])(heater_ttbl_map[e]);
 
-    for (i=1; i<heater_ttbllen_map[e]; i++)
-    {
-      if (PGM_RD_W((*tt)[i][0]) > raw)
-      {
+    for (i = 1; i < heater_ttbllen_map[e]; i++) {
+      if (PGM_RD_W((*tt)[i][0]) > raw) {
         celsius = PGM_RD_W((*tt)[i-1][1]) + 
           (raw - PGM_RD_W((*tt)[i-1][0])) * 
           (float)(PGM_RD_W((*tt)[i][1]) - PGM_RD_W((*tt)[i-1][1])) /
@@ -749,10 +757,8 @@ static float analog2tempBed(int raw) {
     float celsius = 0;
     byte i;
 
-    for (i=1; i<BEDTEMPTABLE_LEN; i++)
-    {
-      if (PGM_RD_W(BEDTEMPTABLE[i][0]) > raw)
-      {
+    for (i = 1; i < BEDTEMPTABLE_LEN; i++) {
+      if (PGM_RD_W(BEDTEMPTABLE[i][0]) > raw) {
         celsius  = PGM_RD_W(BEDTEMPTABLE[i-1][1]) + 
           (raw - PGM_RD_W(BEDTEMPTABLE[i-1][0])) * 
           (float)(PGM_RD_W(BEDTEMPTABLE[i][1]) - PGM_RD_W(BEDTEMPTABLE[i-1][1])) /
@@ -816,11 +822,11 @@ static void updateTemperaturesFromRawValues() {
 #endif
 
 
-
-
-
-void tp_init()
-{
+/**
+ * Initialize the temperature manager
+ * The manager is implemented by periodic calls to manage_heater()
+ */
+void tp_init() {
   #if MB(RUMBA) && ((TEMP_SENSOR_0==-1)||(TEMP_SENSOR_1==-1)||(TEMP_SENSOR_2==-1)||(TEMP_SENSOR_BED==-1))
     //disable RUMBA JTAG in case the thermocouple extension is plugged on top of JTAG connector
     MCUCR=BIT(JTD);
@@ -992,7 +998,7 @@ void tp_init()
 
 void setWatch() {
   #ifdef WATCH_TEMP_PERIOD
-    unsigned long ms = millis();
+    millis_t ms = millis();
     for (int e = 0; e < EXTRUDERS; e++) {
       if (degHotend(e) < degTargetHotend(e) - (WATCH_TEMP_INCREASE * 2)) {
         watch_start_temp[e] = degHotend(e);
@@ -1004,7 +1010,7 @@ void setWatch() {
 
 #if HAS_HEATER_THERMAL_PROTECTION || HAS_BED_THERMAL_PROTECTION
 
-  void thermal_runaway_protection(TRState *state, unsigned long *timer, float temperature, float target_temperature, int heater_id, int period_seconds, int hysteresis_degc) {
+  void thermal_runaway_protection(TRState *state, millis_t *timer, float temperature, float target_temperature, int heater_id, int period_seconds, int hysteresis_degc) {
 
     static float tr_target_temperature[EXTRUDERS+1] = { 0.0 };
 
@@ -1059,7 +1065,7 @@ void setWatch() {
         SERIAL_ERRORLNPGM(MSG_THERMAL_RUNAWAY_STOP);
         if (heater_id < 0) SERIAL_ERRORLNPGM("bed"); else SERIAL_ERRORLN(heater_id);
         LCD_ALERTMESSAGEPGM(MSG_THERMAL_RUNAWAY);
-        disable_heater();
+        disable_all_heaters();
         disable_all_steppers();
         for (;;) {
           manage_heater();
@@ -1070,7 +1076,7 @@ void setWatch() {
 
 #endif // HAS_HEATER_THERMAL_PROTECTION || HAS_BED_THERMAL_PROTECTION
 
-void disable_heater() {
+void disable_all_heaters() {
   for (int i=0; i<EXTRUDERS; i++) setTargetHotend(0, i);
   setTargetBed(0);
 
@@ -1109,16 +1115,18 @@ void disable_heater() {
 
 #ifdef HEATER_0_USES_MAX6675
   #define MAX6675_HEAT_INTERVAL 250u
-  unsigned long max6675_previous_millis = MAX6675_HEAT_INTERVAL;
+  static millis_t next_max6675_ms = 0;
   int max6675_temp = 2000;
 
   static int read_max6675() {
 
-    unsigned long ms = millis();
-    if (ms < max6675_previous_millis + MAX6675_HEAT_INTERVAL)
+    millis_t ms = millis();
+
+    if (ms < next_max6675_ms)
       return max6675_temp;
     
-    max6675_previous_millis = ms;
+    next_max6675_ms = ms + MAX6675_HEAT_INTERVAL;
+
     max6675_temp = 0;
 
     #ifdef PRR
@@ -1206,11 +1214,15 @@ static void set_current_temp_raw() {
   temp_meas_ready = true;
 }
 
-//
-// Timer 0 is shared with millies
-//
+/**
+ * Timer 0 is shared with millies
+ *  - Manage PWM to all the heaters and fan
+ *  - Update the raw temperature values
+ *  - Check new temperature values for MIN/MAX errors
+ *  - Step the babysteps value for each axis towards 0
+ */
 ISR(TIMER0_COMPB_vect) {
-  //these variables are only accesible from the ISR, but static, so they don't lose their value
+
   static unsigned char temp_count = 0;
   static TempState temp_state = StartupDelay;
   static unsigned char pwm_count = BIT(SOFT_PWM_SCALE);
@@ -1412,6 +1424,7 @@ ISR(TIMER0_COMPB_vect) {
     #define START_ADC(pin) ADCSRB = 0; SET_ADMUX_ADCSRA(pin)
   #endif
 
+  // Prepare or measure a sensor, each one every 12th frame
   switch(temp_state) {
     case PrepareTemp_0:
       #if HAS_TEMP_0
@@ -1580,16 +1593,16 @@ ISR(TIMER0_COMPB_vect) {
   } // temp_count >= OVERSAMPLENR
 
   #ifdef BABYSTEPPING
-    for (uint8_t axis=X_AXIS; axis<=Z_AXIS; axis++) {
-      int curTodo=babystepsTodo[axis]; //get rid of volatile for performance
+    for (uint8_t axis = X_AXIS; axis <= Z_AXIS; axis++) {
+      int curTodo = babystepsTodo[axis]; //get rid of volatile for performance
      
       if (curTodo > 0) {
         babystep(axis,/*fwd*/true);
-        babystepsTodo[axis]--; //less to do next time
+        babystepsTodo[axis]--; //fewer to do next time
       }
-      else if(curTodo < 0) {
+      else if (curTodo < 0) {
         babystep(axis,/*fwd*/false);
-        babystepsTodo[axis]++; //less to do next time
+        babystepsTodo[axis]++; //fewer to do next time
       }
     }
   #endif //BABYSTEPPING
diff --git a/Marlin/temperature.h b/Marlin/temperature.h
index 847c41b009a525ee22ea0f24e906ced921e78315..f9effaa9ed48c842ee8938bcf4e1e9f58ba5f0a2 100644
--- a/Marlin/temperature.h
+++ b/Marlin/temperature.h
@@ -129,7 +129,7 @@ HOTEND_ROUTINES(0);
 #endif
 
 int getHeaterPower(int heater);
-void disable_heater();
+void disable_all_heaters();
 void setWatch();
 void updatePID();
 
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index caaf55ab74866b4537851e5a659d9145391405e4..4dd32eb0def2aae884012f919741f5f516dee85a 100644
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -5,7 +5,7 @@
 #include "cardreader.h"
 #include "temperature.h"
 #include "stepper.h"
-#include "ConfigurationStore.h"
+#include "configuration_store.h"
 
 int8_t encoderDiff; /* encoderDiff is updated from interrupt context and added to encoderPosition every LCD update */
 
@@ -22,7 +22,7 @@ int absPreheatHPBTemp;
 int absPreheatFanSpeed;
 
 #ifdef FILAMENT_LCD_DISPLAY
-  unsigned long message_millis = 0;
+  millis_t previous_lcd_status_ms = 0;
 #endif
 
 /* !Configuration settings */
@@ -58,7 +58,7 @@ static void lcd_status_screen();
   static void lcd_control_temperature_preheat_abs_settings_menu();
   static void lcd_control_motion_menu();
   static void lcd_control_volumetric_menu();
-  #ifdef DOGLCD
+  #ifdef HAS_LCD_CONTRAST
     static void lcd_set_contrast();
   #endif
   #ifdef FWRETRACT
@@ -77,8 +77,6 @@ static void lcd_status_screen();
     static void lcd_level_bed();
   #endif
 
-  static void lcd_quick_feedback();//Cause an LCD refresh, and give the user visual or audible feedback that something has happened
-
   /* Different types of actions that can be used in menu items. */
   static void menu_action_back(menuFunc_t data);
   static void menu_action_submenu(menuFunc_t data);
@@ -154,10 +152,10 @@ static void lcd_status_screen();
    *     lcd_implementation_drawmenu_function(sel, row, PSTR(MSG_PAUSE_PRINT), lcd_sdcard_pause)
    *     menu_action_function(lcd_sdcard_pause)
    *
-   *   MENU_ITEM_EDIT(int3, MSG_SPEED, &feedmultiply, 10, 999)
-   *   MENU_ITEM(setting_edit_int3, MSG_SPEED, PSTR(MSG_SPEED), &feedmultiply, 10, 999)
-   *     lcd_implementation_drawmenu_setting_edit_int3(sel, row, PSTR(MSG_SPEED), PSTR(MSG_SPEED), &feedmultiply, 10, 999)
-   *     menu_action_setting_edit_int3(PSTR(MSG_SPEED), &feedmultiply, 10, 999)
+   *   MENU_ITEM_EDIT(int3, MSG_SPEED, &feedrate_multiplier, 10, 999)
+   *   MENU_ITEM(setting_edit_int3, MSG_SPEED, PSTR(MSG_SPEED), &feedrate_multiplier, 10, 999)
+   *     lcd_implementation_drawmenu_setting_edit_int3(sel, row, PSTR(MSG_SPEED), PSTR(MSG_SPEED), &feedrate_multiplier, 10, 999)
+   *     menu_action_setting_edit_int3(PSTR(MSG_SPEED), &feedrate_multiplier, 10, 999)
    *
    */
   #define MENU_ITEM(type, label, args...) do { \
@@ -175,6 +173,9 @@ static void lcd_status_screen();
   } while(0)
 
   #ifdef ENCODER_RATE_MULTIPLIER
+
+    //#define ENCODER_RATE_MULTIPLIER_DEBUG  // If defined, output the encoder steps per second value
+
     /**
      * MENU_MULTIPLIER_ITEM generates drawing and handling code for a multiplier menu item
      */
@@ -220,7 +221,7 @@ static void lcd_status_screen();
     volatile uint8_t slow_buttons; // Bits of the pressed buttons.
   #endif
   uint8_t currentMenuViewOffset;              /* scroll offset in the current menu */
-  uint32_t blocking_enc;
+  millis_t next_button_update_ms;
   uint8_t lastEncoderBits;
   uint32_t encoderPosition;
   #if (SDCARDDETECT > 0)
@@ -230,7 +231,7 @@ static void lcd_status_screen();
 #endif // ULTIPANEL
 
 menuFunc_t currentMenu = lcd_status_screen; /* function pointer to the currently active menu */
-uint32_t lcd_next_update_millis;
+millis_t next_lcd_update_ms;
 uint8_t lcd_status_update_delay;
 bool ignore_click = false;
 bool wait_for_unclick;
@@ -248,12 +249,16 @@ menuFunc_t callbackFunc;
 // place-holders for Ki and Kd edits
 float raw_Ki, raw_Kd;
 
-static void lcd_goto_menu(menuFunc_t menu, const uint32_t encoder=0, const bool feedback=true) {
+/**
+ * General function to go directly to a menu
+ */
+static void lcd_goto_menu(menuFunc_t menu, const bool feedback=false, const uint32_t encoder=0) {
   if (currentMenu != menu) {
     currentMenu = menu;
-    encoderPosition = encoder;
-    if (feedback) lcd_quick_feedback();
-
+    #ifdef NEWPANEL
+      encoderPosition = encoder;
+      if (feedback) lcd_quick_feedback();
+    #endif
     // For LCD_PROGRESS_BAR re-initialize the custom characters
     #ifdef LCD_PROGRESS_BAR
       lcd_set_custom_characters(menu == lcd_status_screen);
@@ -261,111 +266,109 @@ 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 */
+/**
+ *
+ * "Info Screen"
+ *
+ * This is very display-dependent, so the lcd implementation draws this.
+ */
+
 static void lcd_status_screen() {
 	encoderRateMultiplierEnabled = false;
 
   #ifdef LCD_PROGRESS_BAR
-    unsigned long ms = millis();
+    millis_t ms = millis();
     #ifndef PROGRESS_MSG_ONCE
-      if (ms > progressBarTick + PROGRESS_BAR_MSG_TIME + PROGRESS_BAR_BAR_TIME) {
-        progressBarTick = ms;
+      if (ms > progress_bar_ms + PROGRESS_BAR_MSG_TIME + PROGRESS_BAR_BAR_TIME) {
+        progress_bar_ms = ms;
       }
     #endif
     #if PROGRESS_MSG_EXPIRE > 0
       // Handle message expire
-      if (expireStatusMillis > 0) {
+      if (expire_status_ms > 0) {
         if (card.isFileOpen()) {
           // Expire the message when printing is active
           if (IS_SD_PRINTING) {
             // Expire the message when printing is active
-            if (ms >= expireStatusMillis) {
+            if (ms >= expire_status_ms) {
               lcd_status_message[0] = '\0';
-              expireStatusMillis = 0;
+              expire_status_ms = 0;
             }
           }
           else {
-            expireStatusMillis += LCD_UPDATE_INTERVAL;
+            expire_status_ms += LCD_UPDATE_INTERVAL;
           }
         }
         else {
-          expireStatusMillis = 0;
+          expire_status_ms = 0;
         }
       }
     #endif
   #endif //LCD_PROGRESS_BAR
 
-    lcd_implementation_status_screen();
+  lcd_implementation_status_screen();
 
-#ifdef ULTIPANEL
+  #ifdef ULTIPANEL
 
     bool current_click = LCD_CLICKED;
 
     if (ignore_click) {
-        if (wait_for_unclick) {
-          if (!current_click) {
-              ignore_click = wait_for_unclick = false;
-          }
-          else {
-              current_click = false;
-          }
-        }
-        else if (current_click) {
-            lcd_quick_feedback();
-            wait_for_unclick = true;
-            current_click = false;
-        }
+      if (wait_for_unclick) {
+        if (!current_click)
+          ignore_click = wait_for_unclick = false;
+        else
+          current_click = false;
+      }
+      else if (current_click) {
+        lcd_quick_feedback();
+        wait_for_unclick = true;
+        current_click = false;
+      }
     }
 
-    if (current_click)
-    {
-        lcd_goto_menu(lcd_main_menu);
-        lcd_implementation_init( // to maybe revive the LCD if static electricity killed it.
-          #ifdef LCD_PROGRESS_BAR
-            currentMenu == lcd_status_screen
-          #endif
-        );
-        #ifdef FILAMENT_LCD_DISPLAY
-          message_millis = millis();  // get status message to show up for a while
+    if (current_click) {
+      lcd_goto_menu(lcd_main_menu, true);
+      lcd_implementation_init( // to maybe revive the LCD if static electricity killed it.
+        #ifdef LCD_PROGRESS_BAR
+          currentMenu == lcd_status_screen
         #endif
+      );
+      #ifdef FILAMENT_LCD_DISPLAY
+        previous_lcd_status_ms = millis();  // get status message to show up for a while
+      #endif
     }
 
-#ifdef ULTIPANEL_FEEDMULTIPLY
-    // Dead zone at 100% feedrate
-    if ((feedmultiply < 100 && (feedmultiply + int(encoderPosition)) > 100) ||
-            (feedmultiply > 100 && (feedmultiply + int(encoderPosition)) < 100))
-    {
-        encoderPosition = 0;
-        feedmultiply = 100;
-    }
-
-    if (feedmultiply == 100 && int(encoderPosition) > ENCODER_FEEDRATE_DEADZONE)
-    {
-        feedmultiply += int(encoderPosition) - ENCODER_FEEDRATE_DEADZONE;
+    #ifdef ULTIPANEL_FEEDMULTIPLY
+      // Dead zone at 100% feedrate
+      if ((feedrate_multiplier < 100 && (feedrate_multiplier + int(encoderPosition)) > 100) ||
+              (feedrate_multiplier > 100 && (feedrate_multiplier + int(encoderPosition)) < 100)) {
         encoderPosition = 0;
-    }
-    else if (feedmultiply == 100 && int(encoderPosition) < -ENCODER_FEEDRATE_DEADZONE)
-    {
-        feedmultiply += int(encoderPosition) + ENCODER_FEEDRATE_DEADZONE;
-        encoderPosition = 0;
-    }
-    else if (feedmultiply != 100)
-    {
-        feedmultiply += int(encoderPosition);
+        feedrate_multiplier = 100;
+      }
+      if (feedrate_multiplier == 100) {
+        if (int(encoderPosition) > ENCODER_FEEDRATE_DEADZONE) {
+          feedrate_multiplier += int(encoderPosition) - ENCODER_FEEDRATE_DEADZONE;
+          encoderPosition = 0;
+        }
+        else if (int(encoderPosition) < -ENCODER_FEEDRATE_DEADZONE) {
+          feedrate_multiplier += int(encoderPosition) + ENCODER_FEEDRATE_DEADZONE;
+          encoderPosition = 0;
+        }
+      }
+      else {
+        feedrate_multiplier += int(encoderPosition);
         encoderPosition = 0;
-    }
-#endif //ULTIPANEL_FEEDMULTIPLY
+      }
+    #endif // ULTIPANEL_FEEDMULTIPLY
 
-    if (feedmultiply < 10)
-        feedmultiply = 10;
-    else if (feedmultiply > 999)
-        feedmultiply = 999;
-#endif //ULTIPANEL
+    feedrate_multiplier = constrain(feedrate_multiplier, 10, 999);
+
+  #endif //ULTIPANEL
 }
 
 #ifdef ULTIPANEL
 
-static void lcd_return_to_status() { lcd_goto_menu(lcd_status_screen, 0, false); }
+static void lcd_return_to_status() { lcd_goto_menu(lcd_status_screen); }
 
 static void lcd_sdcard_pause() { card.pauseSDPrint(); }
 
@@ -380,7 +383,12 @@ static void lcd_sdcard_stop() {
   lcd_setstatus(MSG_PRINT_ABORTED, true);
 }
 
-/* Menu implementation */
+/**
+ *
+ * "Main" menu
+ *
+ */
+
 static void lcd_main_menu() {
   START_MENU();
   MENU_ITEM(back, MSG_WATCH, lcd_status_screen);
@@ -440,7 +448,7 @@ void lcd_set_home_offsets() {
   plan_set_position(0.0, 0.0, 0.0, current_position[E_AXIS]);
 
   // Audio feedback
-  enquecommands_P(PSTR("M300 S659 P200\nM300 S698 P200"));
+  enqueuecommands_P(PSTR("M300 S659 P200\nM300 S698 P200"));
   lcd_return_to_status();
 }
 
@@ -462,10 +470,16 @@ void lcd_set_home_offsets() {
 
 #endif //BABYSTEPPING
 
+/**
+ *
+ * "Tune" submenu
+ *
+ */
+
 static void lcd_tune_menu() {
   START_MENU();
   MENU_ITEM(back, MSG_MAIN, lcd_main_menu);
-  MENU_ITEM_EDIT(int3, MSG_SPEED, &feedmultiply, 10, 999);
+  MENU_ITEM_EDIT(int3, MSG_SPEED, &feedrate_multiplier, 10, 999);
   #if TEMP_SENSOR_0 != 0
     MENU_MULTIPLIER_ITEM_EDIT(int3, MSG_NOZZLE, &target_temperature[0], 0, HEATER_0_MAXTEMP - 15);
   #endif
@@ -603,17 +617,55 @@ void lcd_cooldown() {
   lcd_return_to_status();
 }
 
+/**
+ *
+ * "Prepare" submenu
+ *
+ */
+
 static void lcd_prepare_menu() {
   START_MENU();
+
+  //
+  // ^ Main
+  //
   MENU_ITEM(back, MSG_MAIN, lcd_main_menu);
-  #if defined( SDSUPPORT ) && defined( MENU_ADDAUTOSTART )
-    MENU_ITEM(function, MSG_AUTOSTART, lcd_autostart_sd);
-  #endif
-  MENU_ITEM(gcode, MSG_DISABLE_STEPPERS, PSTR("M84"));
+
+  //
+  // Auto Home
+  //
   MENU_ITEM(gcode, MSG_AUTO_HOME, PSTR("G28"));
+
+  //
+  // Set Home Offsets
+  //
   MENU_ITEM(function, MSG_SET_HOME_OFFSETS, lcd_set_home_offsets);
   //MENU_ITEM(gcode, MSG_SET_ORIGIN, PSTR("G92 X0 Y0 Z0"));
 
+  //
+  // Level Bed
+  //
+  #ifdef ENABLE_AUTO_BED_LEVELING
+    if (axis_known_position[X_AXIS] && axis_known_position[Y_AXIS])
+      MENU_ITEM(gcode, MSG_LEVEL_BED, PSTR("G29"));
+  #elif defined(MANUAL_BED_LEVELING)
+    MENU_ITEM(submenu, MSG_LEVEL_BED, lcd_level_bed);
+  #endif
+
+  //
+  // Move Axis
+  //
+  MENU_ITEM(submenu, MSG_MOVE_AXIS, lcd_move_menu);
+
+  //
+  // Disable Steppers
+  //
+  MENU_ITEM(gcode, MSG_DISABLE_STEPPERS, PSTR("M84"));
+
+  //
+  // Preheat PLA
+  // Preheat ABS
+  //
   #if TEMP_SENSOR_0 != 0
     #if TEMP_SENSOR_1 != 0 || TEMP_SENSOR_2 != 0 || TEMP_SENSOR_3 != 0 || TEMP_SENSOR_BED != 0
       MENU_ITEM(submenu, MSG_PREHEAT_PLA, lcd_preheat_pla_menu);
@@ -624,8 +676,14 @@ static void lcd_prepare_menu() {
     #endif
   #endif
 
+  //
+  // Cooldown
+  //
   MENU_ITEM(function, MSG_COOLDOWN, lcd_cooldown);
 
+  //
+  // Switch power on/off
+  //
   #if HAS_POWER_SWITCH
     if (powersupply)
       MENU_ITEM(gcode, MSG_SWITCH_PS_OFF, PSTR("M81"));
@@ -633,18 +691,19 @@ static void lcd_prepare_menu() {
       MENU_ITEM(gcode, MSG_SWITCH_PS_ON, PSTR("M80"));
   #endif
 
-  MENU_ITEM(submenu, MSG_MOVE_AXIS, lcd_move_menu);
-
-  #if defined(MANUAL_BED_LEVELING)
-    MENU_ITEM(submenu, MSG_LEVEL_BED, lcd_level_bed);
+  //
+  // Autostart
+  //
+  #if defined(SDSUPPORT) && defined(MENU_ADDAUTOSTART)
+    MENU_ITEM(function, MSG_AUTOSTART, lcd_autostart_sd);
   #endif
-	
+
   END_MENU();
 }
 
 #ifdef DELTA_CALIBRATION_MENU
-static void lcd_delta_calibrate_menu()
-{
+
+  static void lcd_delta_calibrate_menu() {
     START_MENU();
     MENU_ITEM(back, MSG_MAIN, lcd_main_menu);
     MENU_ITEM(gcode, MSG_AUTO_HOME, PSTR("G28"));
@@ -653,25 +712,36 @@ static void lcd_delta_calibrate_menu()
     MENU_ITEM(gcode, MSG_DELTA_CALIBRATE_Z, PSTR("G0 F8000 X0 Y90 Z0"));
     MENU_ITEM(gcode, MSG_DELTA_CALIBRATE_CENTER, PSTR("G0 F8000 X0 Y0 Z0"));
     END_MENU();
-}
+  }
+
 #endif // DELTA_CALIBRATION_MENU
 
+inline void line_to_current(AxisEnum axis) {
+  #ifdef DELTA
+    calculate_delta(current_position);
+    plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], manual_feedrate[axis]/60, active_extruder);
+  #else
+    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[axis]/60, active_extruder);
+  #endif
+}
+
+/**
+ *
+ * "Prepare" > "Move Axis" submenu
+ *
+ */
+
 float move_menu_scale;
 static void lcd_move_menu_axis();
 
-static void _lcd_move(const char *name, int axis, int min, int max) {
+static void _lcd_move(const char *name, AxisEnum axis, int min, int max) {
   if (encoderPosition != 0) {
     refresh_cmd_timeout();
     current_position[axis] += float((int)encoderPosition) * move_menu_scale;
     if (min_software_endstops && current_position[axis] < min) current_position[axis] = min;
     if (max_software_endstops && current_position[axis] > max) current_position[axis] = max;
     encoderPosition = 0;
-    #ifdef DELTA
-      calculate_delta(current_position);
-      plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], manual_feedrate[axis]/60, active_extruder);
-    #else
-      plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[axis]/60, active_extruder);
-    #endif
+    line_to_current(axis);
     lcdDrawUpdate = 1;
   }
   if (lcdDrawUpdate) lcd_implementation_drawedit(name, ftostr31(current_position[axis]));
@@ -680,23 +750,23 @@ static void _lcd_move(const char *name, int axis, int min, int max) {
 static void lcd_move_x() { _lcd_move(PSTR("X"), X_AXIS, X_MIN_POS, X_MAX_POS); }
 static void lcd_move_y() { _lcd_move(PSTR("Y"), Y_AXIS, Y_MIN_POS, Y_MAX_POS); }
 static void lcd_move_z() { _lcd_move(PSTR("Z"), Z_AXIS, Z_MIN_POS, Z_MAX_POS); }
-
 static void lcd_move_e() {
   if (encoderPosition != 0) {
     current_position[E_AXIS] += float((int)encoderPosition) * move_menu_scale;
     encoderPosition = 0;
-    #ifdef DELTA
-      calculate_delta(current_position);
-      plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], manual_feedrate[E_AXIS]/60, active_extruder);
-    #else
-      plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[E_AXIS]/60, active_extruder);
-    #endif
+    line_to_current(E_AXIS);
     lcdDrawUpdate = 1;
   }
   if (lcdDrawUpdate) lcd_implementation_drawedit(PSTR("Extruder"), ftostr31(current_position[E_AXIS]));
   if (LCD_CLICKED) lcd_goto_menu(lcd_move_menu_axis);
 }
 
+/**
+ *
+ * "Prepare" > "Move Xmm" > "Move XYZ" submenu
+ *
+ */
+
 static void lcd_move_menu_axis() {
   START_MENU();
   MENU_ITEM(back, MSG_MOVE_AXIS, lcd_move_menu);
@@ -722,6 +792,12 @@ static void lcd_move_menu_01mm() {
   lcd_move_menu_axis();
 }
 
+/**
+ *
+ * "Prepare" > "Move Axis" submenu
+ *
+ */
+
 static void lcd_move_menu() {
   START_MENU();
   MENU_ITEM(back, MSG_PREPARE, lcd_prepare_menu);
@@ -732,6 +808,12 @@ static void lcd_move_menu() {
   END_MENU();
 }
 
+/**
+ *
+ * "Control" submenu
+ *
+ */
+
 static void lcd_control_menu() {
   START_MENU();
   MENU_ITEM(back, MSG_MAIN, lcd_main_menu);
@@ -739,7 +821,7 @@ static void lcd_control_menu() {
   MENU_ITEM(submenu, MSG_MOTION, lcd_control_motion_menu);
   MENU_ITEM(submenu, MSG_VOLUMETRIC, lcd_control_volumetric_menu);
 
-  #ifdef DOGLCD
+  #ifdef HAS_LCD_CONTRAST
     //MENU_ITEM_EDIT(int3, MSG_CONTRAST, &lcd_contrast, 0, 63);
     MENU_ITEM(submenu, MSG_CONTRAST, lcd_set_contrast);
   #endif
@@ -754,6 +836,12 @@ static void lcd_control_menu() {
   END_MENU();
 }
 
+/**
+ *
+ * "Temperature" submenu
+ *
+ */
+
 #ifdef PIDTEMP
 
   // Helpers for editing PID Ki & Kd values
@@ -785,6 +873,12 @@ static void lcd_control_menu() {
 
 #endif //PIDTEMP
 
+/**
+ *
+ * "Control" > "Temperature" submenu
+ *
+ */
+
 static void lcd_control_temperature_menu() {
   START_MENU();
   MENU_ITEM(back, MSG_CONTROL, lcd_control_menu);
@@ -873,15 +967,21 @@ static void lcd_control_temperature_menu() {
   END_MENU();
 }
 
+/**
+ *
+ * "Temperature" > "Preheat PLA conf" submenu
+ *
+ */
+
 static void lcd_control_temperature_preheat_pla_settings_menu() {
   START_MENU();
   MENU_ITEM(back, MSG_TEMPERATURE, lcd_control_temperature_menu);
   MENU_ITEM_EDIT(int3, MSG_FAN_SPEED, &plaPreheatFanSpeed, 0, 255);
   #if TEMP_SENSOR_0 != 0
-    MENU_ITEM_EDIT(int3, MSG_NOZZLE, &plaPreheatHotendTemp, 0, HEATER_0_MAXTEMP - 15);
+    MENU_ITEM_EDIT(int3, MSG_NOZZLE, &plaPreheatHotendTemp, HEATER_0_MINTEMP, HEATER_0_MAXTEMP - 15);
   #endif
   #if TEMP_SENSOR_BED != 0
-    MENU_ITEM_EDIT(int3, MSG_BED, &plaPreheatHPBTemp, 0, BED_MAXTEMP - 15);
+    MENU_ITEM_EDIT(int3, MSG_BED, &plaPreheatHPBTemp, BED_MINTEMP, BED_MAXTEMP - 15);
   #endif
   #ifdef EEPROM_SETTINGS
     MENU_ITEM(function, MSG_STORE_EPROM, Config_StoreSettings);
@@ -889,15 +989,21 @@ static void lcd_control_temperature_preheat_pla_settings_menu() {
   END_MENU();
 }
 
+/**
+ *
+ * "Temperature" > "Preheat ABS conf" submenu
+ *
+ */
+
 static void lcd_control_temperature_preheat_abs_settings_menu() {
   START_MENU();
   MENU_ITEM(back, MSG_TEMPERATURE, lcd_control_temperature_menu);
   MENU_ITEM_EDIT(int3, MSG_FAN_SPEED, &absPreheatFanSpeed, 0, 255);
   #if TEMP_SENSOR_0 != 0
-    MENU_ITEM_EDIT(int3, MSG_NOZZLE, &absPreheatHotendTemp, 0, HEATER_0_MAXTEMP - 15);
+    MENU_ITEM_EDIT(int3, MSG_NOZZLE, &absPreheatHotendTemp, HEATER_0_MINTEMP, HEATER_0_MAXTEMP - 15);
   #endif
   #if TEMP_SENSOR_BED != 0
-    MENU_ITEM_EDIT(int3, MSG_BED, &absPreheatHPBTemp, 0, BED_MAXTEMP - 15);
+    MENU_ITEM_EDIT(int3, MSG_BED, &absPreheatHPBTemp, BED_MINTEMP, BED_MAXTEMP - 15);
   #endif
   #ifdef EEPROM_SETTINGS
     MENU_ITEM(function, MSG_STORE_EPROM, Config_StoreSettings);
@@ -905,6 +1011,12 @@ static void lcd_control_temperature_preheat_abs_settings_menu() {
   END_MENU();
 }
 
+/**
+ *
+ * "Control" > "Motion" submenu
+ *
+ */
+
 static void lcd_control_motion_menu() {
   START_MENU();
   MENU_ITEM(back, MSG_CONTROL, lcd_control_menu);
@@ -941,6 +1053,12 @@ static void lcd_control_motion_menu() {
   END_MENU();
 }
 
+/**
+ *
+ * "Control" > "Filament" submenu
+ *
+ */
+
 static void lcd_control_volumetric_menu() {
   START_MENU();
   MENU_ITEM(back, MSG_CONTROL, lcd_control_menu);
@@ -963,8 +1081,13 @@ static void lcd_control_volumetric_menu() {
   END_MENU();
 }
 
-#ifdef DOGLCD
+/**
+ *
+ * "Control" > "Contrast" submenu
+ *
+ */
 
+#ifdef HAS_LCD_CONTRAST
   static void lcd_set_contrast() {
     if (encoderPosition != 0) {
       lcd_contrast -= encoderPosition;
@@ -976,11 +1099,15 @@ static void lcd_control_volumetric_menu() {
     if (lcdDrawUpdate) lcd_implementation_drawedit(PSTR(MSG_CONTRAST), itostr2(lcd_contrast));
     if (LCD_CLICKED) lcd_goto_menu(lcd_control_menu);
   }
+#endif // HAS_LCD_CONTRAST
 
-#endif // DOGLCD
+/**
+ *
+ * "Control" > "Retract" submenu
+ *
+ */
 
 #ifdef FWRETRACT
-
   static void lcd_control_retract_menu() {
     START_MENU();
     MENU_ITEM(back, MSG_CONTROL, lcd_control_menu);
@@ -998,16 +1125,13 @@ static void lcd_control_volumetric_menu() {
     MENU_ITEM_EDIT(float3, MSG_CONTROL_RETRACT_RECOVERF, &retract_recover_feedrate, 1, 999);
     END_MENU();
   }
-
 #endif // FWRETRACT
 
 #if SDCARDDETECT == -1
-
   static void lcd_sd_refresh() {
     card.initsd();
     currentMenuViewOffset = 0;
   }
-
 #endif
 
 static void lcd_sd_updir() {
@@ -1015,6 +1139,12 @@ static void lcd_sd_updir() {
   currentMenuViewOffset = 0;
 }
 
+/**
+ *
+ * "Print from SD" submenu
+ *
+ */
+
 void lcd_sdcard_menu() {
   if (lcdDrawUpdate == 0 && LCD_CLICKED == 0) return;	// nothing to do (so don't thrash the SD card)
   uint16_t fileCnt = card.getnrfilenames();
@@ -1050,6 +1180,11 @@ void lcd_sdcard_menu() {
   END_MENU();
 }
 
+/**
+ *
+ * Functions for editing single values
+ *
+ */
 #define menu_edit_type(_type, _name, _strFunc, scale) \
   bool _menu_edit_ ## _name () { \
     bool isClicked = LCD_CLICKED; \
@@ -1096,6 +1231,11 @@ menu_edit_type(float, float51, ftostr51, 10)
 menu_edit_type(float, float52, ftostr52, 100)
 menu_edit_type(unsigned long, long5, ftostr5, 0.01)
 
+/**
+ *
+ * Handlers for RepRap World Keypad input
+ *
+ */
 #ifdef REPRAPWORLD_KEYPAD
   static void reprapworld_keypad_move_z_up() {
     encoderPosition = 1;
@@ -1128,15 +1268,19 @@ menu_edit_type(unsigned long, long5, ftostr5, 0.01)
     lcd_move_y();
   }
   static void reprapworld_keypad_move_home() {
-    enquecommands_P((PSTR("G28"))); // move all axis home
+    enqueuecommands_P((PSTR("G28"))); // move all axis home
   }
-#endif //REPRAPWORLD_KEYPAD
+#endif // REPRAPWORLD_KEYPAD
 
-/** End of menus **/
 
-static void lcd_quick_feedback() {
+/**
+ *
+ * Audio feedback for controller clicks
+ *
+ */
+void lcd_quick_feedback() {
   lcdDrawUpdate = 2;
-  blocking_enc = millis() + 500;
+  next_button_update_ms = millis() + 500;
     
   #ifdef LCD_USE_I2C_BUZZER
     #ifndef LCD_FEEDBACK_FREQUENCY_HZ
@@ -1147,38 +1291,37 @@ static void lcd_quick_feedback() {
     #endif    
     lcd_buzz(LCD_FEEDBACK_FREQUENCY_DURATION_MS, LCD_FEEDBACK_FREQUENCY_HZ);
   #elif defined(BEEPER) && BEEPER > -1
-    SET_OUTPUT(BEEPER);
     #ifndef LCD_FEEDBACK_FREQUENCY_HZ
       #define LCD_FEEDBACK_FREQUENCY_HZ 5000
     #endif
     #ifndef LCD_FEEDBACK_FREQUENCY_DURATION_MS
       #define LCD_FEEDBACK_FREQUENCY_DURATION_MS 2
     #endif
-    const unsigned int delay = 1000000 / LCD_FEEDBACK_FREQUENCY_HZ / 2;
-    int i = LCD_FEEDBACK_FREQUENCY_DURATION_MS * LCD_FEEDBACK_FREQUENCY_HZ / 1000;
-    while (i--) {
-      WRITE(BEEPER,HIGH);
-      delayMicroseconds(delay);
-      WRITE(BEEPER,LOW);
-      delayMicroseconds(delay);
-    }
-    const int j = max(10000 - LCD_FEEDBACK_FREQUENCY_DURATION_MS * 1000, 0);
-    if (j) delayMicroseconds(j);
+    lcd_buzz(LCD_FEEDBACK_FREQUENCY_DURATION_MS, LCD_FEEDBACK_FREQUENCY_HZ);
+  #else
+    #ifndef LCD_FEEDBACK_FREQUENCY_DURATION_MS
+      #define LCD_FEEDBACK_FREQUENCY_DURATION_MS 2
+    #endif
+    delay(LCD_FEEDBACK_FREQUENCY_DURATION_MS);
   #endif
 }
 
-/** Menu action functions **/
-static void menu_action_back(menuFunc_t data) { lcd_goto_menu(data); }
-static void menu_action_submenu(menuFunc_t data) { lcd_goto_menu(data); }
-static void menu_action_gcode(const char* pgcode) { enquecommands_P(pgcode); }
-static void menu_action_function(menuFunc_t data) { (*data)(); }
+/**
+ *
+ * Menu actions
+ *
+ */
+static void menu_action_back(menuFunc_t func) { lcd_goto_menu(func); }
+static void menu_action_submenu(menuFunc_t func) { lcd_goto_menu(func); }
+static void menu_action_gcode(const char* pgcode) { enqueuecommands_P(pgcode); }
+static void menu_action_function(menuFunc_t func) { (*func)(); }
 static void menu_action_sdfile(const char* filename, char* longFilename) {
   char cmd[30];
   char* c;
   sprintf_P(cmd, PSTR("M23 %s"), filename);
   for(c = &cmd[4]; *c; c++) *c = tolower(*c);
-  enquecommand(cmd);
-  enquecommands_P(PSTR("M24"));
+  enqueuecommand(cmd);
+  enqueuecommands_P(PSTR("M24"));
   lcd_return_to_status();
 }
 static void menu_action_sddirectory(const char* filename, char* longFilename) {
@@ -1264,8 +1407,23 @@ int lcd_strlen_P(const char *s) {
   return j;
 }
 
+/**
+ * Update the LCD, read encoder buttons, etc.
+ *   - Read button states
+ *   - Check the SD Card slot state
+ *   - Act on RepRap World keypad input
+ *   - Update the encoder position
+ *   - Apply acceleration to the encoder position
+ *   - Reset the Info Screen timeout if there's any input
+ *   - Update status indicators, if any
+ *   - Clear the LCD if lcdDrawUpdate == 2
+ *
+ * Warning: This function is called from interrupt context!
+ */
 void lcd_update() {
-  static unsigned long timeoutToStatus = 0;
+  #ifdef ULTIPANEL
+    static millis_t return_to_status_ms = 0;
+  #endif
 
   #ifdef LCD_HAS_SLOW_BUTTONS
     slow_buttons = lcd_implementation_read_slow_buttons(); // buttons which take too long to read in interrupt context
@@ -1294,8 +1452,8 @@ void lcd_update() {
     }
   #endif//CARDINSERTED
   
-  uint32_t ms = millis();
-  if (ms > lcd_next_update_millis) {
+  millis_t ms = millis();
+  if (ms > next_lcd_update_ms) {
 
     #ifdef ULTIPANEL
 
@@ -1347,7 +1505,7 @@ void lcd_update() {
           encoderPosition += (encoderDiff * encoderMultiplier) / ENCODER_PULSES_PER_STEP;
           encoderDiff = 0;
         }
-        timeoutToStatus = ms + LCD_TIMEOUT_TO_STATUS;
+        return_to_status_ms = ms + LCD_TIMEOUT_TO_STATUS;
         lcdDrawUpdate = 1;
       }
     #endif //ULTIPANEL
@@ -1383,20 +1541,24 @@ void lcd_update() {
     #endif
 
     #ifdef ULTIPANEL
+
+      // Return to Status Screen after a timeout
       if (currentMenu != lcd_status_screen &&
-        #if defined(MANUAL_BED_LEVELING)
-          currentMenu != _lcd_level_bed && 
-          currentMenu != _lcd_level_bed_homing && 
-        #endif  // MANUAL_BED_LEVELING
-          millis() > timeoutToStatus) {
+        #ifdef MANUAL_BED_LEVELING
+          currentMenu != _lcd_level_bed &&
+          currentMenu != _lcd_level_bed_homing &&
+        #endif
+        millis() > return_to_status_ms
+      ) {
         lcd_return_to_status();
         lcdDrawUpdate = 2;
       }
-    #endif //ULTIPANEL
+
+    #endif // ULTIPANEL
 
     if (lcdDrawUpdate == 2) lcd_implementation_clear();
     if (lcdDrawUpdate) lcdDrawUpdate--;
-    lcd_next_update_millis = millis() + LCD_UPDATE_INTERVAL;
+    next_lcd_update_ms = ms + LCD_UPDATE_INTERVAL;
   }
 }
 
@@ -1407,20 +1569,20 @@ void lcd_ignore_click(bool b) {
 
 void lcd_finishstatus(bool persist=false) {
   #ifdef LCD_PROGRESS_BAR
-    progressBarTick = millis();
+    progress_bar_ms = millis();
     #if PROGRESS_MSG_EXPIRE > 0
-      expireStatusMillis = persist ? 0 : progressBarTick + PROGRESS_MSG_EXPIRE;
+      expire_status_ms = persist ? 0 : progress_bar_ms + PROGRESS_MSG_EXPIRE;
     #endif
   #endif
   lcdDrawUpdate = 2;
 
   #ifdef FILAMENT_LCD_DISPLAY
-    message_millis = millis();  //get status message to show up for a while
+    previous_lcd_status_ms = millis();  //get status message to show up for a while
   #endif
 }
 
 #if defined(LCD_PROGRESS_BAR) && PROGRESS_MSG_EXPIRE > 0
-  void dontExpireStatus() { expireStatusMillis = 0; }
+  void dontExpireStatus() { expire_status_ms = 0; }
 #endif
 
 void set_utf_strlen(char *s, uint8_t n) {
@@ -1433,6 +1595,8 @@ void set_utf_strlen(char *s, uint8_t n) {
   s[i] = 0;
 }
 
+bool lcd_hasstatus() { return (lcd_status_message[0] != '\0'); }
+
 void lcd_setstatus(const char* message, bool persist) {
   if (lcd_status_message_level > 0) return;
   strncpy(lcd_status_message, message, 3*LCD_WIDTH);
@@ -1458,7 +1622,7 @@ void lcd_setalertstatuspgm(const char* message) {
 
 void lcd_reset_alert_level() { lcd_status_message_level = 0; }
 
-#ifdef DOGLCD
+#ifdef HAS_LCD_CONTRAST
   void lcd_setcontrast(uint8_t value) {
     lcd_contrast = value & 0x3F;
     u8g.setContrast(lcd_contrast);
@@ -1467,101 +1631,116 @@ void lcd_reset_alert_level() { lcd_status_message_level = 0; }
 
 #ifdef ULTIPANEL
 
-////////////////////////
-// Setup Rotary Encoder Bit Values (for two pin encoders to indicate movement)
-// These values are independent of which pins are used for EN_A and EN_B indications
-// The rotary encoder part is also independent to the chipset used for the LCD
-#if defined(EN_A) && defined(EN_B)
-  #define encrot0 0
-  #define encrot1 2
-  #define encrot2 3
-  #define encrot3 1
-#endif 
-
-/* Warning: This function is called from interrupt context */
-void lcd_buttons_update() {
-  #ifdef NEWPANEL
-    uint8_t newbutton = 0;
-    if (READ(BTN_EN1) == 0) newbutton |= EN_A;
-    if (READ(BTN_EN2) == 0) newbutton |= EN_B;
-    #if BTN_ENC > 0
-      if (millis() > blocking_enc && READ(BTN_ENC) == 0) newbutton |= EN_C;
-    #endif
-    buttons = newbutton;
-    #ifdef LCD_HAS_SLOW_BUTTONS
-      buttons |= slow_buttons;
-    #endif
-    #ifdef REPRAPWORLD_KEYPAD
-      // for the reprapworld_keypad
-      uint8_t newbutton_reprapworld_keypad=0;
+  /**
+   * Setup Rotary Encoder Bit Values (for two pin encoders to indicate movement)
+   * These values are independent of which pins are used for EN_A and EN_B indications
+   * The rotary encoder part is also independent to the chipset used for the LCD
+   */
+  #if defined(EN_A) && defined(EN_B)
+    #define encrot0 0
+    #define encrot1 2
+    #define encrot2 3
+    #define encrot3 1
+  #endif 
+
+  /**
+   * Read encoder buttons from the hardware registers
+   * Warning: This function is called from interrupt context!
+   */
+  void lcd_buttons_update() {
+    #ifdef NEWPANEL
+      uint8_t newbutton = 0;
+      if (READ(BTN_EN1) == 0) newbutton |= EN_A;
+      if (READ(BTN_EN2) == 0) newbutton |= EN_B;
+      #if BTN_ENC > 0
+        if (millis() > next_button_update_ms && READ(BTN_ENC) == 0) newbutton |= EN_C;
+      #endif
+      buttons = newbutton;
+      #ifdef LCD_HAS_SLOW_BUTTONS
+        buttons |= slow_buttons;
+      #endif
+      #ifdef REPRAPWORLD_KEYPAD
+        // for the reprapworld_keypad
+        uint8_t newbutton_reprapworld_keypad=0;
+        WRITE(SHIFT_LD, LOW);
+        WRITE(SHIFT_LD, HIGH);
+        for(int8_t i = 0; i < 8; i++) {
+          newbutton_reprapworld_keypad >>= 1;
+          if (READ(SHIFT_OUT)) newbutton_reprapworld_keypad |= BIT(7);
+          WRITE(SHIFT_CLK, HIGH);
+          WRITE(SHIFT_CLK, LOW);
+        }
+        buttons_reprapworld_keypad=~newbutton_reprapworld_keypad; //invert it, because a pressed switch produces a logical 0
+      #endif
+    #else   //read it from the shift register
+      uint8_t newbutton = 0;
       WRITE(SHIFT_LD, LOW);
       WRITE(SHIFT_LD, HIGH);
-      for(int8_t i = 0; i < 8; i++) {
-        newbutton_reprapworld_keypad >>= 1;
-        if (READ(SHIFT_OUT)) newbutton_reprapworld_keypad |= BIT(7);
+      unsigned char tmp_buttons = 0;
+      for(int8_t i=0; i<8; i++) {
+        newbutton >>= 1;
+        if (READ(SHIFT_OUT)) newbutton |= BIT(7);
         WRITE(SHIFT_CLK, HIGH);
         WRITE(SHIFT_CLK, LOW);
       }
-      buttons_reprapworld_keypad=~newbutton_reprapworld_keypad; //invert it, because a pressed switch produces a logical 0
-    #endif
-  #else   //read it from the shift register
-    uint8_t newbutton = 0;
-    WRITE(SHIFT_LD, LOW);
-    WRITE(SHIFT_LD, HIGH);
-    unsigned char tmp_buttons = 0;
-    for(int8_t i=0; i<8; i++) {
-      newbutton >>= 1;
-      if (READ(SHIFT_OUT)) newbutton |= BIT(7);
-      WRITE(SHIFT_CLK, HIGH);
-      WRITE(SHIFT_CLK, LOW);
-    }
-    buttons = ~newbutton; //invert it, because a pressed switch produces a logical 0
-  #endif //!NEWPANEL
-
-  //manage encoder rotation
-  uint8_t enc=0;
-  if (buttons & EN_A) enc |= B01;
-  if (buttons & EN_B) enc |= B10;
-  if (enc != lastEncoderBits) {
-    switch(enc) {
-      case encrot0:
-        if (lastEncoderBits==encrot3) encoderDiff++;
-        else if (lastEncoderBits==encrot1) encoderDiff--;
-        break;
-      case encrot1:
-        if (lastEncoderBits==encrot0) encoderDiff++;
-        else if (lastEncoderBits==encrot2) encoderDiff--;
-        break;
-      case encrot2:
-        if (lastEncoderBits==encrot1) encoderDiff++;
-        else if (lastEncoderBits==encrot3) encoderDiff--;
-        break;
-      case encrot3:
-        if (lastEncoderBits==encrot2) encoderDiff++;
-        else if (lastEncoderBits==encrot0) encoderDiff--;
-        break;
+      buttons = ~newbutton; //invert it, because a pressed switch produces a logical 0
+    #endif //!NEWPANEL
+
+    //manage encoder rotation
+    uint8_t enc=0;
+    if (buttons & EN_A) enc |= B01;
+    if (buttons & EN_B) enc |= B10;
+    if (enc != lastEncoderBits) {
+      switch(enc) {
+        case encrot0:
+          if (lastEncoderBits==encrot3) encoderDiff++;
+          else if (lastEncoderBits==encrot1) encoderDiff--;
+          break;
+        case encrot1:
+          if (lastEncoderBits==encrot0) encoderDiff++;
+          else if (lastEncoderBits==encrot2) encoderDiff--;
+          break;
+        case encrot2:
+          if (lastEncoderBits==encrot1) encoderDiff++;
+          else if (lastEncoderBits==encrot3) encoderDiff--;
+          break;
+        case encrot3:
+          if (lastEncoderBits==encrot2) encoderDiff++;
+          else if (lastEncoderBits==encrot0) encoderDiff--;
+          break;
+      }
     }
+    lastEncoderBits = enc;
   }
-  lastEncoderBits = enc;
-}
 
-bool lcd_detected(void) {
-  #if (defined(LCD_I2C_TYPE_MCP23017) || defined(LCD_I2C_TYPE_MCP23008)) && defined(DETECT_DEVICE)
-    return lcd.LcdDetected() == 1;
-  #else
-    return true;
-  #endif
-}
+  bool lcd_detected(void) {
+    #if (defined(LCD_I2C_TYPE_MCP23017) || defined(LCD_I2C_TYPE_MCP23008)) && defined(DETECT_DEVICE)
+      return lcd.LcdDetected() == 1;
+    #else
+      return true;
+    #endif
+  }
 
-void lcd_buzz(long duration, uint16_t freq) {
-  #ifdef LCD_USE_I2C_BUZZER
-    lcd.buzz(duration,freq);
-  #endif
-}
+  void lcd_buzz(long duration, uint16_t freq) {
+    if (freq > 0) {
+      #if BEEPER > 0
+        SET_OUTPUT(BEEPER);
+        tone(BEEPER, freq, duration);
+        delay(duration);
+      #elif defined(LCD_USE_I2C_BUZZER)
+        lcd.buzz(duration, freq);
+      #else
+        delay(duration);
+      #endif
+    }
+    else {
+      delay(duration);
+    }
+  }
 
-bool lcd_clicked() { return LCD_CLICKED; }
+  bool lcd_clicked() { return LCD_CLICKED; }
 
-#endif //ULTIPANEL
+#endif // ULTIPANEL
 
 /*********************************/
 /** Number to string conversion **/
@@ -1623,8 +1802,7 @@ char *ftostr32(const float &x) {
 }
 
 // Convert float to string with 1.234 format
-char *ftostr43(const float &x)
-{
+char *ftostr43(const float &x) {
 	long xx = x * 1000;
     if (xx >= 0)
 		conv[0] = (xx / 1000) % 10 + '0';
@@ -1640,8 +1818,7 @@ char *ftostr43(const float &x)
 }
 
 // Convert float to string with 1.23 format
-char *ftostr12ns(const float &x)
-{
+char *ftostr12ns(const float &x) {
   long xx=x*100;
   
   xx=abs(xx);
@@ -1799,76 +1976,87 @@ char *ftostr52(const float &x) {
 }
 
 #ifdef MANUAL_BED_LEVELING
-static int _lcd_level_bed_position;
-static void _lcd_level_bed()
-{
-  if (encoderPosition != 0) {
-    refresh_cmd_timeout();
-    current_position[Z_AXIS] += float((int)encoderPosition) * MBL_Z_STEP;
-    if (min_software_endstops && current_position[Z_AXIS] < Z_MIN_POS) current_position[Z_AXIS] = Z_MIN_POS;
-    if (max_software_endstops && current_position[Z_AXIS] > Z_MAX_POS) current_position[Z_AXIS] = Z_MAX_POS;
-    encoderPosition = 0;
-    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[Z_AXIS]/60, active_extruder);
-    lcdDrawUpdate = 1;
-  }
-  if (lcdDrawUpdate) lcd_implementation_drawedit(PSTR("Z"), ftostr43(current_position[Z_AXIS]));
-  static bool debounce_click = false;
-  if (LCD_CLICKED) {
-    if (!debounce_click) {
-      debounce_click = true;
-      int ix = _lcd_level_bed_position % MESH_NUM_X_POINTS;
-      int iy = _lcd_level_bed_position / MESH_NUM_X_POINTS;
-      if (iy&1) { // Zig zag
-        ix = (MESH_NUM_X_POINTS - 1) - ix;
-      }
-      mbl.set_z(ix, iy, current_position[Z_AXIS]);
-      _lcd_level_bed_position++;
-      if (_lcd_level_bed_position == MESH_NUM_X_POINTS*MESH_NUM_Y_POINTS) {
-        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], manual_feedrate[X_AXIS]/60, active_extruder);
-        mbl.active = 1;
-        enquecommands_P(PSTR("G28"));
-        lcd_return_to_status();
-      } else {
-        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], manual_feedrate[X_AXIS]/60, active_extruder);
-        ix = _lcd_level_bed_position % MESH_NUM_X_POINTS;
-        iy = _lcd_level_bed_position / MESH_NUM_X_POINTS;
-        if (iy&1) { // Zig zag
-          ix = (MESH_NUM_X_POINTS - 1) - ix;
+
+  static int _lcd_level_bed_position;
+
+  /**
+   * MBL Wait for controller movement and clicks:
+   *   - Movement adjusts the Z axis
+   *   - Click saves the Z and goes to the next mesh point
+   */
+  static void _lcd_level_bed() {
+    if (encoderPosition != 0) {
+      refresh_cmd_timeout();
+      current_position[Z_AXIS] += float((int)encoderPosition) * MBL_Z_STEP;
+      if (min_software_endstops && current_position[Z_AXIS] < Z_MIN_POS) current_position[Z_AXIS] = Z_MIN_POS;
+      if (max_software_endstops && current_position[Z_AXIS] > Z_MAX_POS) current_position[Z_AXIS] = Z_MAX_POS;
+      encoderPosition = 0;
+      line_to_current(Z_AXIS);
+      lcdDrawUpdate = 2;
+    }
+    if (lcdDrawUpdate) lcd_implementation_drawedit(PSTR("Z"), ftostr43(current_position[Z_AXIS]));
+    static bool debounce_click = false;
+    if (LCD_CLICKED) {
+      if (!debounce_click) {
+        debounce_click = true;
+        int ix = _lcd_level_bed_position % MESH_NUM_X_POINTS,
+            iy = _lcd_level_bed_position / 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]);
+        _lcd_level_bed_position++;
+        if (_lcd_level_bed_position == MESH_NUM_X_POINTS*MESH_NUM_Y_POINTS) {
+          current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
+          line_to_current(Z_AXIS);
+          mbl.active = 1;
+          enqueuecommands_P(PSTR("G28"));
+          lcd_return_to_status();
+        }
+        else {
+          current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
+          line_to_current(Z_AXIS);
+          ix = _lcd_level_bed_position % MESH_NUM_X_POINTS;
+          iy = _lcd_level_bed_position / 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);
+          line_to_current(manual_feedrate[X_AXIS] <= manual_feedrate[Y_AXIS] ? X_AXIS : Y_AXIS);
+          lcdDrawUpdate = 2;
         }
-        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], manual_feedrate[X_AXIS]/60, active_extruder);
-        lcdDrawUpdate = 1;
       }
     }
-  } else {
-    debounce_click = false;
+    else {
+      debounce_click = false;
+    }
   }
-}
-static void _lcd_level_bed_homing()
-{
-  if (axis_known_position[X_AXIS] &&
-      axis_known_position[Y_AXIS] &&
-      axis_known_position[Z_AXIS]) {
-    current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
-    plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-    current_position[X_AXIS] = MESH_MIN_X;
-    current_position[Y_AXIS] = MESH_MIN_Y;
-    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[X_AXIS]/60, active_extruder);
-    _lcd_level_bed_position = 0;
-    lcd_goto_menu(_lcd_level_bed);
+
+  /**
+   * MBL Move to mesh starting point
+   */
+  static void _lcd_level_bed_homing() {
+    if (lcdDrawUpdate) lcd_implementation_drawedit(PSTR("XYZ"), "Homing");
+    if (axis_known_position[X_AXIS] && axis_known_position[Y_AXIS] && axis_known_position[Z_AXIS]) {
+      current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
+      plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+      current_position[X_AXIS] = MESH_MIN_X;
+      current_position[Y_AXIS] = MESH_MIN_Y;
+      line_to_current(manual_feedrate[X_AXIS] <= manual_feedrate[Y_AXIS] ? X_AXIS : Y_AXIS);
+      _lcd_level_bed_position = 0;
+      lcd_goto_menu(_lcd_level_bed);
+    }
+    lcdDrawUpdate = 2;
   }
-}
-static void lcd_level_bed() {
-  axis_known_position[X_AXIS] = false;
-  axis_known_position[Y_AXIS] = false;
-  axis_known_position[Z_AXIS] = false;
-  mbl.reset();
-  enquecommands_P(PSTR("G28"));
-  lcd_goto_menu(_lcd_level_bed_homing);
-}
+
+  /**
+   * MBL entry-point
+   */
+  static void lcd_level_bed() {
+    axis_known_position[X_AXIS] = axis_known_position[Y_AXIS] = axis_known_position[Z_AXIS] = false;
+    mbl.reset();
+    enqueuecommands_P(PSTR("G28"));
+    lcdDrawUpdate = 2;
+    lcd_goto_menu(_lcd_level_bed_homing);
+  }
+
 #endif  // MANUAL_BED_LEVELING
 
-#endif //ULTRA_LCD
+#endif // ULTRA_LCD
diff --git a/Marlin/ultralcd.h b/Marlin/ultralcd.h
index fe4f5976802ed78023a8df72b021973c916b4d99..8f96f008ba65faa7b3c49eab8b2db5131ec572f6 100644
--- a/Marlin/ultralcd.h
+++ b/Marlin/ultralcd.h
@@ -8,6 +8,7 @@
   int lcd_strlen_P(const char *s);
   void lcd_update();
   void lcd_init();
+  bool lcd_hasstatus();
   void lcd_setstatus(const char* message, const bool persist=false);
   void lcd_setstatuspgm(const char* message, const uint8_t level=0);
   void lcd_setalertstatuspgm(const char* message);
@@ -49,10 +50,11 @@
   extern bool cancel_heatup;
   
   #ifdef FILAMENT_LCD_DISPLAY
-    extern unsigned long message_millis;
+    extern millis_t previous_lcd_status_ms;
   #endif
 
   void lcd_buzz(long duration,uint16_t freq);
+  void lcd_quick_feedback(); // Audible feedback for a button click - could also be visual
   bool lcd_clicked();
 
   void lcd_ignore_click(bool b=true);
@@ -99,6 +101,7 @@
 #else //no LCD
   FORCE_INLINE void lcd_update() {}
   FORCE_INLINE void lcd_init() {}
+  FORCE_INLINE bool lcd_hasstatus() { return false; }
   FORCE_INLINE void lcd_setstatus(const char* message, const bool persist=false) {}
   FORCE_INLINE void lcd_setstatuspgm(const char* message, const uint8_t level=0) {}
   FORCE_INLINE void lcd_buttons_update() {}
@@ -106,8 +109,8 @@
   FORCE_INLINE void lcd_buzz(long duration,uint16_t freq) {}
   FORCE_INLINE bool lcd_detected(void) { return true; }
 
-  #define LCD_MESSAGEPGM(x) 
-  #define LCD_ALERTMESSAGEPGM(x) 
+  #define LCD_MESSAGEPGM(x) do{}while(0)
+  #define LCD_ALERTMESSAGEPGM(x) do{}while(0)
 
 #endif //ULTRA_LCD
 
diff --git a/Marlin/ultralcd_implementation_hitachi_HD44780.h b/Marlin/ultralcd_implementation_hitachi_HD44780.h
index 5698fd94b95cce0b23b742fcb23bbb261c3bc779..eb1f525cc292d93173e73c086ce66da05ff5df8d 100644
--- a/Marlin/ultralcd_implementation_hitachi_HD44780.h
+++ b/Marlin/ultralcd_implementation_hitachi_HD44780.h
@@ -194,25 +194,13 @@
 #include "utf_mapper.h"
 
 #ifdef LCD_PROGRESS_BAR
-  static uint16_t progressBarTick = 0;
+  static millis_t progress_bar_ms = 0;
   #if PROGRESS_MSG_EXPIRE > 0
-    static uint16_t expireStatusMillis = 0;
+    static millis_t expire_status_ms = 0;
   #endif
   #define LCD_STR_PROGRESS  "\x03\x04\x05"
 #endif
 
-/* Custom characters defined in the first 8 characters of the LCD */
-#define LCD_STR_BEDTEMP     "\x00"  // this will have 'unexpected' results when used in a string!
-#define LCD_STR_DEGREE      "\x01"
-#define LCD_STR_THERMOMETER "\x02"
-#define LCD_STR_UPLEVEL     "\x03"
-#define LCD_STR_REFRESH     "\x04"
-#define LCD_STR_FOLDER      "\x05"
-#define LCD_STR_FEEDRATE    "\x06"
-#define LCD_STR_CLOCK       "\x07"
-//#define LCD_STR_ARROW_RIGHT "\x7E"  /* from the default character set. Only available on DISPLAY_CHARSET_HD44780_JAPAN - at this place!*/
-#define LCD_STR_ARROW_RIGHT ">"  /* from the default character set */
-
 static void lcd_set_custom_characters(
   #ifdef LCD_PROGRESS_BAR
     bool progress_bar_set=true
@@ -562,7 +550,7 @@ static void lcd_implementation_status_screen() {
 
     lcd.setCursor(0, 2);
     lcd.print(LCD_STR_FEEDRATE[0]);
-    lcd.print(itostr3(feedmultiply));
+    lcd.print(itostr3(feedrate_multiplier));
     lcd.print('%');
 
     #if LCD_WIDTH > 19 && defined(SDSUPPORT)
@@ -579,8 +567,8 @@ static void lcd_implementation_status_screen() {
 
     lcd.setCursor(LCD_WIDTH - 6, 2);
     lcd.print(LCD_STR_CLOCK[0]);
-    if (starttime != 0) {
-      uint16_t time = millis()/60000 - starttime/60000;
+    if (print_job_start_ms != 0) {
+      uint16_t time = millis()/60000 - print_job_start_ms/60000;
       lcd.print(itostr2(time/60));
       lcd.print(':');
       lcd.print(itostr2(time%60));
@@ -600,8 +588,9 @@ static void lcd_implementation_status_screen() {
   #ifdef LCD_PROGRESS_BAR
 
     if (card.isFileOpen()) {
-      if (millis() >= progressBarTick + PROGRESS_BAR_MSG_TIME || !lcd_status_message[0]) {
-        // draw the progress bar
+      // Draw the progress bar if the message has shown long enough
+      // or if there is no message set.
+      if (millis() >= progress_bar_ms + PROGRESS_BAR_MSG_TIME || !lcd_status_message[0]) {
         int tix = (int)(card.percentDone() * LCD_WIDTH * 3) / 100,
           cel = tix / 3, rem = tix % 3, i = LCD_WIDTH;
         char msg[LCD_WIDTH+1], b = ' ';
@@ -622,7 +611,7 @@ static void lcd_implementation_status_screen() {
 
     // Show Filament Diameter and Volumetric Multiplier %
     // After allowing lcd_status_message to show for 5 seconds
-    if (millis() >= message_millis + 5000) {
+    if (millis() >= previous_lcd_status_ms + 5000) {
       lcd_printPGM(PSTR("Dia "));
       lcd.print(ftostr12ns(filament_width_meas));
       lcd_printPGM(PSTR(" V"));
@@ -736,46 +725,45 @@ static void lcd_implementation_drawmenu_sddirectory(bool sel, uint8_t row, const
 #define lcd_implementation_drawmenu_function(sel, row, pstr, data) lcd_implementation_drawmenu_generic(sel, row, pstr, '>', ' ')
 
 #ifdef LCD_HAS_STATUS_INDICATORS
-static void lcd_implementation_update_indicators()
-{
-  #if defined(LCD_I2C_PANELOLU2) || defined(LCD_I2C_VIKI)
-    //set the LEDS - referred to as backlights by the LiquidTWI2 library 
-    static uint8_t ledsprev = 0;
-    uint8_t leds = 0;
-    if (target_temperature_bed > 0) leds |= LED_A;
-    if (target_temperature[0] > 0) leds |= LED_B;
-    if (fanSpeed) leds |= LED_C;
-    #if EXTRUDERS > 1  
-      if (target_temperature[1] > 0) leds |= LED_C;
+
+  static void lcd_implementation_update_indicators() {
+    #if defined(LCD_I2C_PANELOLU2) || defined(LCD_I2C_VIKI)
+      //set the LEDS - referred to as backlights by the LiquidTWI2 library 
+      static uint8_t ledsprev = 0;
+      uint8_t leds = 0;
+      if (target_temperature_bed > 0) leds |= LED_A;
+      if (target_temperature[0] > 0) leds |= LED_B;
+      if (fanSpeed) leds |= LED_C;
+      #if EXTRUDERS > 1  
+        if (target_temperature[1] > 0) leds |= LED_C;
+      #endif
+      if (leds != ledsprev) {
+        lcd.setBacklight(leds);
+        ledsprev = leds;
+      }
     #endif
-    if (leds != ledsprev) {
-      lcd.setBacklight(leds);
-      ledsprev = leds;
-    }
-  #endif
-}
-#endif
+  }
+
+#endif // LCD_HAS_STATUS_INDICATORS
 
 #ifdef LCD_HAS_SLOW_BUTTONS
-extern uint32_t blocking_enc;
 
-static uint8_t lcd_implementation_read_slow_buttons()
-{
-  #ifdef LCD_I2C_TYPE_MCP23017
-  uint8_t slow_buttons;
-    // Reading these buttons this is likely to be too slow to call inside interrupt context
-    // so they are called during normal lcd_update
-    slow_buttons = lcd.readButtons() << B_I2C_BTN_OFFSET; 
-    #if defined(LCD_I2C_VIKI)
-    if(slow_buttons & (B_MI|B_RI)) { //LCD clicked
-       if(blocking_enc > millis()) {
-         slow_buttons &= ~(B_MI|B_RI); // Disable LCD clicked buttons if screen is updated
-       }
-    }
+  extern millis_t next_button_update_ms;
+
+  static uint8_t lcd_implementation_read_slow_buttons() {
+    #ifdef LCD_I2C_TYPE_MCP23017
+      uint8_t slow_buttons;
+      // Reading these buttons this is likely to be too slow to call inside interrupt context
+      // so they are called during normal lcd_update
+      slow_buttons = lcd.readButtons() << B_I2C_BTN_OFFSET; 
+      #ifdef LCD_I2C_VIKI
+        if ((slow_buttons & (B_MI|B_RI)) && millis() < next_button_update_ms) // LCD clicked
+          slow_buttons &= ~(B_MI|B_RI); // Disable LCD clicked buttons if screen is updated
+      #endif
+      return slow_buttons;
     #endif
-    return slow_buttons; 
-  #endif
-}
-#endif
+  }
+
+#endif // LCD_HAS_SLOW_BUTTONS
 
 #endif //__ULTRALCD_IMPLEMENTATION_HITACHI_HD44780_H
diff --git a/Marlin/ultralcd_st7920_u8glib_rrd.h b/Marlin/ultralcd_st7920_u8glib_rrd.h
index fbf0109824747056920757d106b3554af8e4249a..3181ea2657f10c1ea826801aad2a81bb5919f781 100644
--- a/Marlin/ultralcd_st7920_u8glib_rrd.h
+++ b/Marlin/ultralcd_st7920_u8glib_rrd.h
@@ -16,8 +16,8 @@
 //#define PAGE_HEIGHT 16  //256 byte framebuffer
 #define PAGE_HEIGHT 32  //512 byte framebuffer
 
-#define WIDTH 128
-#define HEIGHT 64
+#define LCD_PIXEL_WIDTH 128
+#define LCD_PIXEL_HEIGHT 64
 
 #include <U8glib.h>
 
@@ -64,12 +64,12 @@ uint8_t u8g_dev_rrd_st7920_128x64_fn(u8g_t *u8g, u8g_dev_t *dev, uint8_t msg, vo
         ST7920_WRITE_BYTE(0x01);       //clear CGRAM ram
         u8g_Delay(15);                 //delay for CGRAM clear
         ST7920_WRITE_BYTE(0x3E);       //extended mode + GDRAM active
-        for(y=0;y<HEIGHT/2;y++)        //clear GDRAM
+        for(y=0;y<LCD_PIXEL_HEIGHT/2;y++)        //clear GDRAM
         {
           ST7920_WRITE_BYTE(0x80|y);   //set y
           ST7920_WRITE_BYTE(0x80);     //set x = 0
           ST7920_SET_DAT();
-          for(i=0;i<2*WIDTH/8;i++)     //2x width clears both segments
+          for(i=0;i<2*LCD_PIXEL_WIDTH/8;i++)     //2x width clears both segments
             ST7920_WRITE_BYTE(0);
           ST7920_SET_CMD();
         }
@@ -103,7 +103,7 @@ uint8_t u8g_dev_rrd_st7920_128x64_fn(u8g_t *u8g, u8g_dev_t *dev, uint8_t msg, vo
           }
 
           ST7920_SET_DAT();
-          ST7920_WRITE_BYTES(ptr,WIDTH/8); //ptr is incremented inside of macro
+          ST7920_WRITE_BYTES(ptr,LCD_PIXEL_WIDTH/8); //ptr is incremented inside of macro
           y++;
         }
         ST7920_NCS();
@@ -119,8 +119,8 @@ uint8_t u8g_dev_rrd_st7920_128x64_fn(u8g_t *u8g, u8g_dev_t *dev, uint8_t msg, vo
 #endif
 }
 
-uint8_t   u8g_dev_st7920_128x64_rrd_buf[WIDTH*(PAGE_HEIGHT/8)] U8G_NOCOMMON;
-u8g_pb_t  u8g_dev_st7920_128x64_rrd_pb = {{PAGE_HEIGHT,HEIGHT,0,0,0},WIDTH,u8g_dev_st7920_128x64_rrd_buf};
+uint8_t   u8g_dev_st7920_128x64_rrd_buf[LCD_PIXEL_WIDTH*(PAGE_HEIGHT/8)] U8G_NOCOMMON;
+u8g_pb_t  u8g_dev_st7920_128x64_rrd_pb = {{PAGE_HEIGHT,LCD_PIXEL_HEIGHT,0,0,0},LCD_PIXEL_WIDTH,u8g_dev_st7920_128x64_rrd_buf};
 u8g_dev_t u8g_dev_st7920_128x64_rrd_sw_spi = {u8g_dev_rrd_st7920_128x64_fn,&u8g_dev_st7920_128x64_rrd_pb,&u8g_com_null_fn};
 
 class U8GLIB_ST7920_128X64_RRD : public U8GLIB
diff --git a/Marlin/watchdog.cpp b/Marlin/watchdog.cpp
index b378ca706abf52b583f5b77f1fed8f98a0ae1cf1..8505f7efac2282377f0e4fb50ca403648162685a 100644
--- a/Marlin/watchdog.cpp
+++ b/Marlin/watchdog.cpp
@@ -7,11 +7,11 @@
 #include "ultralcd.h"
 
 //===========================================================================
-//=============================private variables  ============================
+//============================ private variables ============================
 //===========================================================================
 
 //===========================================================================
-//=============================functinos         ============================
+//================================ functions ================================
 //===========================================================================
 
 
@@ -36,7 +36,7 @@ void watchdog_reset()
 }
 
 //===========================================================================
-//=============================ISR               ============================
+//=================================== ISR ===================================
 //===========================================================================
 
 //Watchdog timer interrupt, called if main program blocks >1sec and manual reset is enabled.
diff --git a/README.md b/README.md
index 7cb93c7e26078f24d7211290f0afc161dbb53db2..9b40193d251edda3547e7d2162a6ef48d12825eb 100644
--- a/README.md
+++ b/README.md
@@ -24,14 +24,13 @@ This firmware is a mashup between [Sprinter](https://github.com/kliment/Sprinter
 ## Current Status: Bug Fixing
 
 The Marlin development is currently revived. There's a long list of reported issues and pull requests, which we are working on currently.
-We are actively looking for testers. So please try the current development version and report new issues and feedback.
 
 [![Coverity Scan Build Status](https://scan.coverity.com/projects/2224/badge.svg)](https://scan.coverity.com/projects/2224)
 [![Travis Build Status](https://travis-ci.org/MarlinFirmware/Marlin.svg)](https://travis-ci.org/MarlinFirmware/Marlin)
 
 ## Contact
 
-__Google Hangout:__ <a href="https://plus.google.com/hangouts/_/g2wp5duzb2y6ahikg6tmwao3kua" target="_blank">Hangout</a>
+__Google Hangout:__ <a href="https://plus.google.com/hangouts/_/gxn3wrea5gdhoo223yimsiforia" target="_blank">Hangout</a>
 
 ## Credits
 
@@ -40,10 +39,9 @@ The current Marlin dev team consists of:
  - Scott Lahteine [@thinkyhead]
  - 
 
-Sprinters lead developers are Kliment and caru.
-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.
+## Donation
+
+If you find our work usefull please consider donating. Donations will be used to pay for our website http://www.marlinfirmware.org/ and to pay some food or rent money for the very active Collaborators
 
 More features have been added by:
   - Lampmaker,
@@ -52,7 +50,7 @@ More features have been added by:
 
 ## License
 
-Marlin is published under the [GPL license](/Documentation/COPYING.md) because I believe in open development.
-Please do not use this code in products (3D printers, CNC etc) that are closed source or are crippled by a patent.
+Marlin is published under the [GPL license](/Documentation/COPYING.md) because We believe in open development.
+Do not use this code in products (3D printers, CNC etc) that are closed source or are crippled by a patent.
 
 [![Flattr this git repo](http://api.flattr.com/button/flattr-badge-large.png)](https://flattr.com/submit/auto?user_id=ErikZalm&url=https://github.com/MarlinFirmware/Marlin&title=Marlin&language=&tags=github&category=software)