Update 4.1.3

c260d3f4 · MagoKimbra · 2347123a · c260d3f4 · c260d3f4 · c260d3f4
Commit c260d3f4 authored Jun 05, 2015 by MagoKimbra
38 changed files
--- a/MarlinKimbra/Configuration.h
+++ b/MarlinKimbra/Configuration.h
 #ifndef CONFIGURATION_H
 #define CONFIGURATION_H
-// This configuration file contains basic settings. Select your:
+#include "boards.h"
-//  - board type
-//  - Mechanism type (cartesian-corexy-delta-scara)
-//  - temperature sensor type
-//
-// Mechanisms-settings can be found in configuration_xxxxxx.h
-// Advanced settings can be found in Configuration_adv.h
+//===========================================================================
+//============================= Getting Started =============================
+//===========================================================================
-#include "boards.h"
+/*
-// Choose your board type.
+ * This configuration file contains basic settings. Select your:
-// Either an numeric ID or name defined in boards.h is valid.
+ * - board type
-// See: https://github.com/MagoKimbra/MarlinKimbra/blob/master/Documentation/Hardware.md
+ * - Mechanism type (cartesian-corexy-delta-scara)
+ * - temperature sensor type
+ *
+ * Mechanisms-settings can be found in configuration_xxxxxx.h
+ * Advanced settings can be found in Configuration_adv.h
+ */
+/*
+ * Choose your board type.
+ * Either an numeric ID or name defined in boards.h is valid.
+ * See: https://github.com/MagoKimbra/MarlinKimbra/blob/master/Documentation/Hardware.md
+ */
 #define MOTHERBOARD BOARD_RAMPS_13_EFB
 // 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 "4.1.2"
+#define STRING_VERSION "4.1.3"
-#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
@@ -33,7 +39,7 @@
 #define SERIAL_PORT 0
 // This determines the communication speed of the printer
-// 115200 - 250000
+// 2400,9600,19200,38400,57600,115200,250000
 #define BAUDRATE 115200
 // This enables the serial port associated to the Bluetooth interface on AT90USB devices
@@ -222,6 +228,7 @@
 #define PID_FUNCTIONAL_RANGE 10         // degC
 #define PID_INTEGRAL_DRIVE_MAX PID_MAX  // Limit for the integral term
 #define K1 0.95                         // Smoothing factor within the PID
+#define MAX_OVERSHOOT_PID_AUTOTUNE 20   // Max valor for overshoot autotune
 //             HotEnd{HE0,HE1,HE2,HE3}
 #define DEFAULT_Kp {40, 40, 40, 40}     // Kp for E0, E1, E2, E3
@@ -253,6 +260,7 @@
 #define MAX_BED_POWER 255 // limits duty cycle to bed; 255=full current
 //#define PID_BED_DEBUG // Sends debug data to the serial port.
+#define PID_BED_INTEGRAL_DRIVE_MAX MAX_BED_POWER // limit for the integral term
 //120v 250W silicone heater into 4mm borosilicate (MendelMax 1.5+)
 //from FOPDT model - kp=.39 Tp=405 Tdead=66, Tc set to 79.2, aggressive factor of .15 (vs .1, 1, 10)
 #define  DEFAULT_bedKp 10.00
@@ -278,7 +286,6 @@
 #define EXTRUDE_MINTEMP 170 // degC
 #define EXTRUDE_MAXLENGTH (X_MAX_LENGTH+Y_MAX_LENGTH) //prevent extrusion of very large distances.
 //===========================================================================
 //======================== Thermal Runaway Protection =======================
 //===========================================================================
@@ -294,30 +301,16 @@
 * The solution: Once the temperature reaches the target, start observing.
 * If the temperature stays too far below the target (hysteresis) for too long,
 * the firmware will halt as a safety precaution.
- *
- * Note that because the countdown starts only AFTER the temperature reaches
- * the target, this will not catch a thermistor that is already disconnected
- * when the print starts!
- *
- * To enable for all extruder heaters, uncomment the two defines below:
 */
-// Parameters for all extruder heaters
+//#define THERMAL_PROTECTION_HOTENDS // Enable thermal protection for all extruders
-#define THERMAL_RUNAWAY_PROTECTION_PERIOD 40    // in seconds
+//#define THERMAL_PROTECTION_BED     // Enable thermal protection for the heated bed
-#define THERMAL_RUNAWAY_PROTECTION_HYSTERESIS 4 // in degree Celsius
-// To enable for the bed heater, uncomment the two defines below:
-// Parameters for the bed heater
-#define THERMAL_RUNAWAY_PROTECTION_BED_PERIOD 20    // in seconds
-#define THERMAL_RUNAWAY_PROTECTION_BED_HYSTERESIS 2 // in degree Celsius
 //===========================================================================
 //============================ User Interfaces ==============================
 //===========================================================================
-//==============================LCD and SD support=============================
+//============================ LCD and SD support ===========================
 // 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.
@@ -352,6 +345,11 @@
 //#define VIKI2
 //#define miniVIKI
+// This is a new controller currently under development.
+// https://github.com/eboston/Adafruit-ST7565-Full-Graphic-Controller/
+// ==> REMEMBER TO INSTALL U8glib to your ARDUINO library folder: http://code.google.com/p/u8glib/wiki/u8glib
+//#define ELB_FULL_GRAPHIC_CONTROLLER
 // The RepRapDiscount Smart Controller (white PCB)
 // http://reprap.org/wiki/RepRapDiscount_Smart_Controller
 //#define REPRAP_DISCOUNT_SMART_CONTROLLER
@@ -376,7 +374,10 @@
 // REMEMBER TO INSTALL LiquidCrystal_I2C.h in your ARDUINO library folder: https://github.com/kiyoshigawa/LiquidCrystal_I2C
 //#define RA_CONTROL_PANEL
-// I2C Panels
+/**
+ * I2C Panels
+ */
 //#define LCD_I2C_SAINSMART_YWROBOT
 // PANELOLU2 LCD with status LEDs, separate encoder and click inputs
@@ -385,11 +386,15 @@
 // Panucatt VIKI LCD with status LEDs, integrated click & L/R/U/P buttons, separate encoder inputs
 //#define LCD_I2C_VIKI
+// SSD1306 OLED generic display support
+// ==> REMEMBER TO INSTALL U8glib to your ARDUINO library folder: http://code.google.com/p/u8glib/wiki/u8glib
+//#define U8GLIB_SSD1306
 // Shift register panels
 // ---------------------
 // 2 wire Non-latching LCD SR from:
 // https://bitbucket.org/fmalpartida/new-liquidcrystal/wiki/schematics#!shiftregister-connection
+// LCD configuration: http://reprap.org/wiki/SAV_3D_LCD
 //#define SAV_3DLCD
 // option for invert rotary switch
@@ -402,13 +407,13 @@
 // #define LCD_SCREEN_ROT_270
 // SPLASH SCREEN duration in millisecond
-#define SPLASH_SCREEN_DURATION 2000 // Millisecond
+#define SPLASH_SCREEN_DURATION 5000 // Millisecond
 /** Display Voltage Logic Selector on Alligator Board
 0 = Voltage level 3.3V
 1 = Voltage level 5V
 */
-#define UI_VOLTAGE_LEVEL 0 // Set 5 o 3.3 V
+#define UI_VOLTAGE_LEVEL 1 // Set 5 o 3.3 V
 //============================== Languages UI =========================
@@ -441,12 +446,20 @@
 // M502 - reverts to the default "factory settings".  You still need to store them in EEPROM afterwards if you want to.
 //define this to enable EEPROM support
 //#define EEPROM_SETTINGS
-//#define EEPROM_CHITCHAT
+#define EEPROM_CHITCHAT
 // to disable EEPROM Serial responses and decrease program space by ~1700 byte: comment this out:
 // please keep turned on if you can.
 //#define DISABLE_M503
 //===========================================================================
+//========================== EXTRA SETTINGS ON SD ===========================
+// Uncomment SD SETTINGS to enable the firmware to write some configuration, that require frequent update, on the SD card.
+//#define SD_SETTINGS
+#define SD_CFG_SECONDS        300         //seconds between update
+#define CFG_SD_FILE           "INFO.CFG"  //name of the configuration file
+#define CFG_SD_MAX_KEY_LEN    3+1         //icrease this if you add key name longer than the actual value.
+#define CFG_SD_MAX_VALUE_LEN  12+1        //this should be enought for int, long and float if you need to retrive strings increase this carefully
+//===========================================================================
 //==================== Bowden Filament management ===========================
 //#define EASY_LOAD

--- a/MarlinKimbra/Configuration_Cartesian.h
+++ b/MarlinKimbra/Configuration_Cartesian.h
@@ -25,10 +25,12 @@
 #define X_MIN_ENDSTOP_LOGIC   false   // set to true to invert the logic of the endstop.
 #define Y_MIN_ENDSTOP_LOGIC   false   // set to true to invert the logic of the endstop.
 #define Z_MIN_ENDSTOP_LOGIC   false   // set to true to invert the logic of the endstop.
+#define Z2_MIN_ENDSTOP_LOGIC  false   // set to true to invert the logic of the endstop.
 #define E_MIN_ENDSTOP_LOGIC   false   // set to true to invert the logic of the endstop.
 #define X_MAX_ENDSTOP_LOGIC   false   // set to true to invert the logic of the endstop.
 #define Y_MAX_ENDSTOP_LOGIC   false   // set to true to invert the logic of the endstop.
 #define Z_MAX_ENDSTOP_LOGIC   false   // set to true to invert the logic of the endstop.
+#define Z2_MAX_ENDSTOP_LOGIC  false   // set to true to invert the logic of the endstop.
 #define Z_PROBE_ENDSTOP_LOGIC false   // set to true to invert the logic of the endstop.
 // ENDSTOP SETTINGS:
@@ -74,10 +76,10 @@
 #define E_MIN_POS 0
 //=====================================================================================
-//============================= Bed Manual or Auto Leveling ===========================
+//================ Manual Bed Leveling (MBL) or Auto Bed Leveling =====================
 //=====================================================================================
-// set the rectangle in which to probe in manual or automatic
+// set the rectangle in which to probe in MBL or ABL
 #define LEFT_PROBE_BED_POSITION 20
 #define RIGHT_PROBE_BED_POSITION 180
 #define FRONT_PROBE_BED_POSITION 20
@@ -186,9 +188,7 @@
 // default settings
 #define DEFAULT_AXIS_STEPS_PER_UNIT     {80,80,3200,625,625,625,625}       // X, Y, Z, E0, E1, E2, E3 default steps per unit
 #define DEFAULT_MAX_FEEDRATE            {300,300,2,100,100,100,100}        // X, Y, Z, E0, E1, E2, E3 (mm/sec)
-#define DEFAULT_RETRACTION_MAX_FEEDRATE {110,110,110,110}                  // E0, E1, E2, E3 (mm/sec)
 #define DEFAULT_MAX_ACCELERATION        {3000,3000,50,1000,1000,1000,1000} // X, Y, Z, E0, E1, E2, E3 maximum start speed for accelerated moves.
 #define DEFAULT_ACCELERATION          2500      // X, Y, Z and E max acceleration in mm/s^2 for printing moves
 #define DEFAULT_RETRACT_ACCELERATION 10000      // E max acceleration in mm/s^2 for retracts
 #define DEFAULT_TRAVEL_ACCELERATION   3000      // X, Y, Z acceleration in mm/s^2 for travel (non printing) moves

--- a/MarlinKimbra/Configuration_Corexy.h
+++ b/MarlinKimbra/Configuration_Corexy.h
@@ -25,10 +25,12 @@
 #define X_MIN_ENDSTOP_LOGIC   false   // set to true to invert the logic of the endstop.
 #define Y_MIN_ENDSTOP_LOGIC   false   // set to true to invert the logic of the endstop.
 #define Z_MIN_ENDSTOP_LOGIC   false   // set to true to invert the logic of the endstop.
+#define Z2_MIN_ENDSTOP_LOGIC  false   // set to true to invert the logic of the endstop.
 #define E_MIN_ENDSTOP_LOGIC   false   // set to true to invert the logic of the endstop.
 #define X_MAX_ENDSTOP_LOGIC   false   // set to true to invert the logic of the endstop.
 #define Y_MAX_ENDSTOP_LOGIC   false   // set to true to invert the logic of the endstop.
 #define Z_MAX_ENDSTOP_LOGIC   false   // set to true to invert the logic of the endstop.
+#define Z2_MAX_ENDSTOP_LOGIC  false   // set to true to invert the logic of the endstop.
 #define Z_PROBE_ENDSTOP_LOGIC false   // set to true to invert the logic of the endstop.
 // ENDSTOP SETTINGS:
@@ -74,10 +76,10 @@
 #define E_MIN_POS 0
 //=====================================================================================
-//============================= Bed Manual or Auto Leveling ===========================
+//================ Manual Bed Leveling (MBL) or Auto Bed Leveling =====================
 //=====================================================================================
-// set the rectangle in which to probe in manual or automatic
+// set the rectangle in which to probe in MBL or ABL
 #define LEFT_PROBE_BED_POSITION 20
 #define RIGHT_PROBE_BED_POSITION 180
 #define FRONT_PROBE_BED_POSITION 20
@@ -88,7 +90,7 @@
 //If you have enabled the Auto Bed Levelling and are using the same Z Probe for Z Homing,
 //it is highly recommended you let this Z_SAFE_HOMING enabled!!!
 //#define Z_SAFE_HOMING
-#ifdef Z_SAFE_HOMING
+#ifndef Z_SAFE_HOMING
  #define Z_SAFE_HOMING_X_POINT (X_MAX_LENGTH/2)    // X point for Z homing when homing all axis (G28) or homing Z
  #define Z_SAFE_HOMING_Y_POINT (Y_MAX_LENGTH/2)    // Y point for Z homing when homing all axis (G28) or homing Z
 #endif
@@ -186,9 +188,7 @@
 // default settings
 #define DEFAULT_AXIS_STEPS_PER_UNIT     {80,80,3200,625,625,625,625}       // X, Y, Z, E0, E1, E2, E3 default steps per unit
 #define DEFAULT_MAX_FEEDRATE            {300,300,2,100,100,100,100}        // X, Y, Z, E0, E1, E2, E3 (mm/sec)
-#define DEFAULT_RETRACTION_MAX_FEEDRATE {110,110,110,110}                  // E0, E1, E2, E3 (mm/sec)
 #define DEFAULT_MAX_ACCELERATION        {3000,3000,50,1000,1000,1000,1000} // X, Y, Z, E0, E1, E2, E3 maximum start speed for accelerated moves.
 #define DEFAULT_ACCELERATION          2500      // X, Y, Z and E max acceleration in mm/s^2 for printing moves
 #define DEFAULT_RETRACT_ACCELERATION 10000      // E max acceleration in mm/s^2 for retracts
 #define DEFAULT_TRAVEL_ACCELERATION   3000      // X, Y, Z acceleration in mm/s^2 for travel (non printing) moves

--- a/MarlinKimbra/Configuration_Delta.h
+++ b/MarlinKimbra/Configuration_Delta.h
@@ -9,7 +9,9 @@
 // Make delta curves from many straight lines (linear interpolation).
 // This is a trade-off between visible corners (not enough segments)
 // and processor overload (too many expensive sqrt calls).
-#define DELTA_SEGMENTS_PER_SECOND 200
+// The new function do not use segments per second but segments per mm
+// if you want use new function comment this (using // at the start of the line)
+#define DELTA_SEGMENTS_PER_SECOND 150
 // Center-to-center distance of the holes in the diagonal push rods.
 #define DEFAULT_DELTA_DIAGONAL_ROD 217.0      // mm
@@ -29,6 +31,9 @@
 //Uncomment to enable autocalibration debug messages
 #define DEBUG_MESSAGES
+//Amount to lift head after probing a point
+#define AUTOCAL_PROBELIFT 3 // mm
 // Precision for G30 delta autocalibration function
 #define AUTOCALIBRATION_PRECISION 0.1      // mm
@@ -37,14 +42,14 @@
 // Z-Probe variables
 // Start and end location values are used to deploy/retract the probe (will move from start to end and back again)
-#define PROBING_FEEDRATE 500                          // Speed for individual probe Use: G30 A F600
+#define PROBING_FEEDRATE 1000                         // Speed in mm/min for individual probe Use: G30 A F600
 #define Z_PROBE_OFFSET {0, 0, -1, 0}                  // X, Y, Z, E distance between hotend nozzle and deployed bed leveling probe.
 #define Z_PROBE_DEPLOY_START_LOCATION {0, 0, 30, 0}   // X, Y, Z, E start location for z-probe deployment sequence
 #define Z_PROBE_DEPLOY_END_LOCATION {0, 0, 30, 0}     // X, Y, Z, E end location for z-probe deployment sequence
 #define Z_PROBE_RETRACT_START_LOCATION {0, 0, 30, 0}  // X, Y, Z, E start location for z-probe retract sequence
 #define Z_PROBE_RETRACT_END_LOCATION {0, 0, 30, 0}    // X, Y, Z, E end location for z-probe retract sequence
 #define Z_RAISE_BETWEEN_PROBINGS 2                    // How much the extruder will be raised when travelling from between next probing points
-#define AUTOLEVEL_GRID 24                             // Distance between autolevel Z probing points, should be less than print surface radius/3.
+#define AUTOLEVEL_GRID 20                             // Distance between autolevel Z probing points, should be less than print surface radius/3.
 //===========================================================================
 //=============================Mechanical Settings===========================
@@ -80,9 +85,9 @@
 // ENDSTOP SETTINGS:
 // Sets direction of endstop when homing; 1=MAX, -1=MIN
-#define X_HOME_DIR 1 //DELTA MUST HAVE MAX ENDSTOP
+#define X_HOME_DIR 1 // DELTA MUST HAVE MAX ENDSTOP
-#define Y_HOME_DIR 1 //DELTA MUST HAVE MAX ENDSTOP
+#define Y_HOME_DIR 1 // DELTA MUST HAVE MAX ENDSTOP
-#define Z_HOME_DIR 1 //DELTA MUST HAVE MAX ENDSTOP
+#define Z_HOME_DIR 1 // DELTA MUST HAVE MAX ENDSTOP
 #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.
@@ -136,9 +141,7 @@
 // delta speeds must be the same on xyz
 #define DEFAULT_AXIS_STEPS_PER_UNIT {80,80,80,451,625,625,625}        // X, Y, Z, E0, E1, E2, E3
 #define DEFAULT_MAX_FEEDRATE {300,300,300,45,100,100,100}             // X, Y, Z, E0, E1, E2, E3 (mm/sec)
-#define DEFAULT_RETRACTION_MAX_FEEDRATE {150,150,150,150}             // E0, E1, E2, E3 (mm/sec)
 #define DEFAULT_MAX_ACCELERATION {2000,2000,2000,1000,1000,1000,1000} // X, Y, Z, E0, E1, E2, E3 maximum start speed for accelerated moves.
 #define DEFAULT_ACCELERATION          1000      // X, Y, Z and E max acceleration in mm/s^2 for printing moves
 #define DEFAULT_RETRACT_ACCELERATION  2500      // X, Y, Z and E max acceleration in mm/s^2 for retracts
 #define DEFAULT_TRAVEL_ACCELERATION   3000      // X, Y, Z acceleration in mm/s^2 for travel (non printing) moves

--- a/MarlinKimbra/Configuration_Scara.h
+++ b/MarlinKimbra/Configuration_Scara.h
@@ -7,7 +7,7 @@
 // QHARLEYS Autobedlevelling has not been ported, because Marlin has now Bed-levelling
 // You might need Z-Min endstop on SCARA-Printer to use this feature. Actually untested!
 // Uncomment to use Morgan scara mode
-#define scara_segments_per_second 200 //careful, two much will decrease performance...
+#define SCARA_SEGMENTS_PER_SECOND 200 // If movement is choppy try lowering this value
 // Length of inner support arm
 #define Linkage_1 150 //mm      Preprocessor cannot handle decimal point...
 // Length of outer support arm     Measure arm lengths precisely and enter 
@@ -210,9 +210,7 @@
 // default settings
 #define DEFAULT_AXIS_STEPS_PER_UNIT     {103.69,103.69,200/1.25,1000,1000,1000,1000}    // X, Y, Z, E0, E1, E2, E3
 #define DEFAULT_MAX_FEEDRATE            {300,300,4,45,45,45,45}                         // X, Y, Z, E0, E1, E2, E3 (mm/sec)
-#define DEFAULT_RETRACTION_MAX_FEEDRATE {80,80,80,80}                                   // E0, E1, E2, E3 (mm/sec)
 #define DEFAULT_MAX_ACCELERATION        {5000,5000,50,5000,5000,5000,5000}              // X, Y, Z, E0, E1, E2, E3 maximum start speed for accelerated moves.
 #define DEFAULT_ACCELERATION           400      // X, Y, Z and E max acceleration in mm/s^2 for printing moves
 #define DEFAULT_RETRACT_ACCELERATION  2000      // X, Y, Z and E max acceleration in mm/s^2 for retracts
 #define DEFAULT_TRAVEL_ACCELERATION    400      // X, Y, Z acceleration in mm/s^2 for travel (non printing) moves

--- a/MarlinKimbra/Configuration_adv.h
+++ b/MarlinKimbra/Configuration_adv.h
@@ -23,24 +23,38 @@
 #define BED_CHECK_INTERVAL 5000 //ms between checks in bang-bang control
 /**
- * Heating Sanity Check
+ * Thermal Protection parameters
- *
+ */
- * Whenever an M104 or M109 increases the target temperature this will wait for WATCH_TEMP_PERIOD milliseconds,
+#ifdef THERMAL_PROTECTION_HOTENDS
- * and if the temperature hasn't increased by WATCH_TEMP_INCREASE degrees, the machine is halted, requiring a
+  #define THERMAL_PROTECTION_PERIOD    40     // Seconds
- * hard reset. This test restarts with any M104/M109, but only if the current temperature is below the target
+  #define THERMAL_PROTECTION_HYSTERESIS 4     // Degrees Celsius
- * by at least 2 * WATCH_TEMP_INCREASE degrees celsius.
+  /**
+   * Whenever an M104 or M109 increases the target temperature the firmware will wait for the
+   * WATCH_TEMP_PERIOD to expire, and if the temperature hasn't increased by WATCH_TEMP_INCREASE
+   * degrees, the machine is halted, requiring a hard reset. This test restarts with any M104/M109,
+   * but only if the current temperature is far enough below the target for a reliable test.
   */
-//#define WATCH_TEMP_PERIOD 16000 // 16 seconds
+  #define WATCH_TEMP_PERIOD  16               // Seconds
-//#define WATCH_TEMP_INCREASE 4  // Heat up at least 4 degrees in 16 seconds
+  #define WATCH_TEMP_INCREASE 4               // Degrees Celsius
+#endif
+#ifdef THERMAL_PROTECTION_BED
+  #define THERMAL_PROTECTION_BED_PERIOD    20 // Seconds
+  #define THERMAL_PROTECTION_BED_HYSTERESIS 2 // Degrees Celsius
+#endif
-//automatic temperature: The hot end target temperature is calculated by all the buffered lines of gcode.
-//The maximum buffered steps/sec of the extruder motor are called "se".
+/**
-//You enter the autotemp mode by a M109 S<mintemp> B<maxtemp> F<factor>
+ * Automatic Temperature:
-// the target temperature is set to mintemp+factor*se[steps/sec] and limited by mintemp and maxtemp
+ * The hotend target temperature is calculated by all the buffered lines of gcode.
-// you exit the value by any M109 without F*
+ * The maximum buffered steps/sec of the extruder motor is called "se".
-// Also, if the temperature is set to a value <mintemp, it is not changed by autotemp.
+ * Start autotemp mode with M109 S<mintemp> B<maxtemp> F<factor>
-// on an Ultimaker, some initial testing worked with M109 S215 B260 F1 in the start.gcode
+ * The target temperature is set to mintemp+factor*se[steps/sec] and is limited by
+ * mintemp and maxtemp. Turn this off by excuting M109 without F*
+ * Also, if the temperature is set to a value below mintemp, it will not be changed by autotemp.
+ * On an Ultimaker, some initial testing worked with M109 S215 B260 F1 in the start.gcode
+ */
 #define AUTOTEMP
 #ifdef AUTOTEMP
  #define AUTOTEMP_OLDWEIGHT 0.98
@@ -50,11 +64,13 @@
 //The M105 command return, besides traditional information, the ADC value read from temperature sensors.
 //#define SHOW_TEMP_ADC_VALUES
-//extruder idle oozing prevention
+/**
-//if the extruder motor is idle for more than SECONDS, and the temperature over MINTEMP,
+ * extruder idle oozing prevention
-//some filament is retracted. The filament retracted is re-added before the next extrusion
+ * if the extruder motor is idle for more than SECONDS, and the temperature over MINTEMP,
-//or when the target temperature is less than EXTRUDE_MINTEMP and the actual temperature
+ * some filament is retracted. The filament retracted is re-added before the next extrusion
-//is greater than IDLE_OOZING_MINTEMP and less than IDLE_OOZING_FEEDRATE
+ * or when the target temperature is less than EXTRUDE_MINTEMP and the actual temperature
+ * is greater than IDLE_OOZING_MINTEMP and less than IDLE_OOZING_FEEDRATE
+ */
 //#define IDLE_OOZING_PREVENT
 #define IDLE_OOZING_MINTEMP           EXTRUDE_MINTEMP + 5
 #define IDLE_OOZING_MAXTEMP           IDLE_OOZING_MINTEMP + 5
@@ -214,6 +230,9 @@
 // Default stepper release if idle. Set to 0 to deactivate.
 #define DEFAULT_STEPPER_DEACTIVE_TIME 60
+// Default step delay for any driver
+//#define STEPPER_HIGH_LOW_DELAY 1  // Delay in microseconds
 #define DEFAULT_MINIMUMFEEDRATE       0.0     // minimum feedrate
 #define DEFAULT_MINTRAVELFEEDRATE     0.0
@@ -284,7 +303,7 @@
  //#define MENU_ADDAUTOSTART
  // Show a progress bar on HD44780 LCDs for SD printing
-  #define LCD_PROGRESS_BAR
+  //#define LCD_PROGRESS_BAR
  #ifdef LCD_PROGRESS_BAR
    // Amount of time (ms) to show the bar
@@ -297,8 +316,22 @@
    //#define PROGRESS_MSG_ONCE
  #endif
+  // This allows hosts to request long names for files and folders with M33
+  //#define LONG_FILENAME_HOST_SUPPORT
 #endif // SDSUPPORT
+// for dogm lcd displays you can choose some additional fonts:
+#ifdef DOGLCD
+  // save 3120 bytes of PROGMEM by commenting out #define USE_BIG_EDIT_FONT
+  // we don't have a big font for Cyrillic, Kana
+  //#define USE_BIG_EDIT_FONT
+  // If you have spare 2300Byte of progmem and want to use a 
+  // smaller font on the Info-screen uncomment the next line.
+  //#define USE_SMALL_INFOFONT
+#endif // DOGLCD
 // The hardware watchdog should reset the microcontroller disabling all outputs, in case the firmware gets stuck and doesn't do temperature regulation.
 //#define USE_WATCHDOG
@@ -341,7 +374,7 @@
 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
-const unsigned int dropsegments=5; //everything with less than this number of steps will be ignored as move and joined with the next movement
+const unsigned int dropsegments = 5; // everything with less than this number of steps will be ignored as move and joined with the next movement
 // Control heater 0 and heater 1 in parallel.
 //#define HEATERS_PARALLEL
@@ -364,9 +397,10 @@ const unsigned int dropsegments=5; //everything with less than this number of st
 #define BUFSIZE 4
 // Bad Serial-connections can miss a received command by sending an 'ok'
-// Therefore some clients go after 30 seconds in a timeout. Some other clients start sending commands while receiving a 'wait'.
+// Therefore some clients abort after 30 seconds in a timeout.
-// This wait is only send when the buffer is empty. The timeout-length is in milliseconds. 1000 is a good value.
+// Some other clients start sending commands while receiving a 'wait'.
-#define NO_TIMEOUTS 1000
+// This "wait" is only sent when the buffer is empty. 1 second is a good value here.
+#define NO_TIMEOUTS 1000 // Milliseconds
 // Some clients will have this feature soon. This could make the NO_TIMEOUTS unnecessary.
 #define ADVANCED_OK
@@ -389,7 +423,7 @@ const unsigned int dropsegments=5; //everything with less than this number of st
  #define RETRACT_RECOVER_FEEDRATE    8   //default feedrate for recovering from retraction (mm/s)
 #endif
-// Add support for experimental filament exchange support M600; requires display
+// Add support for filament exchange support M600; requires display
 #ifdef ULTIPANEL
  #define FILAMENTCHANGEENABLE
  #ifdef FILAMENTCHANGEENABLE

--- a/MarlinKimbra/Makefile
+++ b/MarlinKimbra/Makefile
@@ -266,8 +266,8 @@ VPATH += $(ARDUINO_INSTALL_DIR)/hardware/teensy/cores/teensy
 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		\
+	SdFile.cpp SdVolume.cpp planner.cpp stepper.cpp \
-	stepper.cpp temperature.cpp cardreader.cpp configuration_store.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)

--- a/MarlinKimbra/Marlin.h
+++ b/MarlinKimbra/Marlin.h
@@ -23,6 +23,11 @@
 #include <avr/pgmspace.h>
 #include <avr/interrupt.h>
 #include "Configuration.h"
+#include "pins.h"
+#ifndef SANITYCHECK_H
+  #error Your Configuration.h and Configuration_adv.h files are outdated!
+#endif
 #if (ARDUINO >= 100)
  #include "Arduino.h"
@@ -30,10 +35,19 @@
  #include "WProgram.h"
 #endif
+// Macros for bit masks
 #define BIT(b) (1<<(b))
 #define TEST(n,b) (((n)&BIT(b))!=0)
+#define SET_BIT(n,b,value) (n) ^= ((-value)^(n)) & (BIT(b))
+// Macros for maths shortcuts
+#define M_PI 3.1415926536
 #define RADIANS(d) ((d)*M_PI/180.0)
-#define DEGREES(r) ((d)*180.0/M_PI)
+#define DEGREES(r) ((r)*180.0/M_PI)
+#define SIN_60 0.8660254037844386
+#define COS_60 0.5
+// Macros to contrain values
 #define NOLESS(v,n) do{ if (v < n) v = n; }while(0)
 #define NOMORE(v,n) do{ if (v > n) v = n; }while(0)
@@ -47,7 +61,8 @@ typedef unsigned long millis_t;
 #include "comunication.h"
 void get_command();
-void process_commands();
+void idle(bool ignore_stepper_queue = false);
 void manage_inactivity(bool ignore_stepper_queue=false);
@@ -135,37 +150,38 @@ void manage_inactivity(bool ignore_stepper_queue=false);
 */
 enum AxisEnum {X_AXIS=0, Y_AXIS=1, A_AXIS=0, B_AXIS=1, Z_AXIS=2, E_AXIS=3, X_HEAD=4, Y_HEAD=5};
+enum EndstopEnum {X_MIN=0, Y_MIN=1, Z_MIN=2, Z_PROBE=3, X_MAX=4, Y_MAX=5, Z_MAX=6, Z2_MIN=7, Z2_MAX=8};
 void enable_all_steppers();
 void disable_all_steppers();
 void FlushSerialRequestResend();
-void ClearToSend();
+void ok_to_send();
-void get_coordinates();
 #ifdef DELTA
-float probe_bed(float x, float y);
+  float probe_bed(float x, float y);
-void set_delta_constants();
+  void set_delta_constants();
-void home_delta_axis();
+  void home_delta_axis();
-void calibration_report();
+  void calibration_report();
-void bed_probe_all();
+  void bed_probe_all();
-void set_default_z_probe_offset();
+  void set_default_z_probe_offset();
-void set_delta_constants();
+  void set_delta_constants();
-void save_carriage_positions(int position_num);
+  void save_carriage_positions(int position_num);
-void calculate_delta(float cartesian[3]);
+  void calculate_delta(float cartesian[3]);
-void adjust_delta(float cartesian[3]);
+  void adjust_delta(float cartesian[3]);
-void prepare_move_raw();
+  void prepare_move_raw();
-extern float delta[3];
+  extern float delta[3];
-extern float delta_tmp[3];
+  extern float delta_tmp[3];
-extern float delta_tower1_x,delta_tower1_y;
+  extern float delta_tower1_x, delta_tower1_y;
-extern float delta_tower2_x,delta_tower2_y;
+  extern float delta_tower2_x, delta_tower2_y;
-extern float delta_tower3_x,delta_tower3_y;
+  extern float delta_tower3_x, delta_tower3_y;
 #endif
 #ifdef SCARA
  void calculate_delta(float cartesian[3]);
  void calculate_SCARA_forward_Transform(float f_scara[3]);
 #endif
 void prepare_move();
-void kill();
+void kill(const char *);
 void Stop();
 #ifdef FILAMENT_RUNOUT_SENSOR
@@ -237,6 +253,7 @@ extern float home_offset[3];
  extern float tower_adj[6];
  extern float delta_radius;
  extern float delta_diagonal_rod;
+  extern float delta_segments_per_second;
 #elif defined(Z_DUAL_ENDSTOPS)
  extern float z_endstop_adj;
 #endif
@@ -253,7 +270,6 @@ extern float zprobe_zoffset;
 // Lifetime stats
 extern unsigned long printer_usage_seconds;  //this can old about 136 year before go overflow. If you belive that you can live more than this please contact me.
-extern millis_t config_last_update;
 #ifdef PREVENT_DANGEROUS_EXTRUDE
  extern float extrude_min_temp;
@@ -306,6 +322,11 @@ extern int fanSpeed;
  extern int laser_ttl_modulation;
 #endif
+#if defined(SDSUPPORT) && defined(SD_SETTINGS)
+  extern millis_t config_last_update;
+  extern bool config_readed;
+#endif
 extern millis_t print_job_start_ms;
 extern millis_t print_job_stop_ms;

--- a/MarlinKimbra/Marlin_main.cpp
+++ b/MarlinKimbra/Marlin_main.cpp
--- a/MarlinKimbra/SdBaseFile.cpp
+++ b/MarlinKimbra/SdBaseFile.cpp
@@ -1097,8 +1097,9 @@ int16_t SdBaseFile::read(void* buf, uint16_t nbyte) {
 fail:
  return -1;
 }
-//------------------------------------------------------------------------------
-/** Read the next directory entry from a directory file.
+/**
+ * Read the next entry in a directory.
 *
 * \param[out] dir The dir_t struct that will receive the data.
 *
@@ -1114,50 +1115,38 @@ int8_t SdBaseFile::readDir(dir_t* dir, char* longFilename) {
  if (!isDir() || (0X1F & curPosition_)) return -1;
  //If we have a longFilename buffer, mark it as invalid. If we find a long filename it will be filled automaticly.
-  if (longFilename != NULL)
+  if (longFilename != NULL) longFilename[0] = '\0';
-  {
-  	longFilename[0] = '\0';
-  }
  while (1) {
    n = read(dir, sizeof(dir_t));
    if (n != sizeof(dir_t)) return n == 0 ? 0 : -1;
    // last entry if DIR_NAME_FREE
    if (dir->name[0] == DIR_NAME_FREE) return 0;
    // skip empty entries and entry for .  and ..
    if (dir->name[0] == DIR_NAME_DELETED || dir->name[0] == '.') continue;
-    //Fill the long filename if we have a long filename entry,
-	// long filename entries are stored before the actual filename.
+    // Fill the long filename if we have a long filename entry.
-	if (DIR_IS_LONG_NAME(dir) && longFilename != NULL)
+    // Long filename entries are stored before the short filename.
-    {
+    if (longFilename != NULL && DIR_IS_LONG_NAME(dir)) {
      vfat_t *VFAT = (vfat_t*)dir;
-		//Sanity check the VFAT entry. The first cluster is always set to zero. And th esequence number should be higher then 0
+      // Sanity-check the VFAT entry. The first cluster is always set to zero. And the sequence number should be higher than 0
-    	if (VFAT->firstClusterLow == 0 && (VFAT->sequenceNumber & 0x1F) > 0 && (VFAT->sequenceNumber & 0x1F) <= MAX_VFAT_ENTRIES)
+      if (VFAT->firstClusterLow == 0 && (VFAT->sequenceNumber & 0x1F) > 0 && (VFAT->sequenceNumber & 0x1F) <= MAX_VFAT_ENTRIES) {
-    	{
+        // TODO: Store the filename checksum to verify if a none-long filename aware system modified the file table.
-			//TODO: Store the filename checksum to verify if a none-long filename aware system modified the file table.
        n = ((VFAT->sequenceNumber & 0x1F) - 1) * FILENAME_LENGTH;
-			longFilename[n+0] = VFAT->name1[0];
+        for (uint8_t i=0; i<FILENAME_LENGTH; i++)
-			longFilename[n+1] = VFAT->name1[1];
+          longFilename[n+i] = (i < 5) ? VFAT->name1[i] : (i < 11) ? VFAT->name2[i-5] : VFAT->name3[i-11];
-			longFilename[n+2] = VFAT->name1[2];
+        // If this VFAT entry is the last one, add a NUL terminator at the end of the string
-			longFilename[n+3] = VFAT->name1[3];
+        if (VFAT->sequenceNumber & 0x40) longFilename[n+FILENAME_LENGTH] = '\0';
-			longFilename[n+4] = VFAT->name1[4];
+      }
-			longFilename[n+5] = VFAT->name2[0];
+    }
-			longFilename[n+6] = VFAT->name2[1];
+    // Return if normal file or subdirectory
-			longFilename[n+7] = VFAT->name2[2];
-			longFilename[n+8] = VFAT->name2[3];
-			longFilename[n+9] = VFAT->name2[4];
-			longFilename[n+10] = VFAT->name2[5];
-			longFilename[n+11] = VFAT->name3[0];
-			longFilename[n+12] = VFAT->name3[1];
-			//If this VFAT entry is the last one, add a NUL terminator at the end of the string
-			if (VFAT->sequenceNumber & 0x40)
-				longFilename[n+FILENAME_LENGTH] = '\0';
-		}
-    }
-    // return if normal file or subdirectory
    if (DIR_IS_FILE_OR_SUBDIR(dir)) return n;
  }
 }
 //------------------------------------------------------------------------------
 // Read next directory entry into the cache
 // Assumes file is correctly positioned

--- a/MarlinKimbra/cardreader.cpp
+++ b/MarlinKimbra/cardreader.cpp
@@ -20,6 +20,7 @@ CardReader::CardReader() {
  autostart_stilltocheck = true; //the SD start is delayed, because otherwise the serial cannot answer fast enough to make contact with the host software.
  autostart_index = 0;
  //power to SD reader
  #if SDPOWER > -1
    OUT_WRITE(SDPOWER, HIGH);
@@ -39,24 +40,43 @@ char *createFilename(char *buffer, const dir_t &p) { //buffer > 12characters
  return buffer;
 }
+/**
+ * Dive into a folder and recurse depth-first to perform a pre-set operation lsAction:
+ *   LS_Count       - Add +1 to nrFiles for every file within the parent
+ *   LS_GetFilename - Get the filename of the file indexed by nrFiles
+ *   LS_SerialPrint - Print the full path of each file to serial output
+ */
 void CardReader::lsDive(const char *prepend, SdFile parent, const char * const match/*=NULL*/) {
  dir_t p;
  uint8_t cnt = 0;
+  // Read the next entry from a directory
  while (parent.readDir(p, longFilename) > 0) {
-    if (DIR_IS_SUBDIR(&p) && lsAction != LS_Count && lsAction != LS_GetFilename) { // hence LS_SerialPrint
-      char path[FILENAME_LENGTH*2];
+    // If the entry is a directory and the action is LS_SerialPrint
+    if (DIR_IS_SUBDIR(&p) && lsAction != LS_Count && lsAction != LS_GetFilename) {
+      // Allocate enough stack space for the full path to a folder
+      int len = strlen(prepend) + FILENAME_LENGTH + 1;
+      char path[len];
+      // Get the short name for the item, which we know is a folder
      char lfilename[FILENAME_LENGTH];
      createFilename(lfilename, p);
-      path[0] = 0;
+      // Append the FOLDERNAME12/ to the passed string.
-      if (prepend[0] == 0) strcat(path, "/"); //avoid leading / if already in prepend
+      // It contains the full path to the "parent" argument.
+      // We now have the full path to the item in this folder.
+      path[0] = '\0';
+      if (prepend[0] == '\0') strcat(path, "/"); // a root slash if prepend is empty
      strcat(path, prepend);
      strcat(path, lfilename);
      strcat(path, "/");
-      //Serial.print(path);
+      // Serial.print(path);
+      // Get a new directory object using the full path
+      // and dive recursively into it.
      SdFile dir;
      if (!dir.open(parent, lfilename, O_READ)) {
        if (lsAction == LS_SerialPrint) {
@@ -64,14 +84,13 @@ void CardReader::lsDive(const char *prepend, SdFile parent, const char * const m
        }
      }
      lsDive(path, dir);
-      //close done automatically by destructor of SdFile
+      // close() is done automatically by destructor of SdFile
    }
    else {
      char pn0 = p.name[0];
      if (pn0 == DIR_NAME_FREE) break;
      if (pn0 == DIR_NAME_DELETED || pn0 == '.') continue;
-      char lf0 = longFilename[0];
+      if (longFilename[0] == '.') continue;
-      if (lf0 == '.') continue;
      if (!DIR_IS_FILE_OR_SUBDIR(&p)) continue;
@@ -79,24 +98,26 @@ void CardReader::lsDive(const char *prepend, SdFile parent, const char * const m
      if (!filenameIsDir && (p.name[8] != 'G' || p.name[9] == '~')) continue;
-      //if (cnt++ != nr) continue;
+      switch (lsAction) {
+        case LS_Count:
+          nrFiles++;
+          break;
+        case LS_SerialPrint:
          createFilename(filename, p);
-      if (lsAction == LS_SerialPrint) {
          ECHO_V(prepend);
          ECHO_EV(filename);
-      }
+          break;
-      else if (lsAction == LS_Count) {
+        case LS_GetFilename:
-        nrFiles++;
+          createFilename(filename, p);
-      }
-      else if (lsAction == LS_GetFilename) {
          if (match != NULL) {
            if (strcasecmp(match, filename) == 0) return;
          }
          else if (cnt == nrFiles) return;
          cnt++;
+          break;
      }
    }
-  }
+  } // while readDir
 }
 void CardReader::ls()  {
@@ -105,6 +126,67 @@ void CardReader::ls()  {
  lsDive("", root);
 }
+#ifdef LONG_FILENAME_HOST_SUPPORT
+  /**
+   * Get a long pretty path based on a DOS 8.3 path
+   */
+  void CardReader::printLongPath(char *path) {
+    lsAction = LS_GetFilename;
+    int i, pathLen = strlen(path);
+    // ECHO_M("Full Path: "); ECHO_EV(path);
+    // Zero out slashes to make segments
+    for (i = 0; i < pathLen; i++) if (path[i] == '/') path[i] = '\0';
+    SdFile diveDir = root; // start from the root for segment 1
+    for (i = 0; i < pathLen;) {
+      if (path[i] == '\0') i++; // move past a single nul
+      char *segment = &path[i]; // The segment after most slashes
+      // If a segment is empty (extra-slash) then exit
+      if (!*segment) break;
+      // Go to the next segment
+      while (path[++i]) { }
+      // ECHO_M("Looking for segment: "); ECHO_EV(segment);
+      // Find the item, setting the long filename
+      diveDir.rewind();
+      lsDive("", diveDir, segment);
+      // Print /LongNamePart to serial output
+      ECHO_C('/');
+      ECHO_V(longFilename[0] ? longFilename : "???");
+      // If the filename was printed then that's it
+      if (!filenameIsDir) break;
+      // ECHO_M("Opening dir: "); ECHO_EV(segment);
+      // Open the sub-item as the new dive parent
+      SdFile dir;
+      if (!dir.open(diveDir, segment, O_READ)) {
+        ECHO_E;
+        ECHO_SMV(DB, MSG_SD_CANT_OPEN_SUBDIR, segment);
+        break;
+      }
+      diveDir.close();
+      diveDir = dir;
+    } // while i<pathLen
+    ECHO_E;
+  }
+#endif // LONG_FILENAME_HOST_SUPPORT
 void CardReader::initsd() {
  cardOK = false;
  if (root.isOpen()) root.close();
@@ -120,7 +202,7 @@ void CardReader::initsd() {
      && !card.init(SPI_SPEED, LCD_SDSS)
    #endif
  ) {
-    ECHO_LM(ER, MSG_SD_INIT_FAIL);
+    ECHO_LM(DB, MSG_SD_INIT_FAIL);
  }
  else if (!volume.init(&card)) {
    ECHO_LM(ER, MSG_SD_VOL_INIT_FAIL);
@@ -130,14 +212,15 @@ void CardReader::initsd() {
  }
  else {
    cardOK = true;
-    ECHO_LM(OK, MSG_SD_CARD_OK);
+    ECHO_LM(DB, MSG_SD_CARD_OK);
  }
  workDir = root;
  curDir = &root;
+  /*
-  /*if (!workDir.openRoot(&volume)) {
+  if (!workDir.openRoot(&volume)) {
    ECHO_EM(MSG_SD_WORKDIR_FAIL);
-  }*/
+  }
+  */
 }
 void CardReader::setroot() {
@@ -187,19 +270,29 @@ void CardReader::openFile(char* name, bool read, bool replace_current/*=true*/,
    if (!replace_current) {
      if (file_subcall_ctr > SD_PROCEDURE_DEPTH - 1) {
        ECHO_LMV(ER, MSG_SD_MAX_DEPTH, SD_PROCEDURE_DEPTH);
-        kill();
+        kill(PSTR(MSG_KILLED));
        return;
      }
+      ECHO_SMV(DB, "SUBROUTINE CALL target:\"", name);
+      ECHO_M("\" parent:\"");
      //store current filename and position
      getAbsFilename(filenames[file_subcall_ctr]);
+      ECHO_V(filenames[file_subcall_ctr]);
+      ECHO_EMV("\" pos", sdpos);
      filespos[file_subcall_ctr] = sdpos;
      file_subcall_ctr++;
    }
+    else {
+     ECHO_LMV(DB, "Now doing file: ", name);
+    }
    file.close();
  }
-  else { //opening fresh file
+  else { // opening fresh file
-    file_subcall_ctr = 0; //resetting procedure depth in case user cancels print while in procedure
+    file_subcall_ctr = 0; // resetting procedure depth in case user cancels print while in procedure
+    ECHO_LMV(DB, "Now fresh file: ", name);
  }
  sdprinting = false;
@@ -210,22 +303,28 @@ void CardReader::openFile(char* name, bool read, bool replace_current/*=true*/,
  char *dirname_start, *dirname_end;
  if (name[0] == '/') {
    dirname_start = &name[1];
-    while(dirname_start > 0) {
+    while (dirname_start > 0) {
      dirname_end = strchr(dirname_start, '/');
      if (dirname_end > 0 && dirname_end > dirname_start) {
        char subdirname[FILENAME_LENGTH];
        strncpy(subdirname, dirname_start, dirname_end - dirname_start);
        subdirname[dirname_end - dirname_start] = 0;
+        ECHO_EV(subdirname);
        if (!myDir.open(curDir, subdirname, O_READ)) {
          ECHO_LMV(ER, MSG_SD_OPEN_FILE_FAIL, subdirname);
          return;
        }
+        else {
+          //ECHO_EM("dive ok");
+        }
        curDir = &myDir;
        dirname_start = dirname_end + 1;
      }
      else { // the remainder after all /fsa/fdsa/ is the filename
        fname = dirname_start;
+        //ECHO_EM("remainder");
+        //ECHO_EV(fname);
        break;
      }
    }
@@ -237,24 +336,28 @@ void CardReader::openFile(char* name, bool read, bool replace_current/*=true*/,
  if (read) {
    if (file.open(curDir, fname, O_READ)) {
      filesize = file.fileSize();
-      ECHO_SMV(OK,MSG_SD_FILE_OPENED, fname);
+      ECHO_MV(MSG_SD_FILE_OPENED, fname);
      ECHO_EMV(MSG_SD_SIZE, filesize);
      sdpos = 0;
+      ECHO_EM(MSG_SD_FILE_SELECTED);
      getfilename(0, fname);
      if(lcd_status) lcd_setstatus(longFilename[0] ? longFilename : fname);
    }
    else {
-      ECHO_LMV(ER,MSG_SD_OPEN_FILE_FAIL,fname);
+      ECHO_MV(MSG_SD_OPEN_FILE_FAIL, fname);
+      ECHO_PGM(".\n");
    }
  }
  else { //write
-    if (file.open(curDir, fname, O_CREAT | O_APPEND | O_WRITE | O_TRUNC)) {
+    if (!file.open(curDir, fname, O_CREAT | O_APPEND | O_WRITE | O_TRUNC)) {
-      saving = true;
+      ECHO_MV(MSG_SD_OPEN_FILE_FAIL, fname);
-      ECHO_LMV(OK, MSG_SD_WRITE_TO_FILE, name);
+      ECHO_PGM(".\n");
-      if(lcd_status) lcd_setstatus(fname);
    }
    else {
-      ECHO_LMV(ER, MSG_SD_OPEN_FILE_FAIL,fname);
+      saving = true;
+      ECHO_EMV(MSG_SD_WRITE_TO_FILE, name);
+      if(lcd_status) lcd_setstatus(fname);
    }
  }
 }
@@ -278,8 +381,10 @@ void CardReader::removeFile(char* name) {
        char subdirname[FILENAME_LENGTH];
        strncpy(subdirname, dirname_start, dirname_end - dirname_start);
        subdirname[dirname_end - dirname_start] = 0;
+        ECHO_EV(subdirname);
        if (!myDir.open(curDir, subdirname, O_READ)) {
-          ECHO_LMV(ER, MSG_SD_OPEN_FILE_FAIL, subdirname);
+          ECHO_MV(MSG_SD_OPEN_FILE_FAIL, subdirname);
+          ECHO_C('.');
          return;
        }
@@ -297,21 +402,22 @@ void CardReader::removeFile(char* name) {
  }
  if (file.remove(curDir, fname)) {
-    ECHO_LMV(OK, MSG_SD_FILE_DELETED, fname);
+    ECHO_EMV(MSG_SD_FILE_DELETED, fname);
    sdpos = 0;
  }
  else {
-    ECHO_LMV(ER, MSG_SD_FILE_DELETION_ERR,fname);
+    ECHO_MV(MSG_SD_FILE_DELETION_ERR, fname);
+    ECHO_C('.');
  }
 }
 void CardReader::getStatus() {
  if (cardOK) {
-    ECHO_SMV(OK, MSG_SD_PRINTING_BYTE, sdpos);
+    ECHO_MV(MSG_SD_PRINTING_BYTE, sdpos);
    ECHO_EMV(MSG_SD_SLASH, filesize);
  }
  else {
-    ECHO_LM(OK, MSG_SD_NOT_PRINTING);
+    ECHO_EM(MSG_SD_NOT_PRINTING);
  }
 }
@@ -345,7 +451,7 @@ void CardReader::checkautostart(bool force) {
    if (!cardOK) return; // fail
  }
-  char autoname[30];
+  char autoname[10];
  sprintf_P(autoname, PSTR("auto%i.g"), autostart_index);
  for (int8_t i = 0; i < (int8_t)strlen(autoname); i++) autoname[i] = tolower(autoname[i]);
@@ -357,7 +463,7 @@ void CardReader::checkautostart(bool force) {
  while (root.readDir(p, NULL) > 0) {
    for (int8_t i = 0; i < (int8_t)strlen((char*)p.name); i++) p.name[i] = tolower(p.name[i]);
    if (p.name[9] != '~' && strncmp((char*)p.name, autoname, 5) == 0) {
-      char cmd[30];
+      char cmd[4 + (FILENAME_LENGTH + 1) * MAX_DIR_DEPTH + 2];
      sprintf_P(cmd, PSTR("M23 %s"), autoname);
      enqueuecommand(cmd);
      enqueuecommands_P(PSTR("M24"));
@@ -504,22 +610,22 @@ void CardReader::chdir(const char * relpath) {
  if (workDir.isOpen()) parent = &workDir;
-  if (newfile.open(*parent, relpath, O_READ)) {
+  if (!newfile.open(*parent, relpath, O_READ)) {
+    ECHO_LMV(ER, MSG_SD_CANT_ENTER_SUBDIR, relpath);
+  }
+  else {
    if (workDirDepth < MAX_DIR_DEPTH) {
-      workDirDepth++;
+      ++workDirDepth;
      for (int d = workDirDepth; d--;) workDirParents[d + 1] = workDirParents[d];
      workDirParents[0] = *parent;
    }
    workDir = newfile;
  }
-  else {
-    ECHO_LMV(ER, MSG_SD_CANT_ENTER_SUBDIR, relpath);
-  }
 }
 void CardReader::updir() {
  if (workDirDepth > 0) {
-    workDirDepth--;
+    --workDirDepth;
    workDir = workDirParents[0];
    for (uint16_t d = 0; d < workDirDepth; d++)
      workDirParents[d] = workDirParents[d+1];

--- a/MarlinKimbra/cardreader.h
+++ b/MarlinKimbra/cardreader.h
@@ -18,7 +18,7 @@ public:
  //this is to delay autostart and hence the initialisaiton of the sd card to some seconds after the normal init, so the device is available quick after a reset
  void checkautostart(bool x);
-  void openFile(char* name, bool read, bool replace_current = true, bool lcd_status = true);
+  void openFile(char* name,bool read,bool replace_current=true,bool lcd_status=true);
  void openLogFile(char* name);
  void removeFile(char* name);
  void closeFile(bool store_location = false);
@@ -30,6 +30,10 @@ public:
  void getStatus();
  void printingHasFinished();
+  #ifdef LONG_FILENAME_HOST_SUPPORT
+    void printLongPath(char *path);
+  #endif
  void getfilename(uint16_t nr, const char* const match = NULL);
  uint16_t getnrfilenames();

--- a/MarlinKimbra/comunication.h
+++ b/MarlinKimbra/comunication.h
 /**
 *  Comunication.h - serial messages functions
- *  Part of Marlin
+ *  Part of MarlinKimbra
 *  
 *  Author: Simone Primarosa
 */
@@ -39,6 +39,7 @@
  #endif
 #endif
+<<<<<<< HEAD
 #define START       "start"                //start for host
 #define OK          "ok"                   //ok answer for host
 #define ER          "Error:"               //error for host
@@ -48,6 +49,17 @@
 #define PAUSE       "//action:pause"       //command for host that support action
 #define RESUME      "//action:resume"      //command for host that support action
 #define DISCONNECT  "//action:disconnect"  //command for host that support action
+=======
+#define START       "start"               //start for host
+#define OK          "ok "                 //ok answer for host
+#define ER          "Error: "             //error for host
+#define WT          "wait"                //wait for host
+#define DB          "echo: "              //message for user
+#define RS          "Resend: "            //resend for host
+#define PAUSE       "//action:pause"      //command for host that support action
+#define RESUME      "//action:resume"     //command for host that support action
+#define DISCONNECT  "//action:disconnect" //command for host that support action
+>>>>>>> origin/Development
 #define SERIAL_INIT(baud) MYSERIAL.begin(baud), delay(1)
 #define SERIAL_WRITE(x) MYSERIAL.write(x)
@@ -56,7 +68,10 @@
 FORCE_INLINE void PS_PGM(const char *str) {
  char ch;
-  while ((ch = pgm_read_byte(str++))) { SERIAL_WRITE(ch); }
+  while ((ch = pgm_read_byte(str))) {
+    MYSERIAL.write(ch);
+    str++;
+  }
 }
 #define ECHO_ENDL SERIAL_ENDL
@@ -65,8 +80,8 @@ FORCE_INLINE void PS_PGM(const char *str) {
 #define ECHO_MV(msg, val, args...) ECHO_PGM(msg),ECHO_V(val, ##args)
 #define ECHO_VM(val, msg, args...) ECHO_V(val, ##args),ECHO_PGM(msg)
 #define ECHO_M(msg) ECHO_PGM(msg)
-#define ECHO_V SERIAL_PRINT
+#define ECHO_V(msg, args...) SERIAL_PRINT(msg, ##args)
-#define ECHO_C SERIAL_WRITE
+#define ECHO_C(x) SERIAL_WRITE(x)
 #define ECHO_S(srt) ECHO_PGM(srt)
 #define ECHO_SM(srt, msg) ECHO_S(srt),ECHO_M(msg)

--- a/MarlinKimbra/conditionals.h
+++ b/MarlinKimbra/conditionals.h
@@ -4,17 +4,12 @@
 */
 #ifndef CONDITIONALS_H
-#ifndef M_PI
+  #ifndef CONFIGURATION_LCD // Get the LCD defines which are needed first
-  #define M_PI 3.1415926536
+    #define CONFIGURATION_LCD
-#endif
-#ifndef CONFIGURATION_LCD // Get the LCD defines which are needed first
    #define PIN_EXISTS(PN) (defined(PN##_PIN) && PN##_PIN >= 0)
-  #define CONFIGURATION_LCD
+    #ifdef MAKRPANEL
-  #if defined(MAKRPANEL)
      #define DOGLCD
      #define SDSUPPORT
      #define DEFAULT_LCD_CONTRAST 17
@@ -22,21 +17,32 @@
      #define NEWPANEL
    #endif
-  #if defined(miniVIKI) || defined(VIKI2)
+    #if defined(miniVIKI) || defined(VIKI2) || defined(ELB_FULL_GRAPHIC_CONTROLLER)
      #define ULTRA_LCD  //general LCD support, also 16x2
      #define DOGLCD  // Support for SPI LCD 128x64 (Controller ST7565R graphic Display Family)
      #define ULTIMAKERCONTROLLER //as available from the Ultimaker online store.
      #ifdef miniVIKI
        #define DEFAULT_LCD_CONTRAST 95
-    #else
+      #elif defined(VIKI2)
        #define DEFAULT_LCD_CONTRAST 40
+      #elif defined(ELB_FULL_GRAPHIC_CONTROLLER)
+        #define DEFAULT_LCD_CONTRAST 110
+        #define SDCARDDETECTINVERTED
+        #define SDSLOW
+        #define U8GLIB_LM6059_AF
      #endif
      #define ENCODER_PULSES_PER_STEP 4
      #define ENCODER_STEPS_PER_MENU_ITEM 1
    #endif
+    // Generic support for SSD1306 OLED based LCDs.
+    #if defined(U8GLIB_SSD1306)
+      #define ULTRA_LCD  //general LCD support, also 16x2
+      #define DOGLCD  // Support for I2C LCD 128x64 (Controller SSD1306 graphic Display Family)
+    #endif
    #ifdef PANEL_ONE
      #define SDSUPPORT
      #define ULTIMAKERCONTROLLER
@@ -53,7 +59,7 @@
      #define NEWPANEL
    #endif
-  #if defined(RADDS_DISPLAY)
+    #ifdef RADDS_DISPLAY
      #define ENCODER_PULSES_PER_STEP 2
      #define ENCODER_STEPS_PER_MENU_ITEM 1
@@ -97,11 +103,11 @@
      #define LCD_I2C_ADDRESS 0x20 // I2C Address of the port expander
      #define LCD_USE_I2C_BUZZER //comment out to disable buzzer on LCD
-    #ifndef ENCODER_PULSES_PER_STEP
+      #if !defined(ENCODER_PULSES_PER_STEP)
        #define ENCODER_PULSES_PER_STEP 4
      #endif
-    #ifndef ENCODER_STEPS_PER_MENU_ITEM
+      #if !defined(ENCODER_STEPS_PER_MENU_ITEM)
        #define ENCODER_STEPS_PER_MENU_ITEM 1
      #endif
@@ -202,6 +208,9 @@
      #ifdef U8GLIB_ST7920
        #undef HAS_LCD_CONTRAST
      #endif
+      #ifdef U8GLIB_SSD1306
+        #undef HAS_LCD_CONTRAST
+      #endif
    #endif
    /**
@@ -212,13 +221,12 @@
    /**
     * SPLASH_SCREEN_DURATION for no DOGLCD display
     */
-  #ifndef DOGLCD
+    #if !defined(DOGLCD)
      #undef SPLASH_SCREEN_DURATION
      #define SPLASH_SCREEN_DURATION 500
    #endif
-#else // CONFIGURATION_LCD
+  #else // CONFIGURATION_LCD
    #define CONDITIONALS_H
    /**
@@ -273,19 +281,23 @@
      #define X_MIN_ENDSTOP_INVERTING   !X_MIN_ENDSTOP_LOGIC
      #define Y_MIN_ENDSTOP_INVERTING   !Y_MIN_ENDSTOP_LOGIC
      #define Z_MIN_ENDSTOP_INVERTING   !Z_MIN_ENDSTOP_LOGIC
+      #define Z2_MIN_ENDSTOP_INVERTING  !Z2_MIN_ENDSTOP_LOGIC
      #define E_MIN_ENDSTOP_INVERTING   !E_MIN_ENDSTOP_LOGIC
      #define X_MAX_ENDSTOP_INVERTING   !X_MAX_ENDSTOP_LOGIC
      #define Y_MAX_ENDSTOP_INVERTING   !Y_MAX_ENDSTOP_LOGIC
      #define Z_MAX_ENDSTOP_INVERTING   !Z_MAX_ENDSTOP_LOGIC
+      #define Z2_MAX_ENDSTOP_INVERTING  !Z2_MAX_ENDSTOP_LOGIC
      #define Z_PROBE_ENDSTOP_INVERTING !Z_PROBE_ENDSTOP_LOGIC
    #else
      #define X_MIN_ENDSTOP_INVERTING   X_MIN_ENDSTOP_LOGIC
      #define Y_MIN_ENDSTOP_INVERTING   Y_MIN_ENDSTOP_LOGIC
      #define Z_MIN_ENDSTOP_INVERTING   Z_MIN_ENDSTOP_LOGIC
+      #define Z2_MIN_ENDSTOP_INVERTING  Z2_MIN_ENDSTOP_LOGIC
      #define E_MIN_ENDSTOP_INVERTING   E_MIN_ENDSTOP_LOGIC
      #define X_MAX_ENDSTOP_INVERTING   X_MAX_ENDSTOP_LOGIC
      #define Y_MAX_ENDSTOP_INVERTING   Y_MAX_ENDSTOP_LOGIC
      #define Z_MAX_ENDSTOP_INVERTING   Z_MAX_ENDSTOP_LOGIC
+      #define Z2_MAX_ENDSTOP_INVERTING  Z2_MAX_ENDSTOP_LOGIC
      #define Z_PROBE_ENDSTOP_INVERTING Z_PROBE_ENDSTOP_LOGIC
    #endif
@@ -317,6 +329,7 @@
     */
    #ifdef DELTA
      #undef SLOWDOWN //DELTA not needs SLOWDOWN
+      #define AUTOLEVEL_GRID_MULTI 1/AUTOLEVEL_GRID
      // DELTA must have same valour for 3 axis endstop hits
      #undef Y_HOME_BUMP_MM
      #undef Z_HOME_BUMP_MM
@@ -354,6 +367,180 @@
      #define MAX_PROBE_Y (min(Y_MAX_POS, Y_MAX_POS + Y_PROBE_OFFSET_FROM_EXTRUDER))
    #endif
+     /**
+      * Sled Options
+      */ 
+    #ifdef Z_PROBE_SLED
+      #define Z_SAFE_HOMING
+    #endif
+<<<<<<< HEAD
+  #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
+   */
+  #if defined(DOGLCD) && !defined(DEFAULT_LCD_CONTRAST)
+    #define DEFAULT_LCD_CONTRAST 32
+  #endif
+  #ifdef DOGLCD
+    #define HAS_LCD_CONTRAST
+    #ifdef U8GLIB_ST7920
+      #undef HAS_LCD_CONTRAST
+    #endif
+  #endif
+  /**
+   * LCD BUZZ
+   */
+  #define HAS_LCD_BUZZ (defined(ULTRALCD) || (defined(BEEPER) && BEEPER >= 0) || defined(LCD_USE_I2C_BUZZER))
+  /**
+   * SPLASH_SCREEN_DURATION for no DOGLCD display
+   */
+  #ifndef DOGLCD
+    #undef SPLASH_SCREEN_DURATION
+    #define SPLASH_SCREEN_DURATION 500
+  #endif
+#else // CONFIGURATION_LCD
+  #define CONDITIONALS_H
+  /**
+   * SINGLENOZZLE
+   */
+  #ifdef SINGLENOZZLE
+    #define HOTENDS 1
+    #undef TEMP_SENSOR_1_AS_REDUNDANT
+  #else
+    #define HOTENDS EXTRUDERS
+  #endif
+  /**
+   * DRIVER_EXTRUDERS
+   */
+  #if !defined(MKR4) && !defined(NPR2)
+    #define DRIVER_EXTRUDERS EXTRUDERS // This defines the number of Driver extruder
+  #endif
+  #ifndef __SAM3X8E__
+    #ifndef AT90USB
+      #define HardwareSerial_h // trick to disable the standard HWserial
+    #endif
+  #endif
+  #if (ARDUINO >= 100)
+    #include "Arduino.h"
+  #else
+    #include "WProgram.h"
+  #endif
+  #include "pins.h"
+  /**
+   * ENDSTOPPULLUPS
+   */
+  #ifdef ENDSTOPPULLUPS
+    #define ENDSTOPPULLUP_XMAX
+    #define ENDSTOPPULLUP_YMAX
+    #define ENDSTOPPULLUP_ZMAX
+    #define ENDSTOPPULLUP_XMIN
+    #define ENDSTOPPULLUP_YMIN
+    #define ENDSTOPPULLUP_ZMIN
+    #define ENDSTOPPULLUP_EMIN
+    #define ENDSTOPPULLUP_ZPROBE
+  #endif
+  /**
+   * ENDSTOP LOGICAL
+   */
+  #if MB(ALLIGATOR)
+    #define X_MIN_ENDSTOP_INVERTING   !X_MIN_ENDSTOP_LOGIC
+    #define Y_MIN_ENDSTOP_INVERTING   !Y_MIN_ENDSTOP_LOGIC
+    #define Z_MIN_ENDSTOP_INVERTING   !Z_MIN_ENDSTOP_LOGIC
+    #define E_MIN_ENDSTOP_INVERTING   !E_MIN_ENDSTOP_LOGIC
+    #define X_MAX_ENDSTOP_INVERTING   !X_MAX_ENDSTOP_LOGIC
+    #define Y_MAX_ENDSTOP_INVERTING   !Y_MAX_ENDSTOP_LOGIC
+    #define Z_MAX_ENDSTOP_INVERTING   !Z_MAX_ENDSTOP_LOGIC
+    #define Z_PROBE_ENDSTOP_INVERTING !Z_PROBE_ENDSTOP_LOGIC
+  #else
+    #define X_MIN_ENDSTOP_INVERTING   X_MIN_ENDSTOP_LOGIC
+    #define Y_MIN_ENDSTOP_INVERTING   Y_MIN_ENDSTOP_LOGIC
+    #define Z_MIN_ENDSTOP_INVERTING   Z_MIN_ENDSTOP_LOGIC
+    #define E_MIN_ENDSTOP_INVERTING   E_MIN_ENDSTOP_LOGIC
+    #define X_MAX_ENDSTOP_INVERTING   X_MAX_ENDSTOP_LOGIC
+    #define Y_MAX_ENDSTOP_INVERTING   Y_MAX_ENDSTOP_LOGIC
+    #define Z_MAX_ENDSTOP_INVERTING   Z_MAX_ENDSTOP_LOGIC
+    #define Z_PROBE_ENDSTOP_INVERTING Z_PROBE_ENDSTOP_LOGIC
+  #endif
+  /**
+   * Firmware Test
+   */
+  #ifdef FIRMWARE_TEST
+    #undef BAUDRATE
+    #define BAUDRATE 115200  // Baudrate setting to 115200 because serial monitor arduino function at max 115200 baudrate.
+  #endif
+  /**
+   * Axis lengths
+   */
+  #define X_MAX_LENGTH (X_MAX_POS - X_MIN_POS)
+  #define Y_MAX_LENGTH (Y_MAX_POS - Y_MIN_POS)
+  #define Z_MAX_LENGTH (Z_MAX_POS - Z_MIN_POS)
+  /**
+   * SCARA
+   */
+  #ifdef SCARA
+    #undef SLOWDOWN
+    #define QUICK_HOME //SCARA needs Quickhome
+  #endif
+  /**
+   * DELTA
+   */
+  #ifdef DELTA
+    #undef SLOWDOWN //DELTA not needs SLOWDOWN
+    // DELTA must have same valour for 3 axis endstop hits
+    #undef Y_HOME_BUMP_MM
+    #undef Z_HOME_BUMP_MM
+    #define Y_HOME_BUMP_MM X_HOME_BUMP_MM
+    #define Z_HOME_BUMP_MM X_HOME_BUMP_MM
+  #endif
+=======
+>>>>>>> origin/Development
    /**
     * Servo Leveling
     */
@@ -368,8 +555,8 @@
        #define MAX_STEP_FREQUENCY 120000 // Max step frequency for Toshiba Stepper Controllers
        #define DOUBLE_STEP_FREQUENCY MAX_STEP_FREQUENCY
      #else
-      #define MAX_STEP_FREQUENCY 320000    // Max step frequency for the Due is approx. 330kHz
+        #define MAX_STEP_FREQUENCY 500000    // Max step frequency for the Due is approx. 330kHz
-      #define DOUBLE_STEP_FREQUENCY 100000  //96kHz is close to maximum for an Arduino Due
+        #define DOUBLE_STEP_FREQUENCY 120000  //96kHz is close to maximum for an Arduino Due
      #endif
    #else
      #ifdef CONFIG_STEPPERS_TOSHIBA
@@ -468,6 +655,8 @@
    #elif TEMP_SENSOR_BED == 0
      #undef BED_MINTEMP
      #undef BED_MAXTEMP
+      #undef THERMAL_PROTECTION_BED
+      #undef THERMAL_PROTECTION_BED_PERIOD
    #elif TEMP_SENSOR_BED > 0
      #define THERMISTORBED TEMP_SENSOR_BED
      #define BED_USES_THERMISTOR
@@ -499,7 +688,6 @@
      #define ARRAY_BY_HOTENDS(v1, v2, v3, v4) { v1 }
    #endif
    /**
     * Shorthand for pin tests, used wherever needed
     */
@@ -585,6 +773,12 @@
    #define HAS_E1E3 (PIN_EXISTS(E1E3_CHOICE))
    #define HAS_BTN_BACK (PIN_EXISTS(BTN_BACK))
+    /**
+     * Shorthand for filament sensor and power sensor for ultralcd.cpp, dogm_lcd_implementation.h, ultralcd_implementation_hitachi_HD44780.h
+     */
+    #define HAS_LCD_FILAMENT_SENSOR (HAS_FILAMENT_SENSOR && defined(FILAMENT_LCD_DISPLAY))
+    #define HAS_LCD_POWER_SENSOR (HAS_POWER_CONSUMPTION_SENSOR && defined(POWER_CONSUMPTION_LCD_DISPLAY))
    /**
     * Helper Macros for heaters and extruder fan
     */
@@ -615,11 +809,5 @@
      #define WRITE_FAN(v) WRITE(FAN_PIN, v)
    #endif
-  /**
+  #endif //CONFIGURATION_LCD
-   * Shorthand for filament sensor and power sensor for ultralcd.cpp, dogm_lcd_implementation.h, ultralcd_implementation_hitachi_HD44780.h
-   */
-  #define HAS_LCD_FILAMENT_SENSOR (HAS_FILAMENT_SENSOR && defined(FILAMENT_LCD_DISPLAY))
-  #define HAS_LCD_POWER_SENSOR (HAS_POWER_CONSUMPTION_SENSOR && defined(POWER_CONSUMPTION_LCD_DISPLAY))
-#endif //CONFIGURATION_LCD
 #endif //CONDITIONALS_H
--- a/MarlinKimbra/configuration_store.cpp
+++ b/MarlinKimbra/configuration_store.cpp
@@ -14,15 +14,14 @@
 *
 */
-#define EEPROM_VERSION "V21"
+#define EEPROM_VERSION "V22"
 /**
- * V21 EEPROM Layout:
+ * V22 EEPROM Layout:
 *
 *  ver
 *  M92   XYZ E0 E1 E2 E3 axis_steps_per_unit (x7)
 *  M203  XYZ E0 E1 E2 E3 max_feedrate (x7)
- *  M???      E0 E1 E2 E3 retraction_feedrate (x4)
 *  M201  XYZ E0 E1 E2 E3 max_acceleration_units_per_sq_second (x7)
 *  M204  P               acceleration
 *  M204  R               retract_acceleration
@@ -45,7 +44,7 @@
 *  M666  R               delta_radius
 *  M666  D               delta_diagonal_rod
 *  M666  H               Z max_pos
- *  M666  P               z_probe_offset
+ *  M666  P XYZ           XYZ probe_offset (x3)
 *
 * Z_DUAL_ENDSTOPS
 *  M666  Z               z_endstop_adj
@@ -103,6 +102,10 @@
 #include "ultralcd.h"
 #include "configuration_store.h"
+#ifdef SDSUPPORT
+  #include "cardreader.h"
+#endif
 void _EEPROM_writeData(int &pos, uint8_t* value, uint8_t size) {
  uint8_t c;
  while(size--) {
@@ -127,17 +130,16 @@ 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
 #define EEPROM_OFFSET 100
-#define LIFETIME_EEPROM_OFFSET 600
-#define LIFETIME_MAGIC "L99"
 #ifdef EEPROM_SETTINGS
-/**
- * Store Configuration Settings - M500
- */
 void Config_StoreSettings() {
  float dummy = 0.0f;
  char ver[4] = "000";
@@ -145,7 +147,6 @@ void Config_StoreSettings() {
  EEPROM_WRITE_VAR(i, ver); // invalidate data first
  EEPROM_WRITE_VAR(i, axis_steps_per_unit);
  EEPROM_WRITE_VAR(i, max_feedrate);
-  EEPROM_WRITE_VAR(i, max_retraction_feedrate);
  EEPROM_WRITE_VAR(i, max_acceleration_units_per_sq_second);
  EEPROM_WRITE_VAR(i, acceleration);
  EEPROM_WRITE_VAR(i, retract_acceleration);
@@ -178,9 +179,9 @@ void Config_StoreSettings() {
  #endif
  #ifndef ULTIPANEL
-    int plaPreheatHotendTemp = PLA_PREHEAT_HOTEND_TEMP, plaPreheatHPBTemp = PLA_PREHEAT_HPB_TEMP, plaPreheatFanSpeed = PLA_PREHEAT_FAN_SPEED;
+    int plaPreheatHotendTemp = PLA_PREHEAT_HOTEND_TEMP, plaPreheatHPBTemp = PLA_PREHEAT_HPB_TEMP, plaPreheatFanSpeed = PLA_PREHEAT_FAN_SPEED,
-    int absPreheatHotendTemp = ABS_PREHEAT_HOTEND_TEMP, absPreheatHPBTemp = ABS_PREHEAT_HPB_TEMP, absPreheatFanSpeed = ABS_PREHEAT_FAN_SPEED;
+        absPreheatHotendTemp = ABS_PREHEAT_HOTEND_TEMP, absPreheatHPBTemp = ABS_PREHEAT_HPB_TEMP, absPreheatFanSpeed = ABS_PREHEAT_FAN_SPEED,
-    int gumPreheatHotendTemp = GUM_PREHEAT_HOTEND_TEMP, gumPreheatHPBTemp = GUM_PREHEAT_HPB_TEMP, gumPreheatFanSpeed = GUM_PREHEAT_FAN_SPEED;
+        gumPreheatHotendTemp = GUM_PREHEAT_HOTEND_TEMP, gumPreheatHPBTemp = GUM_PREHEAT_HPB_TEMP, gumPreheatFanSpeed = GUM_PREHEAT_FAN_SPEED;
  #endif
  EEPROM_WRITE_VAR(i, plaPreheatHotendTemp);
@@ -219,8 +220,8 @@ void Config_StoreSettings() {
  EEPROM_WRITE_VAR(i, bedKi);
  EEPROM_WRITE_VAR(i, bedKd);
-  #if !defined(DOGLCD) || LCD_CONTRAST < 0
+  #if defined(DOGLCD) || LCD_CONTRAST < 0
-    int lcd_contrast = 32;
+    const int lcd_contrast = 32;
  #endif
  EEPROM_WRITE_VAR(i, lcd_contrast);
@@ -293,7 +294,6 @@ void Config_RetrieveSettings() {
    // version number match
    EEPROM_READ_VAR(i, axis_steps_per_unit);
    EEPROM_READ_VAR(i, max_feedrate);
-    EEPROM_READ_VAR(i, max_retraction_feedrate);
    EEPROM_READ_VAR(i, max_acceleration_units_per_sq_second);
    // steps per sq second need to be updated to agree with the units per sq second (as they are what is used in the planner)
@@ -377,9 +377,9 @@ void Config_RetrieveSettings() {
      for (int q = 2; q--;) EEPROM_READ_VAR(i, dummy); // bedKi, bedKd
    }
-    #if !defined(DOGLCD) || LCD_CONTRAST < 0
+    #if defined(DOGLCD) || LCD_CONTRAST < 0
      int lcd_contrast;
-    #endif //DOGLCD
+    #endif
    EEPROM_READ_VAR(i, lcd_contrast);
@@ -441,36 +441,33 @@ void Config_RetrieveSettings() {
 * Reset Configuration Settings - M502
 */
 void Config_ResetDefault() {
  float tmp1[] = DEFAULT_AXIS_STEPS_PER_UNIT;
  float tmp2[] = DEFAULT_MAX_FEEDRATE;
-  float tmp3[] = DEFAULT_RETRACTION_MAX_FEEDRATE;
+  long  tmp3[] = DEFAULT_MAX_ACCELERATION;
-  long  tmp4[]  = DEFAULT_MAX_ACCELERATION;
  #ifdef PIDTEMP
-    float tmp5[] = DEFAULT_Kp;
+    float tmp4[] = DEFAULT_Kp;
-    float tmp6[] = DEFAULT_Ki;
+    float tmp5[] = DEFAULT_Ki;
-    float tmp7[] = DEFAULT_Kd;
+    float tmp6[] = DEFAULT_Kd;
  #endif // PIDTEMP
  #if defined(HOTEND_OFFSET_X) && defined(HOTEND_OFFSET_Y)
-    float tmp8[] = HOTEND_OFFSET_X;
+    float tmp7[] = HOTEND_OFFSET_X;
-    float tmp9[] = HOTEND_OFFSET_Y;
+    float tmp8[] = HOTEND_OFFSET_Y;
  #else
+    float tmp7[] = {0,0,0,0};
    float tmp8[] = {0,0,0,0};
-    float tmp9[] = {0,0,0,0};
  #endif
  for (int i = 0; i < 3 + EXTRUDERS; i++) {
    axis_steps_per_unit[i] = tmp1[i];
    max_feedrate[i] = tmp2[i];
-    max_acceleration_units_per_sq_second[i] = tmp4[i];
+    max_acceleration_units_per_sq_second[i] = tmp3[i];
  }
  for (int i = 0; i < EXTRUDERS; i++) {
-    max_retraction_feedrate[i] = tmp3[i];
    #if HOTENDS > 1
-      hotend_offset[X_AXIS][i] = tmp8[i];
+      hotend_offset[X_AXIS][i] = tmp7[i];
-      hotend_offset[Y_AXIS][i] = tmp9[i];
+      hotend_offset[Y_AXIS][i] = tmp8[i];
    #endif
    #ifdef SCARA
      if (i < sizeof(axis_scaling) / sizeof(*axis_scaling))
@@ -494,9 +491,9 @@ void Config_ResetDefault() {
  #ifdef ENABLE_AUTO_BED_LEVELING
    zprobe_zoffset = -Z_PROBE_OFFSET_FROM_EXTRUDER;
-  #elif !defined DELTA
+  #elif !defined(DELTA)
    zprobe_zoffset = 0;
-  #endif //ENABLE_AUTO_BED_LEVELING
+  #endif
  #ifdef DELTA
    endstop_adj[X_AXIS] = endstop_adj[Y_AXIS] = endstop_adj[Z_AXIS] = 0;
@@ -506,7 +503,7 @@ void Config_ResetDefault() {
    max_pos[2] = MANUAL_Z_HOME_POS;
    set_default_z_probe_offset();
    set_delta_constants();
-  #endif //DELTA
+  #endif
  #ifdef ULTIPANEL
    plaPreheatHotendTemp = PLA_PREHEAT_HOTEND_TEMP;
@@ -520,20 +517,26 @@ void Config_ResetDefault() {
    gumPreheatFanSpeed = GUM_PREHEAT_FAN_SPEED;
  #endif
-  #if defined(DOGLCD) && LCD_CONTRAST >= 0
+  #ifdef HAS_LCD_CONTRAST
    lcd_contrast = DEFAULT_LCD_CONTRAST;
  #endif //DOGLCD
  #ifdef PIDTEMP
    for (int e = 0; e < HOTENDS; e++) 
    {
-      Kp[e] = tmp5[e];
+      Kp[e] = tmp4[e];
-      Ki[e] = scalePID_i(tmp6[e]);
+      Ki[e] = scalePID_i(tmp5[e]);
-      Kd[e] = scalePID_d(tmp7[e]);
+      Kd[e] = scalePID_d(tmp6[e]);
    }
    // call updatePID (similar to when we have processed M301)
    updatePID();
-  #endif//PIDTEMP
+  #endif // PIDTEMP
+  #ifdef PIDTEMPBED
+    bedKp = DEFAULT_bedKp;
+    bedKi = scalePID_i(DEFAULT_bedKi);
+    bedKd = scalePID_d(DEFAULT_bedKd);
+  #endif
  #ifdef FWRETRACT
    autoretract_enabled = false;
@@ -558,9 +561,9 @@ void Config_ResetDefault() {
      filament_size[2] = DEFAULT_NOMINAL_FILAMENT_DIA;
      #if EXTRUDERS > 3
        filament_size[3] = DEFAULT_NOMINAL_FILAMENT_DIA;
-      #endif //EXTRUDERS > 3
+      #endif // EXTRUDERS > 3
-    #endif //EXTRUDERS > 2
+    #endif // EXTRUDERS > 2
-  #endif //EXTRUDERS > 1
+  #endif // EXTRUDERS > 1
  calculate_volumetric_multipliers();
  #ifdef IDLE_OOZING_PREVENT
@@ -570,12 +573,12 @@ void Config_ResetDefault() {
  ECHO_LM(DB, "Hardcoded Default Settings Loaded");
 }
-#ifndef DISABLE_M503
+#if !defined(DISABLE_M503)
-/**
+  /**
- * Print Configuration Settings - M502
+   * Print Configuration Settings - M503
   */
-void Config_PrintSettings(bool forReplay) {
+  void Config_PrintSettings(bool forReplay) {
    // Always have this function, even with EEPROM_SETTINGS disabled, the current values will be shown
    if (!forReplay) {
@@ -623,21 +626,6 @@ void Config_PrintSettings(bool forReplay) {
    #endif //EXTRUDERS > 1
    ECHO_E;
-  if (!forReplay) {
-    ECHO_LM(DB, "Retraction Steps per unit:");
-  }
-  ECHO_SMV(DB, "  E0 ",max_retraction_feedrate[0]);
-  #if EXTRUDERS > 1
-    ECHO_MV(" E1 ", max_retraction_feedrate[1]);
-    #if EXTRUDERS > 2
-      ECHO_MV(" E2 ", max_retraction_feedrate[2]);
-      #if EXTRUDERS > 3
-        ECHO_MV(" E3 ", max_retraction_feedrate[3]);
-      #endif //EXTRUDERS > 3
-    #endif //EXTRUDERS > 2
-  #endif //EXTRUDERS > 1
-  ECHO_E;
    if (!forReplay) {
      ECHO_LM(DB, "Maximum Acceleration (mm/s2):");
    }
@@ -692,36 +680,32 @@ void Config_PrintSettings(bool forReplay) {
    #endif //HOTENDS > 1
    #ifdef DELTA
-    if (!forReplay) {
-      ECHO_LM(DB, "Endstop adjustement (mm):");
-    }
-    ECHO_SMV(DB, "  M666 X", endstop_adj[X_AXIS] );
-    ECHO_MV(" Y", endstop_adj[Y_AXIS] );
-    ECHO_EMV(" Z", endstop_adj[Z_AXIS] );
      if (!forReplay) {
        ECHO_LM(DB, "Delta Geometry adjustment:");
      }
-    ECHO_SMV(DB, "  M666 A", tower_adj[0]);
+      ECHO_SMV(DB, "  M666 A", tower_adj[0], 3);
-    ECHO_MV(" B", tower_adj[1]);
+      ECHO_MV(" B", tower_adj[1], 3);
-    ECHO_MV(" C", tower_adj[2]);
+      ECHO_MV(" C", tower_adj[2], 3);
-    ECHO_MV(" E", tower_adj[3]);
+      ECHO_MV(" I", tower_adj[3], 3);
-    ECHO_MV(" F", tower_adj[4]);
+      ECHO_MV(" J", tower_adj[4], 3);
-    ECHO_MV(" G", tower_adj[5]);
+      ECHO_MV(" K", tower_adj[5], 3);
      ECHO_MV(" R", delta_radius);
      ECHO_MV(" D", delta_diagonal_rod);
-    ECHO_MV(" H", max_pos[2]);
+      ECHO_EMV(" H", max_pos[2]);
-    ECHO_EMV(" P", z_probe_offset[3]);
+      if (!forReplay) {
+        ECHO_LM(DB, "Endstop Offsets:");
+      }
+      ECHO_SMV(DB, "  M666 X", endstop_adj[X_AXIS]);
+      ECHO_MV(" Y", endstop_adj[Y_AXIS]);
+      ECHO_EMV(" Z", endstop_adj[Z_AXIS]);
      if (!forReplay) {
-      ECHO_LM(DB, "Tower Positions:");
+        ECHO_LM(DB, "Z-Probe Offset:");
      }
-    ECHO_SMV(DB, "  Tower1 X:", delta_tower1_x);
+      ECHO_SMV(DB, "  M666 P X", z_probe_offset[0]);
-    ECHO_MV(" Y:", delta_tower1_y);
+      ECHO_MV(" Y", z_probe_offset[1]);
-    ECHO_MV(" Tower2 X:", delta_tower2_x);
+      ECHO_EMV(" Z", z_probe_offset[2]);
-    ECHO_MV(" Y:", delta_tower2_y);
-    ECHO_MV(" Tower3 X:", delta_tower3_x);
-    ECHO_EMV(" Y:", delta_tower3_y);
    #elif defined(Z_DUAL_ENDSTOPS)
      if (!forReplay) {
@@ -733,7 +717,7 @@ void Config_PrintSettings(bool forReplay) {
        ECHO_LM(DB, "Z Probe offset (mm)");
      }
      ECHO_LMV(DB, "  M666 P", zprobe_zoffset);
-  #endif // DELTA
+    #endif
    #ifdef ULTIPANEL
      if (!forReplay) {
@@ -823,64 +807,103 @@ void Config_PrintSettings(bool forReplay) {
      }
    }
-  // Print Lifetime stats
+    ConfigSD_PrintSettings(forReplay);
+  }
+  void ConfigSD_PrintSettings(bool forReplay) {
+    // Always have this function, even with SD_SETTINGS disabled, the current values will be shown
    #ifdef POWER_CONSUMPTION
      if (!forReplay) {
        ECHO_LM(DB, "Watt/h consumed:");
      }
-    ECHO_LVM(DB, power_consumption_hour," W/h");
+      ECHO_LVM(OK, power_consumption_hour," W/h");
    #endif
    if (!forReplay) {
      ECHO_LM(DB, "Power on time:");
    }
    char time[30];
-  int hours = printer_usage_seconds / 60 / 60, minutes = (printer_usage_seconds / 60) % 60;
+    int day = printer_usage_seconds / 60 / 60 / 24, hours = (printer_usage_seconds / 60 / 60) % 24, minutes = (printer_usage_seconds / 60) % 60;
-  sprintf_P(time, PSTR("%i " MSG_END_HOUR " %i " MSG_END_MINUTE), hours, minutes);
+    sprintf_P(time, PSTR("  %i " MSG_END_DAY " %i " MSG_END_HOUR " %i " MSG_END_MINUTE), day, hours, minutes);
    ECHO_LV(DB, time);
+  }
-}
+#endif // !DISABLE_M503
-#endif //!DISABLE_M503
 /**
- * Lifetime on EEPROM
+ * Configuration on SD card
+ *
+ * Author: Simone Primarosa
 *
 */
-void load_lifetime_stats() {
+void ConfigSD_ResetDefault() {
-  int i = LIFETIME_EEPROM_OFFSET;
-  char stored_magic[4];
-  char magic[4] = LIFETIME_MAGIC;
-  EEPROM_READ_VAR(i, stored_magic); // read magic
-  if (strncmp(magic, stored_magic, 3) != 0) {
  #ifdef POWER_CONSUMPTION
   power_consumption_hour = 0;
  #endif
  printer_usage_seconds  = 0;
-  }
+  ECHO_LM(OK, "Hardcoded SD Default Settings Loaded");
-  else {
-    EEPROM_READ_VAR(i, printer_usage_seconds);
-    #ifdef POWER_CONSUMPTION
-      EEPROM_READ_VAR(i, power_consumption_hour);
-    #endif
-  }
 }
-void save_lifetime_stats() {
+#if defined(SDSUPPORT) && defined(SD_SETTINGS)
-  int i = LIFETIME_EEPROM_OFFSET;
-  char magic[4] = "000";
-  EEPROM_WRITE_VAR(i, magic); // invalidate data first
-  EEPROM_WRITE_VAR(i, printer_usage_seconds);
+  void ConfigSD_StoreSettings() {
+    if(!IS_SD_INSERTED || card.isFileOpen() || card.sdprinting) return;
+    card.openFile(CFG_SD_FILE, false, true, false);
+    char buff[CFG_SD_MAX_VALUE_LEN];
+    #ifdef POWER_CONSUMPTION
+      ltoa(power_consumption_hour,buff,10);
+      card.unparseKeyLine(cfgSD_KEY[SD_CFG_PWR], buff);
+    #endif
+    ltoa(printer_usage_seconds,buff,10);
+    card.unparseKeyLine(cfgSD_KEY[SD_CFG_TME], buff);
+    card.closeFile(false);
+    config_last_update = millis();
+  }
+  void ConfigSD_RetrieveSettings(bool addValue) {
+    if(!IS_SD_INSERTED || card.isFileOpen() || card.sdprinting || !card.cardOK) return;
+    char key[CFG_SD_MAX_KEY_LEN], value[CFG_SD_MAX_VALUE_LEN];
+    int k_idx;
+    int k_len, v_len;
+    card.openFile(CFG_SD_FILE, true, true, false);
+    while(true) {
+      k_len = CFG_SD_MAX_KEY_LEN;
+      v_len = CFG_SD_MAX_VALUE_LEN;
+      card.parseKeyLine(key, value, k_len, v_len);
+      if(k_len == 0 || v_len == 0) break; //no valid key or value founded
+      k_idx = ConfigSD_KeyIndex(key);
+      if(k_idx == -1) continue;    //unknow key ignore it
+      switch(k_idx) {
        #ifdef POWER_CONSUMPTION
-    EEPROM_WRITE_VAR(i, power_consumption_hour);
+        case SD_CFG_PWR: {
+          if(addValue) power_consumption_hour += (unsigned long)atol(value);
+          else power_consumption_hour = (unsigned long)atol(value);
+        }
+        break;
        #endif
+        case SD_CFG_TME: {
+          if(addValue) printer_usage_seconds += (unsigned long)atol(value);
+          else printer_usage_seconds = (unsigned long)atol(value);
+        }
+        break;
+      }
+    }
+    card.closeFile(false);
+    config_readed = true;
+  }
-  char magic2[4] = LIFETIME_MAGIC;
+  int ConfigSD_KeyIndex(char *key) {    //At the moment a binary search algorithm is used for simplicity, if it will be necessary (Eg. tons of key), an hash search algorithm will be implemented.
-  int j = LIFETIME_EEPROM_OFFSET;
+    int begin = 0, end = SD_CFG_END - 1, middle, cond;
-  EEPROM_WRITE_VAR(j, magic2); // validate data
+    while(begin <= end) {
+      middle = (begin + end) / 2;
+      cond = strcmp(cfgSD_KEY[middle], key);
+      if(!cond) return middle;
+      else if(cond < 0) begin = middle + 1;
+      else end = middle - 1;
+    }
+    return -1;
+  }
-  config_last_update = millis();
+#endif
-}
--- a/MarlinKimbra/configuration_store.h
+++ b/MarlinKimbra/configuration_store.h
@@ -4,13 +4,14 @@
 #include "Configuration.h"
 void Config_ResetDefault();
-void load_lifetime_stats();
+void ConfigSD_ResetDefault();
-void save_lifetime_stats();
 #ifndef DISABLE_M503
-  void Config_PrintSettings(bool forReplay=false);
+  void Config_PrintSettings(bool forReplay = false);
+  void ConfigSD_PrintSettings(bool forReplay = false);
 #else
  FORCE_INLINE void Config_PrintSettings(bool forReplay=false) {}
+  FORCE_INLINE void ConfigSD_PrintSettings(bool forReplay = false) {}
 #endif
 #ifdef EEPROM_SETTINGS
@@ -21,4 +22,27 @@ void save_lifetime_stats();
  FORCE_INLINE void Config_RetrieveSettings() { Config_ResetDefault(); Config_PrintSettings(); }
 #endif
+#if defined(SDSUPPORT) && defined(SD_SETTINGS)
+  static const char *cfgSD_KEY[] = { //Keep this in lexicographical order for better search performance(O(Nlog2(N)) insted of O(N*N)) (if you don't keep this sorted, the algorithm for find the key index won't work, keep attention.)
+    #ifdef POWER_CONSUMPTION
+      "PWR",
+    #endif
+    "TME",
+  };
+  enum cfgSD_ENUM {	//This need to be in the same order as cfgSD_KEY
+    #ifdef POWER_CONSUMPTION
+      SD_CFG_PWR,
+    #endif
+    SD_CFG_TME,
+    SD_CFG_END //Leave this always as the last
+  };
+  void ConfigSD_StoreSettings();
+  void ConfigSD_RetrieveSettings(bool addValue = false);
+  int ConfigSD_KeyIndex(char *key);
+#else
+  FORCE_INLINE void ConfigSD_RetrieveSettings() {  ConfigSD_ResetDefault(); }
+#endif
 #endif //CONFIGURATION_STORE_H
--- a/MarlinKimbra/dogm_lcd_implementation.h
+++ b/MarlinKimbra/dogm_lcd_implementation.h
@@ -35,15 +35,11 @@
 #include "ultralcd_st7920_u8glib_rrd.h"
 #include "Configuration.h"
-// save 3120 bytes of PROGMEM by commenting out #define USE_BIG_EDIT_FONT
+#if !defined(MAPPER_C2C3) && !defined(MAPPER_NON) && defined(USE_BIG_EDIT_FONT)
-// we don't have a big font for Cyrillic, Kana
+   #undef USE_BIG_EDIT_FONT
-#if defined(MAPPER_C2C3) || defined(MAPPER_NON)
-  //#define USE_BIG_EDIT_FONT
 #endif
-// If you have spare 2300Byte of progmem and want to use a 
-// smaller font on the Info-screen uncomment the next line.
-//#define USE_SMALL_INFOFONT
 #ifdef USE_SMALL_INFOFONT
  #include "dogm_font_data_6x9_marlin.h"
  #define FONT_STATUSMENU_NAME u8g_font_6x9
@@ -126,6 +122,12 @@
 #elif defined(VIKI2) || defined(miniVIKI)
  // Mini Viki and Viki 2.0 LCD, ST7565 controller as well
  U8GLIB_NHD_C12864 u8g(DOGLCD_CS, DOGLCD_A0);
+#elif defined(U8GLIB_LM6059_AF)
+  // Based on the Adafruit ST7565 (http://www.adafruit.com/products/250)
+  U8GLIB_LM6059 u8g(DOGLCD_CS, DOGLCD_A0);
+#elif defined U8GLIB_SSD1306
+  // Generic support for SSD1306 OLED I2C LCDs
+  U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_NONE);
 #else
  // for regular DOGM128 display with HW-SPI
  U8GLIB_DOGM128 u8g(DOGLCD_CS, DOGLCD_A0);  // HW-SPI Com: CS, A0
@@ -273,7 +275,6 @@ static void lcd_implementation_status_screen() {
    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 - TALL_FONT_CORRECTION);
@@ -346,19 +347,28 @@ static void lcd_implementation_status_screen() {
  u8g.drawPixel(8,XYZ_BASELINE - 5);
  u8g.drawPixel(8,XYZ_BASELINE - 3);
  u8g.setPrintPos(10,XYZ_BASELINE);
+  if (axis_known_position[X_AXIS])
    lcd_print(ftostr31ns(current_position[X_AXIS]));
+  else
+    lcd_printPGM(PSTR("---"));
  u8g.setPrintPos(43,XYZ_BASELINE);
  lcd_print('Y');
  u8g.drawPixel(49,XYZ_BASELINE - 5);
  u8g.drawPixel(49,XYZ_BASELINE - 3);
  u8g.setPrintPos(51,XYZ_BASELINE);
+  if (axis_known_position[Y_AXIS])
    lcd_print(ftostr31ns(current_position[Y_AXIS]));
+  else
+    lcd_printPGM(PSTR("---"));
  u8g.setPrintPos(83,XYZ_BASELINE);
  lcd_print('Z');
  u8g.drawPixel(89,XYZ_BASELINE - 5);
  u8g.drawPixel(89,XYZ_BASELINE - 3);
  u8g.setPrintPos(91,XYZ_BASELINE);
-  lcd_print(ftostr31(current_position[Z_AXIS]));
+  if (axis_known_position[Z_AXIS])
+    lcd_print(ftostr32sp(current_position[Z_AXIS]));
+  else
+    lcd_printPGM(PSTR("---.--"));
  u8g.setColorIndex(1); // black on white
  // Feedrate

--- a/MarlinKimbra/language.h
+++ b/MarlinKimbra/language.h
@@ -106,18 +106,12 @@
 #define MSG_END_FILE_LIST                   "End file list"
 #define MSG_INVALID_EXTRUDER                "Invalid extruder"
 #define MSG_INVALID_SOLENOID                "Invalid solenoid"
-#define MSG_M104_INVALID_EXTRUDER           "M104 " MSG_INVALID_EXTRUDER " "
-#define MSG_M105_INVALID_EXTRUDER           "M105 " MSG_INVALID_EXTRUDER " "
-#define MSG_M109_INVALID_EXTRUDER           "M109 " MSG_INVALID_EXTRUDER " "
-#define MSG_M200_INVALID_EXTRUDER           "M200 " MSG_INVALID_EXTRUDER " "
-#define MSG_M218_INVALID_EXTRUDER           "M218 " MSG_INVALID_EXTRUDER " "
-#define MSG_M221_INVALID_EXTRUDER           "M221 " MSG_INVALID_EXTRUDER " "
 #define MSG_ERR_NO_THERMISTORS              "No thermistors - no temperature"
 #define MSG_HEATING                         "Heating..."
 #define MSG_HEATING_COMPLETE                "Heating done."
 #define MSG_BED_HEATING                     "Bed Heating."
 #define MSG_BED_DONE                        "Bed done."
-#define MSG_M115_REPORT                     "FIRMWARE_NAME:MarlinKimbra " BUILD_VERSION " FIRMWARE_URL:" FIRMWARE_URL " PROTOCOL_VERSION:" PROTOCOL_VERSION " MACHINE_TYPE:" MACHINE_NAME " EXTRUDER_COUNT:" STRINGIFY(EXTRUDERS) " UUID:" MACHINE_UUID "\n"
+#define MSG_M115_REPORT                     "FIRMWARE_NAME: MarlinKimbra " BUILD_VERSION " FIRMWARE_URL:" FIRMWARE_URL " PROTOCOL_VERSION:" PROTOCOL_VERSION " MACHINE_TYPE:" MACHINE_NAME " EXTRUDER_COUNT:" STRINGIFY(EXTRUDERS) " UUID:" MACHINE_UUID "\n"
 #define MSG_COUNT_X                         " Count X: "
 #define MSG_ERR_KILLED                      "Printer halted. kill() called!"
 #define MSG_ERR_STOPPED                     "Printer stopped due to errors. Fix the error and use M999 to restart. (Temperature is reset. Set it after restarting)"
@@ -134,14 +128,15 @@
 #define MSG_Z2_MAX                          "z2_max: "
 #define MSG_Z_PROBE                         "z_probe: "
 #define MSG_E_MIN                           "e_min: "
+#define MSG_ERR_MATERIAL_INDEX              "M145 S<index> out of range (0-2)"
+#define MSG_ERR_M428_TOO_FAR                "Too far from reference point"
 #define MSG_M119_REPORT                     "Reporting endstop status"
 #define MSG_ENDSTOP_HIT                     "TRIGGERED"
 #define MSG_ENDSTOP_OPEN                    "NOT TRIGGERED"
 #define MSG_HOTEND_OFFSET                   "Hotend offsets:"
 #define MSG_EMPTY_PLANE                     "Autolevel can only be execute on an actual plane, make sure width and height are not 0!"
-#define MSG_ERR_MATERIAL_INDEX              "M145 S<index> out of range (0-2)"
 #define MSG_FILRUNOUT_PIN                   "filament_runout_pin: "
-#define MSG_ERR_M428_TOO_FAR                "Too far from reference point"
 #define MSG_SD_CANT_OPEN_SUBDIR             "Cannot open subdir"
 #define MSG_SD_INIT_FAIL                    "SD init fail"
@@ -202,23 +197,27 @@
 #define MSG_KP                              " Kp: "
 #define MSG_KI                              " Ki: "
 #define MSG_KD                              " Kd: "
-#define MSG_B                               " B:"
+#define MSG_T                               "T:"
-#define MSG_T                               " T:"
+#define MSG_B                               "B:"
-#define MSG_AT                              " @:"
+#define MSG_AT                              "@:"
+#define MSG_BAT                             "B@:"
+#define MSG_W                               "W:"
 #define MSG_PID_AUTOTUNE_FINISHED           MSG_PID_AUTOTUNE " finished! Put the last Kp, Ki and Kd constants from above into Configuration.h or send command M500 for save in EEPROM the new value!"
+#define MSG_PID_DEBUG                       " PID_DEBUG "
-#define MSG_HEATING_FAILED                  "Heating failed"
+#define MSG_PID_DEBUG_INPUT                 ": Input "
-#define MSG_THERMAL_RUNAWAY_STOP            "Thermal Runaway, system stopped! Heater_ID: "
+#define MSG_PID_DEBUG_OUTPUT                " Output "
-#define MSG_THERMAL_RUNAWAY_BED             "bed"
+#define MSG_PID_DEBUG_PTERM                 " pTerm "
-#define MSG_TEMP_READ_ERROR                 "Temp measurement error!"
+#define MSG_PID_DEBUG_ITERM                 " iTerm "
-#define MSG_TEMP_BED                        "bed"
+#define MSG_PID_DEBUG_DTERM                 " dTerm "
-#define MSG_EXTRUDER_SWITCHED_OFF           "Extruder switched off. Temperature difference between temp sensors is too high !"
 #define MSG_INVALID_EXTRUDER_NUM            " - Invalid extruder number !"
-#define MSG_SWITCHED_OFF_MAX                " switched off. MAXTEMP triggered !!"
-#define MSG_MINTEMP_EXTRUDER_OFF            ": Extruder switched off. MINTEMP triggered !"
+#define MSG_HEATER_BED                      "bed"
-#define MSG_MAXTEMP_EXTRUDER_OFF            ": Extruder" MSG_SWITCHED_OFF_MAX
+#define MSG_STOPPED_HEATER                  ", system stopped! Heater_ID: "
-#define MSG_MAXTEMP_BED_OFF                 "Heated bed" MSG_SWITCHED_OFF_MAX
+#define MSG_REDUNDANCY                      "Heater switched off. Temperature difference between temp sensors is too high !"
+#define MSG_T_HEATING_FAILED                "Heating failed"
+#define MSG_T_THERMAL_RUNAWAY               "Thermal Runaway"
+#define MSG_T_MAXTEMP                       "MAXTEMP triggered"
+#define MSG_T_MINTEMP                       "MINTEMP triggered"
 #define MSG_ENDSTOP_XS                      "X"
 #define MSG_ENDSTOP_YS                      "Y"
@@ -226,9 +225,6 @@
 #define MSG_ENDSTOP_ZPS                     "ZP"
 #define MSG_ENDSTOP_ES                      "E"
-//watchdog.cpp
-#define MSG_WATCHDOG_RESET                  "Something is wrong, please turn off the printer."
 //other
 #define MSG_COMPILED                        "Compiled: "
 #define MSG_ERR_HOMING_DIV                  "The Homing Bump Feedrate Divisor cannot be less than 1"

--- a/MarlinKimbra/language_en.h
+++ b/MarlinKimbra/language_en.h
@@ -24,14 +24,15 @@
 #define MSG_AUTOSTART                       "Autostart"
 #define MSG_DISABLE_STEPPERS                "Disable steppers"
 #define MSG_AUTO_HOME                       "Auto home"
-#define MSG_BED_SETTING                     "Bed Setting"
+#define MSG_MBL_SETTING                     "Manual Bed Leveling"
-#define MSG_LP_INTRO                        " Leveling bed...       Press to start  "
+#define MSG_MBL_BUTTON                      " Press the button   "
-#define MSG_LP_1                            " Adjust first point  & Press the button"
+#define MSG_MBL_INTRO                       " Leveling bed...    "
-#define MSG_LP_2                            " Adjust second point & Press the button"
+#define MSG_MBL_1                           " Adjust first point "
-#define MSG_LP_3                            " Adjust third point  & Press the button"
+#define MSG_MBL_2                           " Adjust second point"
-#define MSG_LP_4                            " Adjust fourth point & Press the button"
+#define MSG_MBL_3                           " Adjust third point "
-#define MSG_LP_5                            "     Is it ok?         Press to end"       
+#define MSG_MBL_4                           " Adjust fourth point"
-#define MSG_LP_6                            " BED leveled!"
+#define MSG_MBL_5                           "    Is it ok?       "
+#define MSG_MBL_6                           " BED leveled!       "
 #define MSG_SET_HOME_OFFSETS                "Set home offsets"
 #define MSG_SET_ORIGIN                      "Set origin"
 #define MSG_PREHEAT_PLA                     "Preheat PLA"
@@ -143,20 +144,24 @@
 #define MSG_BABYSTEP_Y                      "Babystep Y"
 #define MSG_BABYSTEP_Z                      "Babystep Z"
 #define MSG_ENDSTOP_ABORT                   "Endstop abort"
-#define MSG_END_HOUR                        "hours"
-#define MSG_END_MINUTE                      "minutes"
 #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_ERR_MINTEMP_BED                 "Err: MINTEMP BED"
+#define MSG_END_DAY                         "days"
+#define MSG_END_HOUR                        "hours"
+#define MSG_END_MINUTE                      "minutes"
+// Debug
 #define MSG_DEBUG_ECHO                      "DEBUG ECHO ENABLED"
 #define MSG_DEBUG_INFO                      "DEBUG INFO ENABLED"
 #define MSG_DEBUG_ERRORS                    "DEBUG ERRORS ENABLED"
 #define MSG_DEBUG_DRYRUN                    "DEBUG DRYRUN ENABLED"
+// Calibrate Delta
 #ifdef DELTA
  #define MSG_DELTA_CALIBRATE               "Delta Calibration"
  #define MSG_DELTA_CALIBRATE_X             "Calibrate X"
@@ -165,6 +170,7 @@
  #define MSG_DELTA_CALIBRATE_CENTER        "Calibrate Center"
 #endif // DELTA
+// Scara
 #ifdef SCARA
  #define MSG_XSCALE                        "X Scale"
  #define MSG_YSCALE                        "Y Scale"

--- a/MarlinKimbra/language_it.h
+++ b/MarlinKimbra/language_it.h
@@ -24,14 +24,15 @@
 #define MSG_AUTOSTART                       "Autostart"
 #define MSG_DISABLE_STEPPERS                "Disabilita Motori"
 #define MSG_AUTO_HOME                       "Auto Home"
-#define MSG_BED_SETTING                     "Bed Setting"
+#define MSG_MBL_SETTING                     "Manual Bed Leveling "
-#define MSG_LP_INTRO                        " Leveling bed...       Press to start  "
+#define MSG_MBL_BUTTON                      " Press the button   "
-#define MSG_LP_1                            " Adjust first point  & Press the button"
+#define MSG_MBL_INTRO                       " Leveling bed...    "
-#define MSG_LP_2                            " Adjust second point & Press the button"
+#define MSG_MBL_1                           " Adjust first point "
-#define MSG_LP_3                            " Adjust third point  & Press the button"
+#define MSG_MBL_2                           " Adjust second point"
-#define MSG_LP_4                            " Adjust fourth point & Press the button"
+#define MSG_MBL_3                           " Adjust third point "
-#define MSG_LP_5                            "     Is it ok?         Press to end"       
+#define MSG_MBL_4                           " Adjust fourth point"
-#define MSG_LP_6                            " BED leveled!"
+#define MSG_MBL_5                           "    Is it ok?       "
+#define MSG_MBL_6                           " BED leveled!       "
 #define MSG_SET_HOME_OFFSETS                "Setta offset home"
 #define MSG_SET_ORIGIN                      "Imposta Origine"
 #define MSG_PREHEAT_PLA                     "Preriscalda PLA"
@@ -79,18 +80,18 @@
 #define MSG_E2                              " E2"
 #define MSG_E3                              " E3"
 #define MSG_E4                              " E4"
-#define MSG_ACC                             "Accel."
+#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_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_AMAX                            "Amax "
 #define MSG_A_RETRACT                       "A-retract"
 #define MSG_A_TRAVEL                        "A-travel"
 #define MSG_XSTEPS                          "X steps/mm"
@@ -103,7 +104,7 @@
 #define MSG_TEMPERATURE                     "Temperatura"
 #define MSG_MOTION                          "Movimento"
 #define MSG_VOLUMETRIC                      "Filamento"
-#define MSG_VOLUMETRIC_ENABLED              "E in mm³"
+#define MSG_VOLUMETRIC_ENABLED              "E in mm3"
 #define MSG_FILAMENT_SIZE_EXTRUDER          "Diam. filo"
 #define MSG_CONTRAST                        "Contrasto LCD"
 #define MSG_STORE_EPROM                     "Salva in EEPROM"
@@ -130,7 +131,7 @@
 #define MSG_CONTROL_RETRACTF                "Ritrai  V"
 #define MSG_CONTROL_RETRACT_ZLIFT           "Salta mm"
 #define MSG_CONTROL_RETRACT_RECOVER         "UnRet +mm"
-#define MSG_CONTROL_RETRACT_RECOVER_SWAP    "Scamb. UnRet +mm"
+#define MSG_CONTROL_RETRACT_RECOVER_SWAP    "S UnRet+mm"
 #define MSG_CONTROL_RETRACT_RECOVERF        "UnRet  V"
 #define MSG_AUTORETRACT                     "AutoArretramento"
 #define MSG_FILAMENTCHANGE                  "Cambia filamento"
@@ -143,20 +144,24 @@
 #define MSG_BABYSTEP_Y                      "Babystep Y"
 #define MSG_BABYSTEP_Z                      "Babystep Z"
 #define MSG_ENDSTOP_ABORT                   "Finecorsa abort"
-#define MSG_END_HOUR                        "ore"
-#define MSG_END_MINUTE                      "minuti"
 #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_ERR_MINTEMP_BED                 "Err: MINTEMP BED"
+#define MSG_END_DAY                         "giorni"
+#define MSG_END_HOUR                        "ore"
+#define MSG_END_MINUTE                      "minuti"
+// Debug
 #define MSG_DEBUG_ECHO                      "DEBUG RIPETI"
 #define MSG_DEBUG_INFO                      "DEBUG INFO"
 #define MSG_DEBUG_ERRORS                    "DEBUG ERRORI"
 #define MSG_DEBUG_DRYRUN                    "DEBUG STAMPA A VUOTO"
+// Calibrate Delta
 #ifdef DELTA
  #define MSG_DELTA_CALIBRATE               "Calibraz. Delta"
  #define MSG_DELTA_CALIBRATE_X             "Calibra X"
@@ -165,6 +170,7 @@
  #define MSG_DELTA_CALIBRATE_CENTER        "Calibra Centro"
 #endif // DELTA
+// Scara
 #ifdef SCARA
  #define MSG_XSCALE                        "X Scale"
  #define MSG_YSCALE                        "Y Scale"

--- a/MarlinKimbra/motion_control.cpp
+++ b/MarlinKimbra/motion_control.cpp
-/*
-  motion_control.c - high level interface for issuing motion commands
-  Part of Grbl
-  Copyright (c) 2009-2011 Simen Svale Skogsrud
-  Copyright (c) 2011 Sungeun K. Jeon
-  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/>.
-*/
-#include "Marlin.h"
-#include "stepper.h"
-#include "planner.h"
-// The arc is approximated by generating a huge number of tiny, linear segments. The length of each 
-// segment is configured in settings.mm_per_arc_segment.  
-void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8_t axis_1, 
-  uint8_t axis_linear, float feed_rate, float radius, uint8_t isclockwise, uint8_t extruder, uint8_t driver)
-{      
-  //   int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled();
-  //   plan_set_acceleration_manager_enabled(false); // disable acceleration management for the duration of the arc
-  float center_axis0 = position[axis_0] + offset[axis_0];
-  float center_axis1 = position[axis_1] + offset[axis_1];
-  float linear_travel = target[axis_linear] - position[axis_linear];
-  float extruder_travel = target[E_AXIS] - position[E_AXIS];
-  float r_axis0 = -offset[axis_0];  // Radius vector from center to current location
-  float r_axis1 = -offset[axis_1];
-  float rt_axis0 = target[axis_0] - center_axis0;
-  float rt_axis1 = target[axis_1] - center_axis1;
-  // CCW angle between position and target from circle center. Only one atan2() trig computation required.
-  float angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
-  if (angular_travel < 0) { angular_travel += 2*M_PI; }
-  if (isclockwise) { angular_travel -= 2*M_PI; }
-  //20141002:full circle for G03 did not work, e.g. G03 X80 Y80 I20 J0 F2000 is giving an Angle of zero so head is not moving
-  //to compensate when start pos = target pos && angle is zero -> angle = 2Pi
-  if (position[axis_0] == target[axis_0] && position[axis_1] == target[axis_1] && angular_travel == 0)
-  {
-	  angular_travel += 2*M_PI;
-  }
-  //end fix G03
-  float millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));
-  if (millimeters_of_travel < 0.001) { return; }
-  uint16_t segments = floor(millimeters_of_travel/MM_PER_ARC_SEGMENT);
-  if(segments == 0) segments = 1;
-  /*  
-    // Multiply inverse feed_rate to compensate for the fact that this movement is approximated
-    // by a number of discrete segments. The inverse feed_rate should be correct for the sum of 
-    // all segments.
-    if (invert_feed_rate) { feed_rate *= segments; }
-  */
-  float theta_per_segment = angular_travel/segments;
-  float linear_per_segment = linear_travel/segments;
-  float extruder_per_segment = extruder_travel/segments;
-  /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
-     and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
-         r_T = [cos(phi) -sin(phi);
-                sin(phi)  cos(phi] * r ;
-     For arc generation, the center of the circle is the axis of rotation and the radius vector is 
-     defined from the circle center to the initial position. Each line segment is formed by successive
-     vector rotations. This requires only two cos() and sin() computations to form the rotation
-     matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
-     all double numbers are single precision on the Arduino. (True double precision will not have
-     round off issues for CNC applications.) Single precision error can accumulate to be greater than
-     tool precision in some cases. Therefore, arc path correction is implemented. 
-     Small angle approximation may be used to reduce computation overhead further. This approximation
-     holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
-     theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
-     to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for 
-     numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
-     issue for CNC machines with the single precision Arduino calculations.
-     This approximation also allows mc_arc to immediately insert a line segment into the planner 
-     without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
-     a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead. 
-     This is important when there are successive arc motions. 
-  */
-  // Vector rotation matrix values
-  float cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation
-  float sin_T = theta_per_segment;
-  float arc_target[4];
-  float sin_Ti;
-  float cos_Ti;
-  float r_axisi;
-  uint16_t i;
-  int8_t count = 0;
-  // Initialize the linear axis
-  arc_target[axis_linear] = position[axis_linear];
-  // Initialize the extruder axis
-  arc_target[E_AXIS] = position[E_AXIS];
-  for (i = 1; i<segments; i++) { // Increment (segments-1)
-    if (count < N_ARC_CORRECTION) {
-      // Apply vector rotation matrix 
-      r_axisi = r_axis0*sin_T + r_axis1*cos_T;
-      r_axis0 = r_axis0*cos_T - r_axis1*sin_T;
-      r_axis1 = r_axisi;
-      count++;
-    } else {
-      // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
-      // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
-      cos_Ti = cos(i*theta_per_segment);
-      sin_Ti = sin(i*theta_per_segment);
-      r_axis0 = -offset[axis_0]*cos_Ti + offset[axis_1]*sin_Ti;
-      r_axis1 = -offset[axis_0]*sin_Ti - offset[axis_1]*cos_Ti;
-      count = 0;
-    }
-    // Update arc_target location
-    arc_target[axis_0] = center_axis0 + r_axis0;
-    arc_target[axis_1] = center_axis1 + r_axis1;
-    arc_target[axis_linear] += linear_per_segment;
-    arc_target[E_AXIS] += extruder_per_segment;
-    clamp_to_software_endstops(arc_target);
-    plan_buffer_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], arc_target[E_AXIS], feed_rate, extruder, driver);
-  }
-  // Ensure last segment arrives at target location.
-  plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], feed_rate, extruder, driver);
-  //   plan_set_acceleration_manager_enabled(acceleration_manager_was_enabled);
-}
--- a/MarlinKimbra/motion_control.h
+++ b/MarlinKimbra/motion_control.h
-/*
-  motion_control.h - high level interface for issuing motion commands
-  Part of Grbl
-  Copyright (c) 2009-2011 Simen Svale Skogsrud
-  Copyright (c) 2011 Sungeun K. Jeon
-  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/>.
-*/
-#ifndef motion_control_h
-#define motion_control_h
-// Execute an arc in offset mode format. position == current xyz, target == target xyz, 
-// offset == offset from current xyz, axis_XXX defines circle plane in tool space, axis_linear is
-// the direction of helical travel, radius == circle radius, isclockwise boolean. Used
-// for vector transformation direction.
-void mc_arc(float *position, float *target, float *offset, unsigned char axis_0, unsigned char axis_1,
-  unsigned char axis_linear, float feed_rate, float radius, unsigned char isclockwise, uint8_t extruder, uint8_t driver);
-#endif
--- a/MarlinKimbra/pins.h
+++ b/MarlinKimbra/pins.h
+/**
+ * pins.h
+ */
 /****************************************************************************************
 * 10 BOARD_GEN7_CUSTOM - Gen7 custom (Alfons3 Version)
 * 11 BOARD_GEN7_12 - Gen7 v1.1, v1.2
@@ -69,7 +73,7 @@
 #include "boards.h"
-/// Preset optional pins
+// Preset optional pins
 #define X_MS1_PIN     -1
 #define X_MS2_PIN     -1
 #define Y_MS1_PIN     -1
@@ -82,6 +86,8 @@
 #define E1_MS2_PIN    -1
 #define DIGIPOTSS_PIN -1
 #define LCD_CONTRAST  -1
+#define Z2_MIN_PIN    -1
+#define Z2_MAX_PIN    -1
 /******************************************************************************
@@ -966,6 +972,17 @@
        #define BTN_ENC       -1
        #define LCD_SDSS      53
        #define SDCARDDETECT  49
+      #elif defined(ELB_FULL_GRAPHIC_CONTROLLER)
+        #define BTN_EN1       35  // reverse if the encoder turns the wrong way.
+        #define BTN_EN2       37
+        #define BTN_ENC       31
+        #define SDCARDDETECT  49
+        #define LCD_SDSS      53
+        #define KILL_PIN      41
+        #define BEEPER        23
+        #define DOGLCD_CS     29
+        #define DOGLCD_A0     27
+        #define LCD_PIN_BL    33
      #else
        //arduino pin which triggers an piezzo beeper
        #define BEEPER        33  // Beeper on AUX-4
@@ -1032,7 +1049,6 @@
 /****************************************************************************************
 * 34
 * RAMPS 1.3 / 1.4
@@ -1159,6 +1175,17 @@
        #define BTN_ENC       -1
        #define LCD_SDSS      53
        #define SDCARDDETECT  49
+      #elif defined(ELB_FULL_GRAPHIC_CONTROLLER)
+        #define BTN_EN1       35  // reverse if the encoder turns the wrong way.
+        #define BTN_EN2       37
+        #define BTN_ENC       31
+        #define SDCARDDETECT  49
+        #define LCD_SDSS      53
+        #define KILL_PIN      41
+        #define BEEPER        23
+        #define DOGLCD_CS     29
+        #define DOGLCD_A0     27
+        #define LCD_PIN_BL    33
      #else
        //arduino pin which triggers an piezzo beeper
        #define BEEPER        33  // Beeper on AUX-4
@@ -1225,7 +1252,6 @@
 /****************************************************************************************
 * 35
 * RAMPS 1.3 / 1.4
@@ -1352,6 +1378,17 @@
        #define BTN_ENC         -1
        #define LCD_SDSS        53
        #define SDCARDDETECT    49
+      #elif defined(ELB_FULL_GRAPHIC_CONTROLLER)
+        #define BTN_EN1       35  // reverse if the encoder turns the wrong way.
+        #define BTN_EN2       37
+        #define BTN_ENC       31
+        #define SDCARDDETECT  49
+        #define LCD_SDSS      53
+        #define KILL_PIN      41
+        #define BEEPER        23
+        #define DOGLCD_CS     29
+        #define DOGLCD_A0     27
+        #define LCD_PIN_BL    33
      #else
        //arduino pin which triggers an piezzo beeper
        #define BEEPER        33  // Beeper on AUX-4
@@ -1418,7 +1455,6 @@
 /****************************************************************************************
 * 36
 * RAMPS 1.3 / 1.4
@@ -1545,6 +1581,17 @@
        #define BTN_ENC       -1
        #define LCD_SDSS      53
        #define SDCARDDETECT  49
+      #elif defined(ELB_FULL_GRAPHIC_CONTROLLER)
+        #define BTN_EN1       35  // reverse if the encoder turns the wrong way.
+        #define BTN_EN2       37
+        #define BTN_ENC       31
+        #define SDCARDDETECT  49
+        #define LCD_SDSS      53
+        #define KILL_PIN      41
+        #define BEEPER        23
+        #define DOGLCD_CS     29
+        #define DOGLCD_A0     27
+        #define LCD_PIN_BL    33
      #else
        //arduino pin which triggers an piezzo beeper
        #define BEEPER        33  // Beeper on AUX-4
@@ -1955,19 +2002,19 @@
 #define ORIG_X_DIR_PIN          62
 #define ORIG_X_ENABLE_PIN       48
 #define X_MIN_PIN               22
-#define X_MAX_PIN                2
+#define X_MAX_PIN               30
 #define ORIG_Y_STEP_PIN         65
 #define ORIG_Y_DIR_PIN          64
 #define ORIG_Y_ENABLE_PIN       46
 #define Y_MIN_PIN               24
-#define Y_MAX_PIN               15
+#define Y_MAX_PIN               38
 #define ORIG_Z_STEP_PIN         67
 #define ORIG_Z_DIR_PIN          66
 #define ORIG_Z_ENABLE_PIN       44
 #define Z_MIN_PIN               26
-#define Z_MAX_PIN               -1
+#define Z_MAX_PIN               34
 #define ORIG_E0_STEP_PIN        36
 #define ORIG_E0_DIR_PIN         28
@@ -2002,11 +2049,11 @@
 #define HEATER_1_PIN            10
 #define HEATER_2_PIN            11
-#define TEMP_BED_PIN             0   // ANALOG NUMBERING
+#define TEMP_BED_PIN             7   // ANALOG NUMBERING
-#define TEMP_0_PIN               1   // ANALOG NUMBERING
+#define TEMP_0_PIN               6   // ANALOG NUMBERING
-#define TEMP_1_PIN              -1   // 2    // ANALOG NUMBERING
+#define TEMP_1_PIN               5   // 2    // ANALOG NUMBERING
-#define TEMP_2_PIN              -1   // 3     // ANALOG NUMBERING
+#define TEMP_2_PIN               4   // 3     // ANALOG NUMBERING
 #define TEMP_3_PIN              -1   // ANALOG NUMBERING
 #define TEMP_4_PIN              -1   // ANALOG NUMBERING
@@ -2334,6 +2381,21 @@
 #endif //REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER
+  #ifdef NUM_SERVOS
+    #define SERVO0_PIN        36
+    #if NUM_SERVOS > 1
+      #define SERVO1_PIN      40
+    #endif
+    #if NUM_SERVOS > 2
+      #define SERVO2_PIN      41
+    #endif
+    #if NUM_SERVOS > 3
+      #define SERVO3_PIN      -1
+    #endif
+  #endif
 #endif //ALLIGATOR
 /****************************************************************************************/
@@ -3401,6 +3463,10 @@
  #define ORIG_E2_DIR_PIN         60
  #define ORIG_E2_ENABLE_PIN      23
+  #define ORIG_E3_STEP_PIN        54
+  #define ORIG_E3_DIR_PIN         55
+  #define ORIG_E3_ENABLE_PI       55
  #define SDPOWER -1
  #define SDSS 53
  #define LED_PIN 13

--- a/MarlinKimbra/pins2tool.h
+++ b/MarlinKimbra/pins2tool.h
@@ -44,7 +44,9 @@
 //FAN pin
 #define FAN_PIN       ORIG_FAN_PIN
 //============================================================================
 //============================================================================
--- a/MarlinKimbra/planner.cpp
+++ b/MarlinKimbra/planner.cpp
@@ -61,7 +61,6 @@
 millis_t minsegmenttime;
 float max_feedrate[3 + EXTRUDERS]; // Max speeds in mm per minute
-float max_retraction_feedrate[EXTRUDERS]; // set the max speeds for retraction
 float axis_steps_per_unit[3 + EXTRUDERS];
 unsigned long max_acceleration_units_per_sq_second[3 + EXTRUDERS]; // Use M201 to override by software
 float minimumfeedrate;
@@ -432,11 +431,12 @@ void check_axes_activity() {
    #ifdef FAN_KICKSTART_TIME
      static millis_t fan_kick_end;
      if (tail_fan_speed) {
+        millis_t ms = millis();
        if (fan_kick_end == 0) {
          // Just starting up fan - run at full power.
-          fan_kick_end = millis() + FAN_KICKSTART_TIME;
+          fan_kick_end = ms + FAN_KICKSTART_TIME;
          tail_fan_speed = 255;
-        } else if (fan_kick_end > millis())
+        } else if (fan_kick_end > ms)
          // Fan still spinning up.
          tail_fan_speed = 255;
        } else {
@@ -478,18 +478,14 @@ float junction_deviation = 0.1;
  void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder, const uint8_t &driver)
 #else
  void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder, const uint8_t &driver)
-#endif  //ENABLE_AUTO_BED_LEVELING
+#endif  // ENABLE_AUTO_BED_LEVELING
 {
  // Calculate the buffer head after we push this byte
  int next_buffer_head = next_block_index(block_buffer_head);
  // If the buffer is full: good! That means we are well ahead of the robot. 
  // Rest here until there is room in the buffer.
-  while(block_buffer_tail == next_buffer_head) {
+  while (block_buffer_tail == next_buffer_head) idle();
-    manage_heater(); 
-    manage_inactivity(); 
-    lcd_update();
-  }
  #ifdef ENABLE_AUTO_BED_LEVELING
    apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
@@ -513,13 +509,12 @@ float junction_deviation = 0.1;
    if (de) {
      #ifdef NPR2
        if (extruder != 1)
-      #endif // NPR2
+      #endif
        {
          if (degHotend(extruder) < extrude_min_temp && !(debugLevel & DEBUG_DRYRUN)) {
            position[E_AXIS] = target[E_AXIS]; //behave as if the move really took place, but ignore E part
            de = 0; // no difference
-            ECHO_S(OK);
+            ECHO_LM(ER, MSG_ERR_COLD_EXTRUDE_STOP);
-            ECHO_EM(MSG_ERR_COLD_EXTRUDE_STOP);
          }
        }
@@ -530,8 +525,7 @@ float junction_deviation = 0.1;
          #endif
          position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
          de = 0; // no difference
-          ECHO_S(OK);
+          ECHO_LM(ER, MSG_ERR_LONG_EXTRUDE_STOP);
-          ECHO_EM(MSG_ERR_LONG_EXTRUDE_STOP);
          #ifdef EASY_LOAD
            }
            allow_lengthy_extrude_once = false;
@@ -693,7 +687,6 @@ float junction_deviation = 0.1;
        break;
      }
    #endif //!MKR4 && !NPR2
-    if (feed_rate < minimumfeedrate) feed_rate = minimumfeedrate;
  }
  if (block->steps[E_AXIS])
@@ -796,24 +789,12 @@ float junction_deviation = 0.1;
  // Calculate and limit speed in mm/sec for each axis
  float current_speed[NUM_AXIS];
  float speed_factor = 1.0; //factor <=1 do decrease speed
-  for (int i = 0; i < 3; i++) {
+  for (int i = 0; i < NUM_AXIS; i++) {
    current_speed[i] = delta_mm[i] * inverse_second;
    float cs = fabs(current_speed[i]), mf = max_feedrate[i];
    if (cs > mf) speed_factor = min(speed_factor, mf / cs);
  }
-  current_speed[E_AXIS] = delta_mm[E_AXIS] * inverse_second;
-  if (target[E_AXIS] < position[E_AXIS])
-  {
-    if(fabs(current_speed[E_AXIS]) > max_retraction_feedrate[extruder])
-      speed_factor = min(speed_factor, max_retraction_feedrate[extruder]/ fabs(current_speed[E_AXIS]));
-  }
-  else
-  {
-    if(fabs(current_speed[E_AXIS]) > max_feedrate[E_AXIS + extruder])
-      speed_factor = min(speed_factor, max_feedrate[E_AXIS + extruder] / fabs(current_speed[E_AXIS]));
-  }
  // Max segement time in us.
  #ifdef XY_FREQUENCY_LIMIT
    #define MAX_FREQ_TIME (1000000.0 / XY_FREQUENCY_LIMIT)
@@ -1028,11 +1009,14 @@ float junction_deviation = 0.1;
 #endif // ENABLE_AUTO_BED_LEVELING
 #ifdef ENABLE_AUTO_BED_LEVELING
-  void plan_set_position(float x, float y, float z, const float &e) {
+  void plan_set_position(float x, float y, float z, const float &e)
-    apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
 #else
-  void plan_set_position(const float &x, const float &y, const float &z, const float &e) {
+  void plan_set_position(const float &x, const float &y, const float &z, const float &e)
 #endif // ENABLE_AUTO_BED_LEVELING
+  {
+    #ifdef ENABLE_AUTO_BED_LEVELING
+      apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
+    #endif
    float nx = position[X_AXIS] = lround(x * axis_steps_per_unit[X_AXIS]),
          ny = position[Y_AXIS] = lround(y * axis_steps_per_unit[Y_AXIS]),

--- a/MarlinKimbra/planner.h
+++ b/MarlinKimbra/planner.h
@@ -114,9 +114,12 @@ FORCE_INLINE uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block
 void plan_set_e_position(const float &e);
+//===========================================================================
+//============================= public variables ============================
+//===========================================================================
 extern millis_t minsegmenttime;
 extern float max_feedrate[3 + EXTRUDERS]; // set the max speeds
-extern float max_retraction_feedrate[EXTRUDERS]; // set the max speeds for retraction
 extern float axis_steps_per_unit[3 + EXTRUDERS];
 extern unsigned long max_acceleration_units_per_sq_second[3 + EXTRUDERS]; // Use M201 to override by software
 extern float minimumfeedrate;

--- a/MarlinKimbra/sanitycheck.h
+++ b/MarlinKimbra/sanitycheck.h
@@ -124,7 +124,7 @@
     * Require a Z Min pin
     */
    #if Z_MIN_PIN == -1
-      #if Z_PROBE_PIN == -1 || (!defined(Z_PROBE_ENDSTOP) || defined(DISABLE_Z_PROBE_ENDSTOP)) // It's possible for someone to set a pin for the Z Probe, but not enable it.
+      #if Z_PROBE_PIN == -1 || defined(Z_PROBE_ENDSTOP) // It's possible for someone to set a pin for the Z Probe, but not enable it.
        #ifdef Z_PROBE_REPEATABILITY_TEST
          #error You must have a Z_MIN or Z_PROBE endstop to enable Z_PROBE_REPEATABILITY_TEST.
        #else
@@ -236,20 +236,13 @@
   */
  #if defined(DELTA) && defined(Z_PROBE_ENDSTOP)
    #ifndef Z_PROBE_PIN
-      #error You must have a Z_PROBE_PIN defined in your pins_XXXX.h file if you enable Z_PROBE_ENDSTOP
+      #error You must have a Z_PROBE_PIN defined in your pins2tool.h file if you enable Z_PROBE_ENDSTOP
    #endif
    #if Z_PROBE_PIN == -1
      #error You must set Z_PROBE_PIN to a valid pin if you enable Z_PROBE_ENDSTOP
    #endif
  #endif
-  /**
-   * Allen Key Z Probe requires Auto Bed Leveling grid and Delta
-   */
-  #if defined(Z_PROBE_ALLEN_KEY) && !(defined(AUTO_BED_LEVELING_GRID) && defined(DELTA))
-    #error Invalid use of Z_PROBE_ALLEN_KEY.
-  #endif
  /**
   * Dual X Carriage requirements
   */
@@ -304,4 +297,23 @@
    #error HEATER_0_PIN not defined for this board
  #endif
+  /**
+   * Warnings for old configurations
+   */
+  #ifdef X_HOME_RETRACT_MM
+    #error [XYZ]_HOME_RETRACT_MM settings have been renamed [XYZ]_HOME_BUMP_MM
+  #endif
+  #if WATCH_TEMP_PERIOD > 500
+    #error WATCH_TEMP_PERIOD now uses seconds instead of milliseconds
+  #endif
+  #if !defined(THERMAL_PROTECTION_HOTENDS) && (defined(WATCH_TEMP_PERIOD) || defined(THERMAL_PROTECTION_PERIOD))
+    #error Thermal Runaway Protection for hotends must now be enabled with THERMAL_PROTECTION_HOTENDS
+  #endif
+  #if !defined(THERMAL_PROTECTION_BED) && defined(THERMAL_PROTECTION_BED_PERIOD)
+    #error Thermal Runaway Protection for the bed must now be enabled with THERMAL_PROTECTION_BED
+  #endif
 #endif //SANITYCHECK_H
--- a/MarlinKimbra/servo.h
+++ b/MarlinKimbra/servo.h
 /*
-  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

--- a/MarlinKimbra/stepper.cpp
+++ b/MarlinKimbra/stepper.cpp
-/*
+/**
-  stepper.c - stepper motor driver: executes motion plans using stepper motors
+ * stepper.cpp - stepper motor driver: executes motion plans using stepper motors
-  Part of Grbl
+ * Marlin Firmware
+ *
-  Copyright (c) 2009-2011 Simen Svale Skogsrud
+ * Derived from 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
+ * Grbl is free software: you can redistribute it and/or modify
-  the Free Software Foundation, either version 3 of the License, or
+ * it under the terms of the GNU General Public License as published by
-  (at your option) any later version.
+ * 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
+ * Grbl is distributed in the hope that it will be useful,
-  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
-  GNU General Public License for more details.
+ * 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/>.
+ * You should have received a copy of the GNU General Public License
-*/
+ * along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
+ */
 /* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith
   and Philipp Tiefenbacher. */
@@ -45,7 +46,7 @@ block_t *current_block;  // A pointer to the block currently being traced
 //static makes it impossible to be called from outside of this file by extern.!
 // Variables used by The Stepper Driver Interrupt
-static unsigned char out_bits;        // The next stepping-bits to be output
+static unsigned char out_bits = 0;        // The next stepping-bits to be output
 static unsigned int cleaning_buffer_counter;
 #ifdef Z_DUAL_ENDSTOPS
@@ -73,14 +74,12 @@ static unsigned short step_loops_nominal;
 volatile long endstops_trigsteps[3] = { 0 };
 volatile long endstops_stepsTotal, endstops_stepsDone;
-static volatile bool endstop_x_hit = false;
+static volatile char endstop_hit_bits = 0; // use X_MIN, Y_MIN, Z_MIN and Z_PROBE as BIT value
-static volatile bool endstop_y_hit = false;
-static volatile bool endstop_z_hit = false;
-static volatile bool endstop_z_probe_hit = false;
-#ifdef NPR2
+#ifndef Z_DUAL_ENDSTOPS
-  static volatile bool endstop_e_hit = false;
+  static byte old_endstop_bits = 0; // use X_MIN, X_MAX... Z_MAX, Z_PROBE, Z2_MIN, Z2_MAX
-  static bool old_e_min_endstop = false;
+#else
+  static uint16_t old_endstop_bits = 0; // use X_MIN, X_MAX... Z_MAX, Z_PROBE, Z2_MIN, Z2_MAX
 #endif
 #ifdef ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED
@@ -91,31 +90,6 @@ static volatile bool endstop_z_probe_hit = false;
  int motor_current_setting[3] = DEFAULT_PWM_MOTOR_CURRENT;
 #endif
-#if HAS_X_MIN
-  static bool old_x_min_endstop = false;
-#endif
-#if HAS_X_MAX
-  static bool old_x_max_endstop = false;
-#endif
-#if HAS_Y_MIN
-  static bool old_y_min_endstop = false;
-#endif
-#if HAS_Y_MAX
-  static bool old_y_max_endstop = false;
-#endif
-static bool old_z_min_endstop = false;
-static bool old_z_max_endstop = false;
-#ifdef Z_DUAL_ENDSTOPS
-  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.
-  static bool old_z_probe_endstop = false;
-#endif
 static bool check_endstops = true;
 volatile long count_position[NUM_AXIS] = { 0 };
@@ -162,11 +136,11 @@ volatile signed char count_direction[NUM_AXIS] = { 1, 1, 1, 1 };
    #define Z_APPLY_STEP(v,Q) \
    if (performing_homing) { \
      if (Z_HOME_DIR > 0) {\
-        if (!(old_z_max_endstop && (count_direction[Z_AXIS] > 0)) && !locked_z_motor) Z_STEP_WRITE(v); \
+        if (!(TEST(old_endstop_bits, Z_MAX) && (count_direction[Z_AXIS] > 0)) && !locked_z_motor) Z_STEP_WRITE(v); \
-        if (!(old_z2_max_endstop && (count_direction[Z_AXIS] > 0)) && !locked_z2_motor) Z2_STEP_WRITE(v); \
+        if (!(TEST(old_endstop_bits, Z2_MAX) && (count_direction[Z_AXIS] > 0)) && !locked_z2_motor) Z2_STEP_WRITE(v); \
      } else {\
-        if (!(old_z_min_endstop && (count_direction[Z_AXIS] < 0)) && !locked_z_motor) Z_STEP_WRITE(v); \
+        if (!(TEST(old_endstop_bits, Z_MIN) && (count_direction[Z_AXIS] < 0)) && !locked_z_motor) Z_STEP_WRITE(v); \
-        if (!(old_z2_min_endstop && (count_direction[Z_AXIS] < 0)) && !locked_z2_motor) Z2_STEP_WRITE(v); \
+        if (!(TEST(old_endstop_bits, Z2_MIN) && (count_direction[Z_AXIS] < 0)) && !locked_z2_motor) Z2_STEP_WRITE(v); \
      } \
    } else { \
      Z_STEP_WRITE(v); \
@@ -268,81 +242,36 @@ volatile signed char count_direction[NUM_AXIS] = { 1, 1, 1, 1 };
 #define ENABLE_STEPPER_DRIVER_INTERRUPT()  TIMSK1 |= BIT(OCIE1A)
 #define DISABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 &= ~BIT(OCIE1A)
-#ifdef NPR2
 void endstops_hit_on_purpose() {
-  endstop_x_hit = endstop_y_hit = endstop_z_hit = endstop_z_probe_hit = endstop_e_hit = false;
+  endstop_hit_bits = 0;
 }
 void checkHitEndstops() {
-  if (endstop_x_hit || endstop_y_hit || endstop_z_hit || endstop_z_probe_hit || endstop_e_hit) {
+  if (endstop_hit_bits) {
-    ECHO_SM(OK, MSG_ENDSTOPS_HIT);
+    ECHO_SM(DB, MSG_ENDSTOPS_HIT);
-    if(endstop_x_hit) {
+    if (endstop_hit_bits & BIT(X_MIN)) {
      ECHO_MV(MSG_ENDSTOP_X, (float)endstops_trigsteps[X_AXIS] / axis_steps_per_unit[X_AXIS]);
      LCD_MESSAGEPGM(MSG_ENDSTOPS_HIT MSG_ENDSTOP_XS);
    }
-    if(endstop_y_hit) {
+    if (endstop_hit_bits & BIT(Y_MIN)) {
      ECHO_MV(MSG_ENDSTOP_Y, (float)endstops_trigsteps[Y_AXIS] / axis_steps_per_unit[Y_AXIS]);
      LCD_MESSAGEPGM(MSG_ENDSTOPS_HIT MSG_ENDSTOP_YS);
    }
-    if(endstop_z_hit) {
+    if (endstop_hit_bits & BIT(Z_MIN)) {
      ECHO_MV(MSG_ENDSTOP_Z, (float)endstops_trigsteps[Z_AXIS] / axis_steps_per_unit[Z_AXIS]);
      LCD_MESSAGEPGM(MSG_ENDSTOPS_HIT MSG_ENDSTOP_ZS);
    }
    #ifdef Z_PROBE_ENDSTOP
-    if (endstop_z_probe_hit) {
+    if (endstop_hit_bits & BIT(Z_PROBE)) {
-      ECHO_MV(" Z_PROBE:", (float)endstops_trigsteps[Z_AXIS] / axis_steps_per_unit[Z_AXIS]);
+      ECHO_MV(MSG_ENDSTOP_ZPS, (float)endstops_trigsteps[Z_AXIS] / axis_steps_per_unit[Z_AXIS]);
      LCD_MESSAGEPGM(MSG_ENDSTOPS_HIT MSG_ENDSTOP_ZPS);
    }
    #endif
-    if(endstop_e_hit) {
+    #ifdef NPR2
+    if (endstop_hit_bits & BIT(E_MIN)) {
      ECHO_MV(MSG_ENDSTOP_E, (float)endstops_trigsteps[E_AXIS] / axis_steps_per_unit[E_AXIS]);
      LCD_MESSAGEPGM(MSG_ENDSTOPS_HIT MSG_ENDSTOP_ES);
    }
-    ECHO_E;
-    endstops_hit_on_purpose();
-    #if defined(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED) && defined(SDSUPPORT)
-      if (abort_on_endstop_hit) {
-        card.sdprinting = false;
-        card.closeFile();
-        quickStop();
-        setTargetHotend0(0);
-        setTargetHotend1(0);
-        setTargetHotend2(0);
-        setTargetHotend3(0);
-        setTargetBed(0);
-      }
-    #endif
-  }
-}
-#else // NOT NPR2
-void endstops_hit_on_purpose() {
-  endstop_x_hit = endstop_y_hit = endstop_z_hit = endstop_z_probe_hit = false;
-}
-void checkHitEndstops() {
-  if (endstop_x_hit || endstop_y_hit || endstop_z_hit || endstop_z_probe_hit) {
-    ECHO_SM(OK, MSG_ENDSTOPS_HIT);
-    if (endstop_x_hit) {
-      ECHO_MV(MSG_ENDSTOP_X, (float)endstops_trigsteps[X_AXIS] / axis_steps_per_unit[X_AXIS]);
-      LCD_MESSAGEPGM(MSG_ENDSTOPS_HIT MSG_ENDSTOP_XS);
-    }
-    if (endstop_y_hit) {
-      ECHO_MV(MSG_ENDSTOP_Y, (float)endstops_trigsteps[Y_AXIS] / axis_steps_per_unit[Y_AXIS]);
-      LCD_MESSAGEPGM(MSG_ENDSTOPS_HIT MSG_ENDSTOP_YS);
-    }
-    if (endstop_z_hit) {
-      ECHO_MV(MSG_ENDSTOP_Z, (float)endstops_trigsteps[Z_AXIS] / axis_steps_per_unit[Z_AXIS]);
-      LCD_MESSAGEPGM(MSG_ENDSTOPS_HIT MSG_ENDSTOP_ZS);
-    }
-    #ifdef Z_PROBE_ENDSTOP
-    if (endstop_z_probe_hit) {
-      ECHO_MV(MSG_ENDSTOP_ZPS, (float)endstops_trigsteps[Z_AXIS] / axis_steps_per_unit[Z_AXIS]);
-      LCD_MESSAGEPGM(MSG_ENDSTOPS_HIT MSG_ENDSTOP_ZPS);
-    }
    #endif
    ECHO_E;
@@ -353,16 +282,11 @@ void checkHitEndstops() {
        card.sdprinting = false;
        card.closeFile();
        quickStop();
-        setTargetHotend0(0);
+        disable_all_heaters(); // switch off all heaters.
-        setTargetHotend1(0);
-        setTargetHotend2(0);
-        setTargetHotend3(0);
-        setTargetBed(0);
      }
    #endif
  }
 }
-#endif // NOT NPR2
 void enable_endstops(bool check) { check_endstops = check; }
@@ -422,9 +346,58 @@ FORCE_INLINE unsigned short calc_timer(unsigned short step_rate) {
  return timer;
 }
+// set the stepper direction of each axis
+void set_stepper_direction() {
+  // Set the direction bits (X_AXIS=A_AXIS and Y_AXIS=B_AXIS for COREXY)
+  if (TEST(out_bits, X_AXIS)) {
+    X_APPLY_DIR(INVERT_X_DIR,0);
+    count_direction[X_AXIS] = -1;
+  }
+  else {
+    X_APPLY_DIR(!INVERT_X_DIR,0);
+    count_direction[X_AXIS] = 1;
+  }
+  if (TEST(out_bits, Y_AXIS)) {
+    Y_APPLY_DIR(INVERT_Y_DIR,0);
+    count_direction[Y_AXIS] = -1;
+  }
+  else {
+    Y_APPLY_DIR(!INVERT_Y_DIR,0);
+    count_direction[Y_AXIS] = 1;
+  }
+  if (TEST(out_bits, Z_AXIS)) {
+    Z_APPLY_DIR(INVERT_Z_DIR,0);
+    count_direction[Z_AXIS] = -1;
+  }
+  else {
+    Z_APPLY_DIR(!INVERT_Z_DIR,0);
+    count_direction[Z_AXIS] = 1;
+  }
+  #ifndef ADVANCE
+    if (TEST(out_bits, E_AXIS)) {
+      REV_E_DIR();
+      count_direction[E_AXIS] = -1;
+    }
+    else {
+      NORM_E_DIR();
+      count_direction[E_AXIS] = 1;
+    }
+  #endif // !ADVANCE
+}
 // Initializes the trapezoid generator from the current block. Called whenever a new
 // block begins.
 FORCE_INLINE void trapezoid_generator_reset() {
+  if (current_block->direction_bits != out_bits) {
+    out_bits = current_block->direction_bits;
+    set_stepper_direction();
+  }
  #ifdef ADVANCE
    advance = current_block->initial_advance;
    final_advance = current_block->final_advance;
@@ -446,8 +419,7 @@ FORCE_INLINE void trapezoid_generator_reset() {
 // It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately.
 ISR(TIMER1_COMPA_vect) {
-  if(cleaning_buffer_counter)
+  if(cleaning_buffer_counter) {
-  {
    current_block = NULL;
    plan_discard_current_block();
    #ifdef SD_FINISHED_RELEASECOMMAND
@@ -487,47 +459,37 @@ ISR(TIMER1_COMPA_vect) {
  }
  if (current_block != NULL) {
-    // Set directions TO DO This should be done once during init of trapezoid. Endstops -> interrupt
-    out_bits = current_block->direction_bits;
-    // Set the direction bits (X_AXIS=A_AXIS and Y_AXIS=B_AXIS for COREXY)
+    // Check endstops
-    if (TEST(out_bits, X_AXIS)) {
+    if (check_endstops) {
-      X_APPLY_DIR(INVERT_X_DIR,0);
-      count_direction[X_AXIS] = -1;
-    }
-    else {
-      X_APPLY_DIR(!INVERT_X_DIR,0);
-      count_direction[X_AXIS] = 1;
-    }
-    if (TEST(out_bits, Y_AXIS)) {
+      #ifdef Z_DUAL_ENDSTOPS
-      Y_APPLY_DIR(INVERT_Y_DIR,0);
+        uint16_t current_endstop_bits = 0;
-      count_direction[Y_AXIS] = -1;
+      #else
-    }
+        byte current_endstop_bits = 0;
-    else {
+      #endif
-      Y_APPLY_DIR(!INVERT_Y_DIR,0);
-      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 _ENDSTOP_HIT(AXIS) endstop_hit_bits |= BIT(_ENDSTOP(AXIS, MIN))
+      #define _ENDSTOP(AXIS, MINMAX) AXIS ##_## MINMAX
-    #define UPDATE_ENDSTOP(axis,AXIS,minmax,MINMAX) \
-      bool _ENDSTOP(axis, minmax) = (READ(_ENDSTOP_PIN(AXIS, MINMAX)) != _ENDSTOP_INVERTING(AXIS, MINMAX)); \
+      // SET_ENDSTOP_BIT: set the current endstop bits for an endstop to its status
-      if (_ENDSTOP(axis, minmax) && _OLD_ENDSTOP(axis, minmax) && (current_block->steps[_AXIS(AXIS)] > 0)) { \
+      #define SET_ENDSTOP_BIT(AXIS, MINMAX) SET_BIT(current_endstop_bits, _ENDSTOP(AXIS, MINMAX), (READ(_ENDSTOP_PIN(AXIS, MINMAX)) != _ENDSTOP_INVERTING(AXIS, MINMAX)))
+      // COPY_BIT: copy the value of COPY_BIT to BIT in bits
+      #define COPY_BIT(bits, COPY_BIT, BIT) SET_BIT(bits, BIT, TEST(bits, COPY_BIT))
+      // TEST_ENDSTOP: test the old and the current status of an endstop
+      #define TEST_ENDSTOP(ENDSTOP) (TEST(current_endstop_bits, ENDSTOP) && TEST(old_endstop_bits, ENDSTOP))
+      #define UPDATE_ENDSTOP(AXIS,MINMAX) \
+        SET_ENDSTOP_BIT(AXIS, MINMAX); \
+        if (TEST_ENDSTOP(_ENDSTOP(AXIS, MINMAX))  && (current_block->steps[_AXIS(AXIS)] > 0)) { \
          endstops_trigsteps[_AXIS(AXIS)] = count_position[_AXIS(AXIS)]; \
-        _ENDSTOP_HIT(axis) = true; \
+          _ENDSTOP_HIT(AXIS); \
          step_events_completed = current_block->step_event_count; \
-      } \
+        }
-      _OLD_ENDSTOP(axis, minmax) = _ENDSTOP(axis, minmax);
-    // Check X and Y endstops
-    if (check_endstops) {
      #ifdef COREXY
        // Head direction in -X axis for CoreXY bots.
        // If DeltaX == -DeltaY, the movement is only in Y axis
@@ -543,7 +505,7 @@ ISR(TIMER1_COMPA_vect) {
            #endif
              {
                #if HAS_X_MIN
-                  UPDATE_ENDSTOP(x, X, min, MIN);
+                  UPDATE_ENDSTOP(X, MIN);
                #endif
              }
          }
@@ -554,7 +516,7 @@ ISR(TIMER1_COMPA_vect) {
            #endif
              {
                #if HAS_X_MAX
-                  UPDATE_ENDSTOP(x, X, max, MAX);
+                  UPDATE_ENDSTOP(X, MAX);
                #endif
              }
          }
@@ -569,144 +531,83 @@ ISR(TIMER1_COMPA_vect) {
      #endif
          { // -direction
            #if HAS_Y_MIN
-              UPDATE_ENDSTOP(y, Y, min, MIN);
+              UPDATE_ENDSTOP(Y, MIN);
            #endif
          }
          else { // +direction
            #if HAS_Y_MAX
-              UPDATE_ENDSTOP(y, Y, max, MAX);
+              UPDATE_ENDSTOP(Y, MAX);
            #endif
          }
      #ifdef COREXY
        }
      #endif
-    }
+      if (TEST(out_bits, Z_AXIS)) { // z -direction
-    if (TEST(out_bits, Z_AXIS)) {   // -direction
-      Z_APPLY_DIR(INVERT_Z_DIR,0);
-      count_direction[Z_AXIS] = -1;
-      if (check_endstops) {
        #if HAS_Z_MIN
          #ifdef Z_DUAL_ENDSTOPS
+            SET_ENDSTOP_BIT(Z, MIN);
-            bool z_min_endstop = READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING,
-                z2_min_endstop =
              #if HAS_Z2_MIN
-                    READ(Z2_MIN_PIN) != Z2_MIN_ENDSTOP_INVERTING
+                SET_ENDSTOP_BIT(Z2, MIN);
              #else
-                    z_min_endstop
+                COPY_BIT(current_endstop_bits, Z_MIN, Z2_MIN)
              #endif
-                ;
-            bool z_min_both = z_min_endstop && old_z_min_endstop,
+            byte z_test = TEST_ENDSTOP(Z_MIN) << 0 + TEST_ENDSTOP(Z2_MIN) << 1; // bit 0 for Z, bit 1 for Z2
-                z2_min_both = z2_min_endstop && old_z2_min_endstop;
-            if ((z_min_both || z2_min_both) && current_block->steps[Z_AXIS] > 0) {
+            if (z_test && current_block->steps[Z_AXIS] > 0) { // z_test = Z_MIN || Z2_MIN
              endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
-              endstop_z_hit = true;
+              endstop_hit_bits |= BIT(Z_MIN);
-              if (!performing_homing || (performing_homing && z_min_both && z2_min_both)) //if not performing home or if both endstops were trigged during homing...
+              if (!performing_homing || (z_test == 0x3))  //if not performing home or if both endstops were trigged during homing...
                step_events_completed = current_block->step_event_count;
            }
-            old_z_min_endstop = z_min_endstop;
-            old_z2_min_endstop = z2_min_endstop;
          #else // !Z_DUAL_ENDSTOPS
-            UPDATE_ENDSTOP(z, Z, min, MIN);
+            UPDATE_ENDSTOP(Z, MIN);
          #endif // !Z_DUAL_ENDSTOPS
        #endif // Z_MIN_PIN
        #ifdef Z_PROBE_ENDSTOP
-          UPDATE_ENDSTOP(z, Z, probe, PROBE);
+          UPDATE_ENDSTOP(Z, 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) ECHO_EV("z_probe_endstop = true");
+          if (TEST_ENDSTOP(Z_PROBE)) {
+            endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
+            endstop_hit_bits |= BIT(Z_PROBE);
          }
-          old_z_probe_endstop = z_probe_endstop;
        #endif
-      } // check_endstops
      }
-    else { // +direction
+      else { // z +direction
-      Z_APPLY_DIR(!INVERT_Z_DIR,0);
-      count_direction[Z_AXIS] = 1;
-      if (check_endstops) {
        #if HAS_Z_MAX
          #ifdef Z_DUAL_ENDSTOPS
-            bool z_max_endstop = READ(Z_MAX_PIN) != Z_MAX_ENDSTOP_INVERTING,
+            SET_ENDSTOP_BIT(Z, MAX);
-                z2_max_endstop =
              #if HAS_Z2_MAX
-                    READ(Z2_MAX_PIN) != Z2_MAX_ENDSTOP_INVERTING
+                SET_ENDSTOP_BIT(Z2, MAX);
              #else
-                    z_max_endstop
+                COPY_BIT(current_endstop_bits, Z_MAX, Z2_MAX)
              #endif
-                ;
-            bool z_max_both = z_max_endstop && old_z_max_endstop,
+            byte z_test = TEST_ENDSTOP(Z_MAX) << 0 + TEST_ENDSTOP(Z2_MAX) << 1; // bit 0 for Z, bit 1 for Z2
-                z2_max_both = z2_max_endstop && old_z2_max_endstop;
-            if ((z_max_both || z2_max_both) && current_block->steps[Z_AXIS] > 0) {
-              endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
-              endstop_z_hit = true;
-             // if (z_max_both) ECHO_EV("z_max_endstop = true");
+            if (z_test && current_block->steps[Z_AXIS] > 0) {  // t_test = Z_MAX || Z2_MAX
-             // if (z2_max_both) ECHO_EV("z2_max_endstop = true");
+              endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
+              endstop_hit_bits |= BIT(Z_MIN);
-              if (!performing_homing || (performing_homing && z_max_both && z2_max_both)) //if not performing home or if both endstops were trigged during homing...
+              if (!performing_homing || (z_test == 0x3))  //if not performing home or if both endstops were trigged during homing...
                step_events_completed = current_block->step_event_count;
            }
-            old_z_max_endstop = z_max_endstop;
-            old_z2_max_endstop = z2_max_endstop;
          #else // !Z_DUAL_ENDSTOPS
-            UPDATE_ENDSTOP(z, Z, max, MAX);
+            UPDATE_ENDSTOP(Z, MAX);
          #endif // !Z_DUAL_ENDSTOPS
        #endif // Z_MAX_PIN
-      } // check_endstops
-    } // +direction
-    #ifndef ADVANCE
-      if (TEST(out_bits, E_AXIS)) {  // -direction
-        REV_E_DIR();
-        count_direction[E_AXIS] = -1;
-	      #ifdef NPR2
-          if (check_endstops) {
-            #if defined(E_MIN_PIN) && E_MIN_PIN > -1
-              bool e_min_endstop=(READ(E_MIN_PIN) != E_MIN_ENDSTOP_INVERTING);
-              if (e_min_endstop && old_e_min_endstop && (current_block->steps[E_AXIS] > 0)) {
-                endstops_trigsteps[E_AXIS] = count_position[E_AXIS];
-                endstop_e_hit = true;
-                step_events_completed = current_block->step_event_count;
      }
-              old_e_min_endstop = e_min_endstop;
+      old_endstop_bits = current_endstop_bits;
-            #endif
-          }
-        #endif
    }
-      else { // +direction
-        NORM_E_DIR();
-        count_direction[E_AXIS] = 1;
-      }
-    #endif //!ADVANCE
    // Take multiple steps per interrupt (For high speed moves)
    for (int8_t i = 0; i < step_loops; i++) {
@@ -726,29 +627,33 @@ ISR(TIMER1_COMPA_vect) {
      #define _APPLY_STEP(AXIS) AXIS ##_APPLY_STEP
      #define _INVERT_STEP_PIN(AXIS) INVERT_## AXIS ##_STEP_PIN
-      #define STEP_ADD(axis, AXIS) \
+      #define STEP_START(axis, AXIS) \
        _COUNTER(axis) += current_block->steps[_AXIS(AXIS)]; \
        if (_COUNTER(axis) > 0) { _APPLY_STEP(AXIS)(!_INVERT_STEP_PIN(AXIS),0); }
-      STEP_ADD(x,X);
+      STEP_START(x,X);
-      STEP_ADD(y,Y);
+      STEP_START(y,Y);
-      STEP_ADD(z,Z);
+      STEP_START(z,Z);
      #ifndef ADVANCE
-        STEP_ADD(e,E);
+        STEP_START(e,E);
+      #endif
+      #ifdef STEPPER_HIGH_LOW_DELAY
+        delayMicroseconds(STEPPER_HIGH_LOW_DELAY);
      #endif
-      #define STEP_IF_COUNTER(axis, AXIS) \
+      #define STEP_END(axis, AXIS) \
        if (_COUNTER(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); \
        }
-      STEP_IF_COUNTER(x, X);
+      STEP_END(x, X);
-      STEP_IF_COUNTER(y, Y);
+      STEP_END(y, Y);
-      STEP_IF_COUNTER(z, Z);
+      STEP_END(z, Z);
      #ifndef ADVANCE
-        STEP_IF_COUNTER(e, E);
+        STEP_END(e, E);
      #endif
      step_events_completed++;
@@ -1005,21 +910,21 @@ void st_init() {
  #if HAS_X_MIN
    SET_INPUT(X_MIN_PIN);
    #ifdef ENDSTOPPULLUP_XMIN
-      WRITE(X_MIN_PIN, HIGH);
+      WRITE(X_MIN_PIN,HIGH);
    #endif
  #endif
  #if HAS_Y_MIN
    SET_INPUT(Y_MIN_PIN);
    #ifdef ENDSTOPPULLUP_YMIN
-      WRITE(Y_MIN_PIN, HIGH);
+      WRITE(Y_MIN_PIN,HIGH);
    #endif
  #endif
  #if HAS_Z_MIN
    SET_INPUT(Z_MIN_PIN);
    #ifdef ENDSTOPPULLUP_ZMIN
-      WRITE(Z_MIN_PIN, HIGH);
+      WRITE(Z_MIN_PIN,HIGH);
    #endif
  #endif
@@ -1033,37 +938,37 @@ void st_init() {
  #if HAS_X_MAX
    SET_INPUT(X_MAX_PIN);
    #ifdef ENDSTOPPULLUP_XMAX
-      WRITE(X_MAX_PIN, HIGH);
+      WRITE(X_MAX_PIN,HIGH);
    #endif
  #endif
  #if HAS_Y_MAX
    SET_INPUT(Y_MAX_PIN);
    #ifdef ENDSTOPPULLUP_YMAX
-      WRITE(Y_MAX_PIN, HIGH);
+      WRITE(Y_MAX_PIN,HIGH);
    #endif
  #endif
  #if HAS_Z_MAX
    SET_INPUT(Z_MAX_PIN);
    #ifdef ENDSTOPPULLUP_ZMAX
-      WRITE(Z_MAX_PIN, HIGH);
+      WRITE(Z_MAX_PIN,HIGH);
    #endif
  #endif
  #if HAS_Z2_MAX
    SET_INPUT(Z2_MAX_PIN);
    #ifdef ENDSTOPPULLUP_ZMAX
-      WRITE(Z2_MAX_PIN, HIGH);
+      WRITE(Z2_MAX_PIN,HIGH);
    #endif
  #endif
-#if (defined(Z_PROBE_PIN) && Z_PROBE_PIN >= 0) && defined(Z_PROBE_ENDSTOP) // Check for Z_PROBE_ENDSTOP so we don't pull a pin high unless it's to be used.
+  #if HAS_Z_PROBE && defined(Z_PROBE_ENDSTOP) // Check for Z_PROBE_ENDSTOP so we don't pull a pin high unless it's to be used.
    SET_INPUT(Z_PROBE_PIN);
    #ifdef ENDSTOPPULLUP_ZPROBE
-    WRITE(Z_PROBE_PIN, HIGH);
+      WRITE(Z_PROBE_PIN,HIGH);
+    #endif
  #endif
-#endif
  #define _STEP_INIT(AXIS) AXIS ##_STEP_INIT
  #define _WRITE_STEP(AXIS, HIGHLOW) AXIS ##_STEP_WRITE(HIGHLOW)
@@ -1142,17 +1047,15 @@ void st_init() {
  enable_endstops(true); // Start with endstops active. After homing they can be disabled
  sei();
+  set_stepper_direction(); // Init directions to out_bits = 0
 }
-// Block until all buffered steps are executed
+/**
-void st_synchronize() {
+ * Block until all buffered steps are executed
-  while (blocks_queued()) {
+ */
-    manage_heater();
+void st_synchronize() { while (blocks_queued()) idle(); }
-    manage_inactivity();
-    lcd_update();
-  }
-}
 void st_set_position(const long &x, const long &y, const long &z, const long &e) {
  CRITICAL_SECTION_START;
@@ -1179,9 +1082,8 @@ long st_get_position(uint8_t axis) {
 #ifdef ENABLE_AUTO_BED_LEVELING
-  float st_get_position_mm(uint8_t axis) {
+  float st_get_position_mm(AxisEnum axis) {
-    float steper_position_in_steps = st_get_position(axis);
+    return st_get_position(axis) / axis_steps_per_unit[axis];
-    return steper_position_in_steps / axis_steps_per_unit[axis];
  }
 #endif  // ENABLE_AUTO_BED_LEVELING
@@ -1382,7 +1284,7 @@ void microstep_ms(uint8_t driver, int8_t ms1, int8_t ms2) {
    case 1: digitalWrite(Y_MS2_PIN, ms2); break;
    case 2: digitalWrite(Z_MS2_PIN, ms2); break;
    case 3: digitalWrite(E0_MS2_PIN, ms2); break;
-    #if defined(E1_MS2_PIN) && E1_MS2_PIN >= 0
+    #if PIN_EXISTS(E1_MS2)
      case 4: digitalWrite(E1_MS2_PIN, ms2); break;
    #endif
  }
@@ -1402,21 +1304,21 @@ void microstep_mode(uint8_t driver, uint8_t stepping_mode) {
 }
 void microstep_readings() {
-  ECHO_SM(OK, MSG_MICROSTEP_MS1_MS2);
+  ECHO_SM(DB, MSG_MICROSTEP_MS1_MS2);
  ECHO_M(MSG_MICROSTEP_X);
  ECHO_V(digitalRead(X_MS1_PIN));
  ECHO_EV(digitalRead(X_MS2_PIN));
-  ECHO_SM(OK, MSG_MICROSTEP_Y);
+  ECHO_SM(DB, MSG_MICROSTEP_Y);
  ECHO_V(digitalRead(Y_MS1_PIN));
  ECHO_EV(digitalRead(Y_MS2_PIN));
-  ECHO_SM(OK, MSG_MICROSTEP_Z);
+  ECHO_SM(DB, MSG_MICROSTEP_Z);
  ECHO_V(digitalRead(Z_MS1_PIN));
  ECHO_EV(digitalRead(Z_MS2_PIN));
-  ECHO_SM(OK, MSG_MICROSTEP_E0);
+  ECHO_SM(DB, MSG_MICROSTEP_E0);
  ECHO_V(digitalRead(E0_MS1_PIN));
  ECHO_EV(digitalRead(E0_MS2_PIN));
  #if HAS_MICROSTEPS_E1
-    ECHO_SM(OK, MSG_MICROSTEP_E1);
+    ECHO_SM(DB, MSG_MICROSTEP_E1);
    ECHO_V(digitalRead(E1_MS1_PIN));
    ECHO_EV(digitalRead(E1_MS2_PIN));
  #endif

--- a/MarlinKimbra/stepper.h
+++ b/MarlinKimbra/stepper.h
@@ -54,7 +54,7 @@
 #endif //DRIVER_EXTRUDERS
 #ifdef ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED
-extern bool abort_on_endstop_hit;
+  extern bool abort_on_endstop_hit;
 #endif
 // Initialize and start the stepper motor subsystem
@@ -71,9 +71,9 @@ void st_set_e_position(const long &e);
 long st_get_position(uint8_t axis);
 #ifdef ENABLE_AUTO_BED_LEVELING
-// Get current position in mm
+  // Get current position in mm
-float st_get_position_mm(uint8_t axis);
+  float st_get_position_mm(AxisEnum axis);
-#endif  //ENABLE_AUTO_BED_LEVELING
+#endif
 // The stepper subsystem goes to sleep when it runs out of things to execute. Call this
 // to notify the subsystem that it is time to go to work.
@@ -111,8 +111,8 @@ void microstep_readings();
  void babystep(const uint8_t axis,const bool direction); // perform a short step with a single stepper motor, outside of any convention
 #endif
-#ifdef NPR2 //Multiextruder
+#ifdef NPR2 // Multiextruder
  void colorstep(long csteps,const bool direction);
-#endif //NPR2
+#endif
-#endif //stepper_h
+#endif // stepper_h
--- a/MarlinKimbra/stepper_indirection.cpp
+++ b/MarlinKimbra/stepper_indirection.cpp
@@ -23,8 +23,8 @@
 #include "Configuration.h"
 #ifdef HAVE_TMCDRIVER
-#include <SPI.h>
+  #include <SPI.h>
-#include <TMC26XStepper.h>
+  #include <TMC26XStepper.h>
 #endif
 // Stepper objects of TMC steppers used

--- a/MarlinKimbra/temperature.cpp
+++ b/MarlinKimbra/temperature.cpp
@@ -36,6 +36,7 @@
 #if defined(PIDTEMPBED) || defined(PIDTEMP)
  #define PID_dT ((OVERSAMPLENR * 14.0)/(F_CPU / 64.0 / 256.0))
+  #define RECI_PID_dT ( 1 / PID_dT )
 #endif
 //===========================================================================
@@ -54,9 +55,9 @@ float current_temperature_bed = 0.0;
 #endif
 #ifdef PIDTEMPBED
-  float bedKp=DEFAULT_bedKp;
+  float bedKp = DEFAULT_bedKp;
-  float bedKi=(DEFAULT_bedKi*PID_dT);
+  float bedKi = (DEFAULT_bedKi * PID_dT);
-  float bedKd=(DEFAULT_bedKd/PID_dT);
+  float bedKd = (DEFAULT_bedKd / PID_dT);
 #endif //PIDTEMPBED
 #ifdef FAN_SOFT_PWM
@@ -69,20 +70,19 @@ unsigned char soft_pwm_bed;
  volatile int babystepsTodo[3] = { 0 };
 #endif
-#if HAS_FILAMENT_SENSOR
+#ifdef FILAMENT_SENSOR
  int current_raw_filwidth = 0;  //Holds measured filament diameter - one extruder only
 #endif  
-#define HAS_HEATER_THERMAL_PROTECTION (defined(THERMAL_RUNAWAY_PROTECTION_PERIOD) && THERMAL_RUNAWAY_PROTECTION_PERIOD > 0)
+#if defined(THERMAL_PROTECTION_HOTENDS) || defined(THERMAL_PROTECTION_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 };
+  static float tr_target_temperature[HOTENDS + 1] = { 0.0 };
  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
+  #ifdef THERMAL_PROTECTION_HOTENDS
    static TRState thermal_runaway_state_machine[4] = { TRReset, TRReset, TRReset, TRReset };
    static millis_t thermal_runaway_timer[4]; // = {0,0,0,0};
  #endif
-  #if HAS_BED_THERMAL_PROTECTION
+  #ifdef THERMAL_PROTECTION_BED
    static TRState thermal_runaway_bed_state_machine = TRReset;
    static millis_t thermal_runaway_bed_timer;
  #endif
@@ -95,6 +95,7 @@ unsigned char soft_pwm_bed;
 //===========================================================================
 //============================ private variables ============================
 //===========================================================================
 static volatile bool temp_meas_ready = false;
 #ifdef PIDTEMP
@@ -143,7 +144,9 @@ static int minttemp_raw[HOTENDS] = ARRAY_BY_HOTENDS( HEATER_0_RAW_LO_TEMP , HEAT
 static int maxttemp_raw[HOTENDS] = ARRAY_BY_HOTENDS( HEATER_0_RAW_HI_TEMP , HEATER_1_RAW_HI_TEMP , HEATER_2_RAW_HI_TEMP, HEATER_3_RAW_HI_TEMP);
 static int minttemp[HOTENDS] = { 0 };
 static int maxttemp[HOTENDS] = ARRAY_BY_HOTENDS( 16383, 16383, 16383, 16383 );
-//static int bed_minttemp_raw = HEATER_BED_RAW_LO_TEMP; /* No bed mintemp error implemented?!? */
+#ifdef BED_MINTEMP
+  static int bed_minttemp_raw = HEATER_BED_RAW_LO_TEMP;
+#endif
 #ifdef BED_MAXTEMP
  static int bed_maxttemp_raw = HEATER_BED_RAW_HI_TEMP;
 #endif
@@ -160,7 +163,7 @@ static float analog2temp(int raw, uint8_t e);
 static float analog2tempBed(int raw);
 static void updateTemperaturesFromRawValues();
-#ifdef WATCH_TEMP_PERIOD
+#ifdef THERMAL_PROTECTION_HOTENDS
  int watch_target_temp[HOTENDS] = { 0 };
  millis_t watch_heater_next_ms[HOTENDS] = { 0 };
 #endif
@@ -181,8 +184,7 @@ static void updateTemperaturesFromRawValues();
 //================================ Functions ================================
 //===========================================================================
-void PID_autotune(float temp, int hotend, int ncycles)
+void PID_autotune(float temp, int hotend, int ncycles) {
-{
  float input = 0.0;
  int cycles = 0;
  bool heating = true;
@@ -245,8 +247,8 @@ void PID_autotune(float temp, int hotend, int ncycles)
        }
      #endif
-      if (heating == true && input > temp) {
+      if (heating && input > temp) {
-        if (ms - t2 > 5000) {
+        if (ms > t2 + 5000) {
          heating = false;
          if (hotend < 0)
            soft_pwm_bed = (bias - d) >> 1;
@@ -257,8 +259,9 @@ void PID_autotune(float temp, int hotend, int ncycles)
          max = temp;
        }
      }
-      if (heating == false && input < temp) {
-        if (ms - t1 > 5000) {
+      if (!heating && input < temp) {
+        if (ms > t1 + 5000) {
          heating = true;
          t2 = ms;
          t_low = t2 - t1;
@@ -299,7 +302,7 @@ void PID_autotune(float temp, int hotend, int ncycles)
        }
      }
    }
-    if (input > temp + 20) {
+    if (input > temp + MAX_OVERSHOOT_PID_AUTOTUNE) {
      ECHO_LM(ER, MSG_PID_TEMP_TOO_HIGH);
      return;
    }
@@ -310,12 +313,14 @@ void PID_autotune(float temp, int hotend, int ncycles)
      if (hotend < 0) {
        p = soft_pwm_bed;
        ECHO_SMV(OK, MSG_B, input);
-        ECHO_EMV(MSG_AT, p);
+        ECHO_MV(" /", temp, 1);
+        ECHO_EMV(" " MSG_AT, p);
      }
      else {
        p = soft_pwm[hotend];
-        ECHO_SMV(OK, MSG_T, input);
+        ECHO_SMV(OK, MSG_T, input, 1);
-        ECHO_EMV(MSG_AT, p);
+        ECHO_MV(" /", temp, 1);
+        ECHO_EMV(" " MSG_AT, p);
      }
      temp_ms = ms;
@@ -356,7 +361,7 @@ void updatePID() {
    }
  #endif
  #ifdef PIDTEMPBED
-    temp_iState_max_bed = PID_INTEGRAL_DRIVE_MAX / bedKi;
+    temp_iState_max_bed = PID_BED_INTEGRAL_DRIVE_MAX / bedKi;
  #endif
 }
@@ -445,39 +450,40 @@ void checkExtruderAutoFans()
 // Temperature Error Handlers
 //
 inline void _temp_error(int e, const char *serial_msg, const char *lcd_msg) {
+  static bool killed = false;
  if (IsRunning()) {
    ECHO_S(ER);
-    if (e >= 0) ECHO_EV((int)e);
    PS_PGM(serial_msg);
-    ECHO_E;
+    ECHO_M(MSG_STOPPED_HEATER);
+    if (e >= 0) ECHO_EV((int)e); else ECHO_EM(MSG_HEATER_BED);
    #ifdef ULTRA_LCD
      lcd_setalertstatuspgm(lcd_msg);
    #endif
  }
  #ifndef BOGUS_TEMPERATURE_FAILSAFE_OVERRIDE
-    Stop();
+    if (!killed) {
+      Running = false;
+      killed = true;
+      kill(lcd_msg);
+    }
+    else
+      disable_all_heaters(); // paranoia
  #endif
 }
 void max_temp_error(uint8_t e) {
-  disable_all_heaters();
+  _temp_error(e, PSTR(MSG_T_MAXTEMP), PSTR(MSG_ERR_MAXTEMP));
-  _temp_error(e, PSTR(MSG_MAXTEMP_EXTRUDER_OFF), PSTR(MSG_ERR_MAXTEMP));
 }
 void min_temp_error(uint8_t e) {
-  disable_all_heaters();
+  _temp_error(e, PSTR(MSG_T_MINTEMP), PSTR(MSG_ERR_MINTEMP));
-  _temp_error(e, PSTR(MSG_MINTEMP_EXTRUDER_OFF), PSTR(MSG_ERR_MINTEMP));
-}
-void bed_max_temp_error(void) {
-  #if HAS_HEATER_BED
-    WRITE_HEATER_BED(0);
-  #endif
-  _temp_error(-1, PSTR(MSG_MAXTEMP_BED_OFF), PSTR(MSG_ERR_MAXTEMP_BED));
 }
 float get_pid_output(int e) {
  float pid_output;
  #ifdef PIDTEMP
-    #ifndef PID_OPENLOOP
+    #ifdef PID_OPENLOOP
+      pid_output = constrain(target_temperature[e], 0, PID_MAX);
+    #else
      pid_error[e] = target_temperature[e] - current_temperature[e];
      if (pid_error[e] > PID_FUNCTIONAL_RANGE) {
        pid_output = BANG_MAX;
@@ -509,17 +515,15 @@ float get_pid_output(int e) {
        }
      }
      temp_dState[e] = current_temperature[e];
-    #else
-      pid_output = constrain(target_temperature[e], 0, PID_MAX);
    #endif //PID_OPENLOOP
    #ifdef PID_DEBUG
-      ECHO_SMV(DB, " PID_DEBUG ", e);
+      ECHO_SMV(DB, MSG_PID_DEBUG, e);
-      ECHO_MV(": Input ", current_temperature[e]);
+      ECHO_MV(MSG_PID_DEBUG_INPUT, current_temperature[e]);
-      ECHO_MV(" Output ", pid_output);
+      ECHO_MV(MSG_PID_DEBUG_OUTPUT, pid_output);
-      ECHO_MV(" pTerm ", pTerm[e]);
+      ECHO_MV(MSG_PID_DEBUG_PTERM, pTerm[e]);
-      ECHO_MV(" iTerm ", iTerm[e]);
+      ECHO_MV(MSG_PID_DEBUG_ITERM, iTerm[e]);
-      ECHO_EMV(" dTerm ", dTerm[e]);
+      ECHO_EMV(MSG_PID_DEBUG_DTERM, dTerm[e]);
    #endif //PID_DEBUG
  #else /* PID off */
@@ -532,7 +536,9 @@ float get_pid_output(int e) {
 #ifdef PIDTEMPBED
  float get_pid_output_bed() {
    float pid_output;
-    #ifndef PID_OPENLOOP
+    #ifdef PID_OPENLOOP
+      pid_output = constrain(target_temperature_bed, 0, MAX_BED_POWER);
+    #else
      pid_error_bed = target_temperature_bed - current_temperature_bed;
      pTerm_bed = bedKp * pid_error_bed;
      temp_iState_bed += pid_error_bed;
@@ -551,8 +557,6 @@ float get_pid_output(int e) {
        if (pid_error_bed < 0) temp_iState_bed -= pid_error_bed; // conditional un-integration
        pid_output = 0;
      }
-    #else
-      pid_output = constrain(target_temperature_bed, 0, MAX_BED_POWER);
    #endif // PID_OPENLOOP
    #ifdef PID_BED_DEBUG
@@ -588,15 +592,15 @@ void manage_heater() {
    if (ct < max(HEATER_0_MINTEMP, 0.01)) min_temp_error(0);
  #endif
-  #if defined(WATCH_TEMP_PERIOD) || !defined(PIDTEMPBED) || HAS_AUTO_FAN
+  #if defined(THERMAL_PROTECTION_HOTENDS) || !defined(PIDTEMPBED) || HAS_AUTO_FAN
    millis_t ms = millis();
  #endif
  // Loop through all hotends
  for (int e = 0; e < HOTENDS; e++) {
-    #if HAS_HEATER_THERMAL_PROTECTION
+    #ifdef THERMAL_PROTECTION_HOTENDS
-      thermal_runaway_protection(&thermal_runaway_state_machine[e], &thermal_runaway_timer[e], current_temperature[e], target_temperature[e], e, THERMAL_RUNAWAY_PROTECTION_PERIOD, THERMAL_RUNAWAY_PROTECTION_HYSTERESIS);
+      thermal_runaway_protection(&thermal_runaway_state_machine[e], &thermal_runaway_timer[e], current_temperature[e], target_temperature[e], e, THERMAL_PROTECTION_PERIOD, THERMAL_PROTECTION_HYSTERESIS);
    #endif
    float pid_output = get_pid_output(e);
@@ -605,25 +609,25 @@ void manage_heater() {
    soft_pwm[e] = current_temperature[e] > minttemp[e] && current_temperature[e] < maxttemp[e] ? (int)pid_output >> 1 : 0;
    // Check if the temperature is failing to increase
-    #ifdef WATCH_TEMP_PERIOD
+    #ifdef THERMAL_PROTECTION_HOTENDS
      // Is it time to check this extruder's heater?
      if (watch_heater_next_ms[e] && ms > watch_heater_next_ms[e]) {
        // Has it failed to increase enough?
        if (degHotend(e) < watch_target_temp[e]) {
          // Stop!
-          disable_all_heaters();
+          _temp_error(e, PSTR(MSG_T_HEATING_FAILED), PSTR(MSG_HEATING_FAILED_LCD));
-          _temp_error(e, MSG_HEATING_FAILED, MSG_HEATING_FAILED_LCD);
        }
        else {
-          // Only check once per M104/M109
+          // Start again if the target is still far off
-          watch_heater_next_ms[e] = 0;
+          start_watching_heater(e);
        }
      }
-    #endif // WATCH_TEMP_PERIOD
+    #endif // THERMAL_PROTECTION_HOTENDS
    #ifdef TEMP_SENSOR_1_AS_REDUNDANT
      if (fabs(current_temperature[0] - redundant_temperature) > MAX_REDUNDANT_TEMP_SENSOR_DIFF) {
-        disable_all_heaters();
        _temp_error(0, PSTR(MSG_EXTRUDER_SWITCHED_OFF), PSTR(MSG_ERR_REDUNDANT_TEMP));
      }
    #endif
@@ -659,8 +663,8 @@ void manage_heater() {
  #if TEMP_SENSOR_BED != 0
-    #if HAS_BED_THERMAL_PROTECTION
+    #ifdef THERMAL_PROTECTION_BED
-      thermal_runaway_protection(&thermal_runaway_bed_state_machine, &thermal_runaway_bed_timer, current_temperature_bed, target_temperature_bed, -1, THERMAL_RUNAWAY_PROTECTION_BED_PERIOD, THERMAL_RUNAWAY_PROTECTION_BED_HYSTERESIS);
+      thermal_runaway_protection(&thermal_runaway_bed_state_machine, &thermal_runaway_bed_timer, current_temperature_bed, target_temperature_bed, -1, THERMAL_PROTECTION_BED_PERIOD, THERMAL_PROTECTION_BED_HYSTERESIS);
    #endif
    #ifdef PIDTEMPBED
@@ -704,7 +708,7 @@ static float analog2temp(int raw, uint8_t e) {
  #endif
    {
      ECHO_LVM(ER, (int)e, MSG_INVALID_EXTRUDER_NUM);
-      kill();
+      kill(PSTR(MSG_KILLED));
      return 0.0;
    }
@@ -732,12 +736,7 @@ static float analog2temp(int raw, uint8_t e) {
    return celsius;
  }
-  #ifdef __SAM3X8E__
-    return ((raw * ((3.3 * 100) / 1024) / OVERSAMPLENR) * TEMP_SENSOR_AD595_GAIN) + TEMP_SENSOR_AD595_OFFSET;
-  #else
    return ((raw * ((5.0 * 100.0) / 1024.0) / OVERSAMPLENR) * TEMP_SENSOR_AD595_GAIN) + TEMP_SENSOR_AD595_OFFSET;
-  #endif
 }
 // Derived from RepRap FiveD extruder::getTemperature()
@@ -762,12 +761,8 @@ static float analog2tempBed(int raw) {
    return celsius;
  #elif defined BED_USES_AD595
-    #ifdef __SAM3X8E__
-      return ((raw * ((3.3 * 100) / 1024) / OVERSAMPLENR) * TEMP_SENSOR_AD595_GAIN) + TEMP_SENSOR_AD595_OFFSET;
-    #else
      return ((raw * ((5.0 * 100.0) / 1024.0) / OVERSAMPLENR) * TEMP_SENSOR_AD595_GAIN) + TEMP_SENSOR_AD595_OFFSET;
-    #endif
+  #else
-  #else //NO BED_USES_THERMISTOR
    return 0;
  #endif
 }
@@ -802,14 +797,16 @@ static void updateTemperaturesFromRawValues() {
    }
  #endif
+  // Update printer usage
  static unsigned int second_overflow = 0;
  second_overflow += from_last_update;
-  if(second_overflow >= 1000) {
+  if (second_overflow >= 1000) {
    printer_usage_seconds++;
    second_overflow -= 1000;
  }
  last_update = temp_last_update;
-  //Reset the watchdog after we know we have a temperature measurement.
+  // Reset the watchdog after we know we have a temperature measurement.
  watchdog_reset();
  CRITICAL_SECTION_START;
@@ -823,7 +820,7 @@ static void updateTemperaturesFromRawValues() {
  // Convert raw Filament Width to millimeters
  float analog2widthFil() {
    return current_raw_filwidth / 16383.0 * 5.0;
-    //return current_raw_filwidth;
+    // return current_raw_filwidth;
  }
  // Convert raw Filament Width to a ratio
@@ -854,7 +851,7 @@ static void updateTemperaturesFromRawValues() {
 */
 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
+    // disable RUMBA JTAG in case the thermocouple extension is plugged on top of JTAG connector
    MCUCR=BIT(JTD);
    MCUCR=BIT(JTD);
  #endif
@@ -869,8 +866,8 @@ void tp_init() {
    #endif //PIDTEMP
    #ifdef PIDTEMPBED
      temp_iState_min_bed = 0.0;
-      temp_iState_max_bed = PID_INTEGRAL_DRIVE_MAX / bedKi;
+      temp_iState_max_bed = PID_BED_INTEGRAL_DRIVE_MAX / bedKi;
-    #endif //PIDTEMPBED
+    #endif // PIDTEMPBED
  }
  #if HAS_HEATER_0
@@ -911,14 +908,8 @@ void tp_init() {
    OUT_WRITE(MAX6675_SS,HIGH);
-  #endif //HEATER_0_USES_MAX6675
+  #endif // HEATER_0_USES_MAX6675
-#ifdef __SAM3X8E__
-  // Use timer0 for temperature measurement
-  // Interleave temperature interrupt with millies interrupt
-  HAL_temp_timer_start(TEMP_TIMER_NUM);
-  HAL_timer_enable_interrupt (TEMP_TIMER_NUM);
-#else
  #ifdef DIDR2
    #define ANALOG_SELECT(pin) do{ if (pin < 8) DIDR0 |= BIT(pin); else DIDR2 |= BIT(pin - 8); }while(0)
  #else
@@ -957,7 +948,6 @@ void tp_init() {
  // Interleave temperature interrupt with millies interrupt
  OCR0B = 128;
  TIMSK0 |= BIT(OCIE0B);  
-#endif
  // Wait for temperature measurement to settle
  delay(250);
@@ -1012,7 +1002,6 @@ void tp_init() {
  #endif // HOTENDS > 1
  #ifdef BED_MINTEMP
-    /* No bed MINTEMP error implemented?!? */ /*
    while(analog2tempBed(bed_minttemp_raw) < BED_MINTEMP) {
      #if HEATER_BED_RAW_LO_TEMP < HEATER_BED_RAW_HI_TEMP
        bed_minttemp_raw += OVERSAMPLENR;
@@ -1020,7 +1009,6 @@ void tp_init() {
        bed_minttemp_raw -= OVERSAMPLENR;
      #endif
    }
-    */
  #endif //BED_MINTEMP
  #ifdef BED_MAXTEMP
    while(analog2tempBed(bed_maxttemp_raw) > BED_MAXTEMP) {
@@ -1030,32 +1018,29 @@ void tp_init() {
        bed_maxttemp_raw += OVERSAMPLENR;
      #endif
    }
-  #endif //BED_MAXTEMP
+  #endif // BED_MAXTEMP
 }
-#ifdef WATCH_TEMP_PERIOD
+#ifdef THERMAL_PROTECTION_HOTENDS
  /**
   * Start Heating Sanity Check for hotends that are below
   * their target temperature by a configurable margin.
   * This is called when the temperature is set. (M104, M109)
   */
  void start_watching_heater(int e) {
-    millis_t ms = millis() + WATCH_TEMP_PERIOD;
+    if (degHotend(e) < degTargetHotend(e) - (WATCH_TEMP_INCREASE + TEMP_HYSTERESIS + 1)) {
-    if (degHotend(e) < degTargetHotend(e) - (WATCH_TEMP_INCREASE * 2)) {
      watch_target_temp[e] = degHotend(e) + WATCH_TEMP_INCREASE;
-      watch_heater_next_ms[e] = ms;
+      watch_heater_next_ms[e] = millis() + WATCH_TEMP_PERIOD * 1000;
    }
    else
      watch_heater_next_ms[e] = 0;
  }
 #endif
-#if HAS_HEATER_THERMAL_PROTECTION || HAS_BED_THERMAL_PROTECTION
+#if defined(THERMAL_PROTECTION_HOTENDS) || defined(THERMAL_PROTECTION_BED)
  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 };
    /*
        ECHO_SM(DB, "Thermal Thermal Runaway Running. Heater ID: ");
        if (heater_id < 0) ECHO_M("bed"); else ECHO_V(heater_id);
@@ -1065,7 +1050,7 @@ void tp_init() {
        ECHO_EMV(" ;  Target Temp:", target_temperature);
    */
-    int heater_index = heater_id >= 0 ? heater_id : EXTRUDERS;
+    int heater_index = heater_id >= 0 ? heater_id : HOTENDS;
    // If the target temperature changes, restart
    if (tr_target_temperature[heater_index] != target_temperature)
@@ -1097,19 +1082,11 @@ void tp_init() {
          *state = TRRunaway;
        break;
      case TRRunaway:
-        ECHO_SM(ER, MSG_THERMAL_RUNAWAY_STOP);
+        _temp_error(heater_id, PSTR(MSG_T_THERMAL_RUNAWAY), PSTR(MSG_THERMAL_RUNAWAY));
-        if (heater_id < 0) ECHO_EM(MSG_THERMAL_RUNAWAY_BED); else ECHO_EV(heater_id);
-        LCD_ALERTMESSAGEPGM(MSG_THERMAL_RUNAWAY);
-        disable_all_heaters();
-        disable_all_steppers();
-        for (;;) {
-          manage_heater();
-          lcd_update();
-        }
    }
  }
-#endif // HAS_HEATER_THERMAL_PROTECTION || HAS_BED_THERMAL_PROTECTION
+#endif // THERMAL_PROTECTION_HOTENDS || THERMAL_PROTECTION_BED
 void disable_all_heaters() {
  for (int i = 0; i < HOTENDS; i++) setTargetHotend(0, i);
@@ -1202,7 +1179,7 @@ void disable_all_heaters() {
    return max6675_temp;
  }
-#endif //HEATER_0_USES_MAX6675
+#endif // HEATER_0_USES_MAX6675
 /**
 * Stages in the ISR loop
@@ -1434,7 +1411,7 @@ ISR(TIMER0_COMPB_vect) {
        WRITE_FAN(soft_pwm_fan > 0 ? 1 : 0);
      }
      if (soft_pwm_fan < pwm_count) WRITE_FAN(0);
-    #endif //FAN_SOFT_PWM
+    #endif // FAN_SOFT_PWM
    pwm_count += BIT(SOFT_PWM_SCALE);
    pwm_count &= 0x7f;
@@ -1647,10 +1624,8 @@ ISR(TIMER0_COMPB_vect) {
      #else
        #define GEBED >=
      #endif
-      if (current_temperature_bed_raw GEBED bed_maxttemp_raw) {
+      if (current_temperature_bed_raw GEBED bed_maxttemp_raw) _temp_error(-1, PSTR(MSG_T_MAXTEMP), PSTR(MSG_ERR_MAXTEMP_BED));
-        target_temperature_bed = 0;
+      if (bed_minttemp_raw GEBED current_temperature_bed_raw) _temp_error(-1, PSTR(MSG_T_MINTEMP), PSTR(MSG_ERR_MINTEMP_BED));
-        bed_max_temp_error();
-      }
    #endif
  } // temp_count >= OVERSAMPLENR

--- a/MarlinKimbra/temperature.h
+++ b/MarlinKimbra/temperature.h
@@ -66,12 +66,13 @@ extern float current_temperature_bed;
 #ifdef PIDTEMP
  extern float Kp[HOTENDS], Ki[HOTENDS], Kd[HOTENDS];
-  #define PID_PARAM(param,e) param[e] // use macro to point to array value
+  #define PID_PARAM(param, e) param[e] // use macro to point to array value
  float scalePID_i(float i);
  float scalePID_d(float d);
  float unscalePID_i(float i);
  float unscalePID_d(float d);
 #endif
 #ifdef PIDTEMPBED
  extern float bedKp,bedKi,bedKd;
 #endif
@@ -98,19 +99,24 @@ FORCE_INLINE float degBed() { return current_temperature_bed; }
 #endif
 FORCE_INLINE float degTargetHotend(uint8_t hotend) { return target_temperature[HOTEND_ARG]; }
 FORCE_INLINE float degTargetBed() { return target_temperature_bed; }
-FORCE_INLINE void setTargetHotend(const float &celsius, uint8_t hotend) { target_temperature[HOTEND_ARG] = celsius; }
+#ifdef THERMAL_PROTECTION_HOTENDS
+  void start_watching_heater(int e=0);
+#endif
+FORCE_INLINE void setTargetHotend(const float &celsius, uint8_t hotend) {
+  target_temperature[HOTEND_ARG] = celsius;
+  #ifdef THERMAL_PROTECTION_HOTENDS
+    start_watching_heater(HOTEND_ARG);
+  #endif
+}
 FORCE_INLINE void setTargetBed(const float &celsius) { target_temperature_bed = celsius; }
 FORCE_INLINE bool isHeatingHotend(uint8_t hotend) { return target_temperature[HOTEND_ARG] > current_temperature[HOTEND_ARG]; }
 FORCE_INLINE bool isHeatingBed() { return target_temperature_bed > current_temperature_bed; }
 FORCE_INLINE bool isCoolingHotend(uint8_t hotend) { return target_temperature[HOTEND_ARG] < current_temperature[HOTEND_ARG]; }
 FORCE_INLINE bool isCoolingBed() { return target_temperature_bed < current_temperature_bed; }
 #define HOTEND_ROUTINES(NR) \
@@ -138,7 +144,6 @@ HOTEND_ROUTINES(0);
 int getHeaterPower(int heater);
 void disable_all_heaters();
-void setWatch();
 void updatePID();
 void PID_autotune(float temp, int extruder, int ncycles);
@@ -146,10 +151,6 @@ void PID_autotune(float temp, int extruder, int ncycles);
 void setExtruderAutoFanState(int pin, bool state);
 void checkExtruderAutoFans();
-#ifdef WATCH_TEMP_PERIOD
-  void start_watching_heater(int e=0);
-#endif
 FORCE_INLINE void autotempShutdown() {
  #ifdef AUTOTEMP
    if (autotemp_enabled) {

--- a/MarlinKimbra/ultralcd.cpp
+++ b/MarlinKimbra/ultralcd.cpp
@@ -7,17 +7,16 @@
 #include "stepper.h"
 #include "configuration_store.h"
-int8_t encoderDiff; /* encoderDiff is updated from interrupt context and added to encoderPosition every LCD update */
+int8_t encoderDiff; // updated from interrupt context and added to encoderPosition every LCD update
 bool encoderRateMultiplierEnabled;
 int32_t lastEncoderMovementMillis;
-int  pageShowInfo = 0;
+#if !defined(DELTA) && !defined(Z_SAFE_HOMING) && Z_HOME_DIR < 0
-boolean ChangeScreen = false;
+  int  pageShowInfo = 0;
-void set_pageShowInfo(int value){ pageShowInfo = value; }
+  void set_pageShowInfo(int value){ pageShowInfo = value; }
-void set_ChangeScreen(boolean state) { ChangeScreen = state; }
+#endif
-/* Configuration settings */
 int plaPreheatHotendTemp;
 int plaPreheatHPBTemp;
 int plaPreheatFanSpeed;
@@ -38,9 +37,7 @@ int gumPreheatFanSpeed;
  millis_t print_millis = 0;
 #endif
-/* !Configuration settings */
+// Function pointer to menu functions.
-//Function pointer to menu functions.
 typedef void (*menuFunc_t)();
 uint8_t lcd_status_message_level;
@@ -48,7 +45,7 @@ char lcd_status_message[3*LCD_WIDTH+1] = WELCOME_MSG; // worst case is kana with
 #ifdef DOGLCD
  #include "dogm_lcd_implementation.h"
-  #define LCD_Printpos(x, y) u8g.setPrintPos(x, y)
+  #define LCD_Printpos(x, y)  u8g.setPrintPos(x + 5, (y + 1) * (DOG_CHAR_HEIGHT + 2))
 #else
  #include "ultralcd_implementation_hitachi_HD44780.h"
  #define LCD_Printpos(x, y)  lcd.setCursor(x, y)
@@ -84,6 +81,9 @@ static void lcd_status_screen();
  #ifdef DELTA
    static void lcd_delta_calibrate_menu();
+  #elif !defined(DELTA) && !defined(Z_SAFE_HOMING) && Z_HOME_DIR < 0
+    static void lcd_level_bed();
+    static void config_lcd_level_bed();
  #endif // DELTA
  /* Different types of actions that can be used in menu items. */
@@ -239,11 +239,11 @@ static void lcd_status_screen();
      } } while(0)
  /** Used variables to keep track of the menu */
-  #ifndef REPRAPWORLD_KEYPAD
+  volatile uint8_t buttons;  //the last checked buttons in a bit array.
-    volatile uint8_t buttons; // Bits of the pressed buttons.
+  #ifdef REPRAPWORLD_KEYPAD
-  #else
+    volatile uint8_t buttons_reprapworld_keypad; // to store the keypad shift register values
-    volatile uint8_t buttons_reprapworld_keypad; // The reprapworld_keypad shift register values
  #endif
  #ifdef LCD_HAS_SLOW_BUTTONS
    volatile uint8_t slow_buttons; // Bits of the pressed buttons.
  #endif
@@ -279,7 +279,7 @@ float raw_Ki, raw_Kd;
 /**
 * 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) {
+static void lcd_goto_menu(menuFunc_t menu, const bool feedback = false, const uint32_t encoder = 0) {
  if (currentMenu != menu) {
    currentMenu = menu;
    #ifdef NEWPANEL
@@ -375,7 +375,7 @@ static void lcd_status_screen() {
          currentMenu == lcd_status_screen
        #endif
      );
-      #ifdef FILAMENT_LCD_DISPLAY
+      #if HAS_LCD_FILAMENT_SENSOR || HAS_LCD_POWER_SENSOR
        previous_lcd_status_ms = millis();  // get status message to show up for a while
      #endif
    }
@@ -412,18 +412,20 @@ static void lcd_status_screen() {
 static void lcd_return_to_status() { lcd_goto_menu(lcd_status_screen); }
-static void lcd_sdcard_pause() { card.pauseSDPrint(); }
+#ifdef SDSUPPORT
+  static void lcd_sdcard_pause() { card.pauseSDPrint(); }
-static void lcd_sdcard_resume() { card.startFileprint(); }
+  static void lcd_sdcard_resume() { card.startFileprint(); }
-static void lcd_sdcard_stop() {
+  static void lcd_sdcard_stop() {
    quickStop();
    card.sdprinting = false;
    card.closeFile();
    autotempShutdown();
    cancel_heatup = true;
    lcd_setstatus(MSG_PRINT_ABORTED, true);
-}
+  }
+#endif
 /**
 *
@@ -583,12 +585,11 @@ static void lcd_tune_menu() {
 void _lcd_preheat(int endnum, const float temph, const float tempb, const int fan) {
  if (temph > 0) setTargetHotend(temph, endnum);
+  #if TEMP_SENSOR_BED != 0
    setTargetBed(tempb);
+  #endif
  fanSpeed = fan;
  lcd_return_to_status();
-  #ifdef WATCH_TEMP_PERIOD
-    if (endnum >= 0) start_watching_heater(endnum);
-  #endif
 }
 void lcd_preheat_pla0() { _lcd_preheat(0, plaPreheatHotendTemp, plaPreheatHPBTemp, plaPreheatFanSpeed); }
 void lcd_preheat_abs0() { _lcd_preheat(0, absPreheatHotendTemp, absPreheatHPBTemp, absPreheatFanSpeed); }
@@ -678,7 +679,7 @@ void lcd_preheat_gum0() { _lcd_preheat(0, gumPreheatHotendTemp, gumPreheatHPBTem
    }
    static void lcd_preheat_gum_menu() {
-      START_MENU();
+      START_MENU(lcd_prepare_menu);
      MENU_ITEM(back, MSG_PREPARE, lcd_prepare_menu);
      MENU_ITEM(function, MSG_PREHEAT_GUM " 1", lcd_preheat_gum0);
      #if TEMP_SENSOR_1 != 0 //2 extruder preheat
@@ -700,93 +701,11 @@ void lcd_preheat_gum0() { _lcd_preheat(0, gumPreheatHotendTemp, gumPreheatHPBTem
 #endif //more than one extruder present
 void lcd_cooldown() {
-  setTargetHotend0(0);
+  disable_all_heaters();
-  setTargetHotend1(0);
-  setTargetHotend2(0);
-  setTargetHotend3(0);
-  setTargetBed(0);
  fanSpeed = 0;
  lcd_return_to_status();
 }
-void config_lcd_level_bed() {
-	setTargetHotend(0,0);
-	ECHO_EM("Leveling...");	
-	currentMenu = lcd_level_bed;
-	enqueuecommands_P(PSTR("G28 M"));
-	pageShowInfo = 0;
-}
-void lcd_level_bed() {
-  if(ChangeScreen) {
-    lcd_implementation_clear;
-    switch(pageShowInfo) {
-      case 0:
-        {
-          LCD_Printpos(0, 1);
-          lcd_printPGM(PSTR(MSG_LP_INTRO));
-          currentMenu = lcd_level_bed;
-          ChangeScreen=false;
-        }
-      break;
-      case 1:
-        {
-          LCD_Printpos(0, 1);
-          lcd_printPGM(PSTR(MSG_LP_1));
-          currentMenu = lcd_level_bed;
-          ChangeScreen=false;
-        }
-      break;
-      case 2:
-        {
-          LCD_Printpos(0, 1);
-          lcd_printPGM(PSTR(MSG_LP_2));
-              currentMenu = lcd_level_bed;
-           ChangeScreen=false;
-        }
-      break;
-      case 3:
-        {  
-          LCD_Printpos(0, 1);
-          lcd_printPGM(PSTR(MSG_LP_3));
-          currentMenu = lcd_level_bed;
-          ChangeScreen=false;
-        }
-      break;        
-      case 4:
-        {
-          LCD_Printpos(0, 1);
-          lcd_printPGM(PSTR(MSG_LP_4));
-          currentMenu = lcd_level_bed;
-          ChangeScreen=false; 
-        }
-      break;
-      case 5:
-        {
-          LCD_Printpos(0, 1);
-          lcd_printPGM(PSTR(MSG_LP_5));
-          currentMenu = lcd_level_bed;
-          ChangeScreen=false;
-        }
-      break;
-      case 6:
-        {
-          LCD_Printpos(2, 2);
-          lcd_printPGM(PSTR(MSG_LP_6));
-          ChangeScreen=false;
-          delay(1200);
-          encoderPosition = 0;
-          lcd_implementation_clear();
-          currentMenu = lcd_status_screen;
-          lcd_status_screen();
-          pageShowInfo=0;
-        }
-      break;
-    }
-  }
-}
 /**
 *
 * "Prepare" submenu
@@ -805,8 +724,15 @@ static void lcd_prepare_menu() {
  // Auto Home
  //
  MENU_ITEM(gcode, MSG_AUTO_HOME, PSTR("G28"));
-  #ifndef DELTA
-    MENU_ITEM(function, MSG_BED_SETTING, config_lcd_level_bed);
+  //
+  // 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(DELTA) && !defined(Z_SAFE_HOMING) && Z_HOME_DIR < 0
+    MENU_ITEM(submenu, MSG_MBL_SETTING, config_lcd_level_bed);
  #endif
  //
@@ -826,16 +752,6 @@ static void lcd_prepare_menu() {
    }
  #endif
-  //
-  // 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(DELTA)
-    MENU_ITEM(function, MSG_BED_SETTING, config_lcd_level_bed);
-  #endif
  //
  // Move Axis
  //
@@ -871,7 +787,7 @@ static void lcd_prepare_menu() {
  //
  // Easy Load
  //
-  #if defined(EASY_LOAD) 
+  #ifdef EASY_LOAD
    MENU_ITEM(function, MSG_E_BOWDEN_LENGTH, lcd_easy_load);
    MENU_ITEM(function, MSG_R_BOWDEN_LENGTH, lcd_easy_unload);
    MENU_ITEM(function, MSG_PURGE_XMM, lcd_purge);
@@ -1073,10 +989,17 @@ static void lcd_control_menu() {
 * "Control" > "Temperature" submenu
 *
 */
 static void lcd_control_temperature_menu() {
  START_MENU(lcd_control_menu);
+  //
+  // ^ Control
+  //
  MENU_ITEM(back, MSG_CONTROL, lcd_control_menu);
+  //
+  // Nozzle, Nozzle 2, Nozzle 3, Nozzle 4
+  //
  #if TEMP_SENSOR_0 != 0
    MENU_MULTIPLIER_ITEM_EDIT(int3, MSG_NOZZLE, &target_temperature[0], 0, HEATER_0_MAXTEMP + LCD_MAX_TEMP_OFFSET);
  #endif
@@ -1095,19 +1018,35 @@ static void lcd_control_temperature_menu() {
      #endif //HOTENDS > 3
    #endif //HOTENDS > 2
  #endif //HOTENDS > 1
+  //
+  // Bed
+  //
  #if TEMP_SENSOR_BED != 0
    MENU_MULTIPLIER_ITEM_EDIT(int3, MSG_BED, &target_temperature_bed, 0, BED_MAXTEMP + LCD_MAX_TEMP_OFFSET);
  #endif
+  //
+  // Fan Speed
+  //
  MENU_MULTIPLIER_ITEM_EDIT(int3, MSG_FAN_SPEED, &fanSpeed, 0, 255);
  #ifdef IDLE_OOZING_PREVENT
    MENU_ITEM_EDIT(bool, MSG_IDLEOOZING, &idleoozing_enabled);
  #endif
+  //
+  // Autotemp, Min, Max, Fact
+  //
  #if defined(AUTOTEMP) && (TEMP_SENSOR_0 != 0)
    MENU_ITEM_EDIT(bool, MSG_AUTOTEMP, &autotemp_enabled);
    MENU_ITEM_EDIT(float3, MSG_MIN, &autotemp_min, 0, HEATER_0_MAXTEMP + LCD_MAX_TEMP_OFFSET);
    MENU_ITEM_EDIT(float3, MSG_MAX, &autotemp_max, 0, HEATER_0_MAXTEMP + LCD_MAX_TEMP_OFFSET);
    MENU_ITEM_EDIT(float32, MSG_FACTOR, &autotemp_factor, 0.0, 1.0);
  #endif
+  //
+  // PID-P, PID-I, PID-D
+  //
  #ifdef PIDTEMP
    // set up temp variables - undo the default scaling
    raw_Ki = unscalePID_i(PID_PARAM(Ki,0));
@@ -1144,8 +1083,20 @@ static void lcd_control_temperature_menu() {
      #endif //HOTENDS > 2
    #endif //HOTENDS > 1
  #endif //PIDTEMP
+  //
+  // Preheat PLA conf
+  //
  MENU_ITEM(submenu, MSG_PREHEAT_PLA_SETTINGS, lcd_control_temperature_preheat_pla_settings_menu);
+  //
+  // Preheat ABS conf
+  //
  MENU_ITEM(submenu, MSG_PREHEAT_ABS_SETTINGS, lcd_control_temperature_preheat_abs_settings_menu);
+  //
+  // Preheat GUM conf
+  //
  MENU_ITEM(submenu, MSG_PREHEAT_GUM_SETTINGS, lcd_control_temperature_preheat_gum_settings_menu);
  END_MENU();
 }
@@ -1155,7 +1106,6 @@ static void lcd_control_temperature_menu() {
 * "Temperature" > "Preheat PLA conf" submenu
 *
 */
 static void lcd_control_temperature_preheat_pla_settings_menu() {
  START_MENU(lcd_control_temperature_menu);
  MENU_ITEM(back, MSG_TEMPERATURE, lcd_control_temperature_menu);
@@ -1177,7 +1127,6 @@ static void lcd_control_temperature_preheat_pla_settings_menu() {
 * "Temperature" > "Preheat ABS conf" submenu
 *
 */
 static void lcd_control_temperature_preheat_abs_settings_menu() {
  START_MENU(lcd_control_temperature_menu);
  MENU_ITEM(back, MSG_TEMPERATURE, lcd_control_temperature_menu);
@@ -1220,7 +1169,6 @@ static void lcd_control_temperature_preheat_gum_settings_menu() {
 * "Control" > "Motion" submenu
 *
 */
 static void lcd_control_motion_menu() {
  START_MENU(lcd_control_menu);
  MENU_ITEM(back, MSG_CONTROL, lcd_control_menu);
@@ -1271,7 +1219,6 @@ static void lcd_control_motion_menu() {
 * "Control" > "Filament" submenu
 *
 */
 static void lcd_control_volumetric_menu() {
  START_MENU(lcd_control_menu);
  MENU_ITEM(back, MSG_CONTROL, lcd_control_menu);
@@ -1299,17 +1246,27 @@ static void lcd_control_volumetric_menu() {
 * "Control" > "Contrast" submenu
 *
 */
 #ifdef HAS_LCD_CONTRAST
  static void lcd_set_contrast() {
    if (encoderPosition != 0) {
+      #ifdef U8GLIB_LM6059_AF
+        lcd_contrast += encoderPosition;
+        lcd_contrast &= 0xFF;
+      #else
        lcd_contrast -= encoderPosition;
        lcd_contrast &= 0x3F;
+      #endif
      encoderPosition = 0;
      lcdDrawUpdate = 1;
      u8g.setContrast(lcd_contrast);
    }
-    if (lcdDrawUpdate) lcd_implementation_drawedit(PSTR(MSG_CONTRAST), itostr2(lcd_contrast));
+    if (lcdDrawUpdate) {
+      #ifdef U8GLIB_LM6059_AF
+        lcd_implementation_drawedit(PSTR(MSG_CONTRAST), itostr3(lcd_contrast));
+      #else
+        lcd_implementation_drawedit(PSTR(MSG_CONTRAST), itostr2(lcd_contrast));
+      #endif
+    }
    if (LCD_CLICKED) lcd_goto_menu(lcd_control_menu);
  }
 #endif // HAS_LCD_CONTRAST
@@ -1319,7 +1276,6 @@ static void lcd_control_volumetric_menu() {
 * "Control" > "Retract" submenu
 *
 */
 #ifdef FWRETRACT
  static void lcd_control_retract_menu() {
    START_MENU(lcd_control_menu);
@@ -1340,25 +1296,25 @@ static void lcd_control_volumetric_menu() {
  }
 #endif // FWRETRACT
-#if SDCARDDETECT == -1
+#ifdef SDSUPPORT
+  #if SDCARDDETECT == -1
    static void lcd_sd_refresh() {
      card.initsd();
      currentMenuViewOffset = 0;
    }
-#endif
+  #endif
-static void lcd_sd_updir() {
+  static void lcd_sd_updir() {
    card.updir();
    currentMenuViewOffset = 0;
-}
+  }
-/**
+  /**
  *
  * "Print from SD" submenu
  *
  */
+  void lcd_sdcard_menu() {
-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();
    START_MENU(lcd_main_menu);
@@ -1391,7 +1347,8 @@ void lcd_sdcard_menu() {
      }
    }
    END_MENU();
-}
+  }
+#endif // SDSUPPORT
 /**
 *
@@ -1483,7 +1440,7 @@ menu_edit_type(unsigned long, long5, ftostr5, 0.01)
  static void reprapworld_keypad_move_home() {
    enqueuecommands_P((PSTR("G28"))); // move all axis home
  }
-#endif //REPRAPWORLD_KEYPAD
+#endif // REPRAPWORLD_KEYPAD
 /**
@@ -1503,7 +1460,7 @@ void lcd_quick_feedback() {
      #define LCD_FEEDBACK_FREQUENCY_DURATION_MS (1000/6)
    #endif    
    lcd_buzz(LCD_FEEDBACK_FREQUENCY_DURATION_MS, LCD_FEEDBACK_FREQUENCY_HZ);
-  #elif defined(BEEPER) && BEEPER > -1
+  #elif defined(BEEPER) && BEEPER >= 0
    #ifndef LCD_FEEDBACK_FREQUENCY_HZ
      #define LCD_FEEDBACK_FREQUENCY_HZ 5000
    #endif
@@ -1528,7 +1485,9 @@ 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) {
+#ifdef SDSUPPORT
+  static void menu_action_sdfile(const char* filename, char* longFilename) {
    char cmd[30];
    char* c;
    sprintf_P(cmd, PSTR("M23 %s"), filename);
@@ -1536,11 +1495,13 @@ static void menu_action_sdfile(const char* filename, char* longFilename) {
    enqueuecommand(cmd);
    enqueuecommands_P(PSTR("M24"));
    lcd_return_to_status();
-}
+  }
-static void menu_action_sddirectory(const char* filename, char* longFilename) {
+  static void menu_action_sddirectory(const char* filename, char* longFilename) {
    card.chdir(filename);
    encoderPosition = 0;
-}
+  }
+#endif // SDSUPPORT
 static void menu_action_setting_edit_bool(const char* pstr, bool* ptr) { *ptr = !(*ptr); }
 static void menu_action_setting_edit_callback_bool(const char* pstr, bool* ptr, menuFunc_t callback) {
  menu_action_setting_edit_bool(pstr, ptr);
@@ -1749,11 +1710,19 @@ void lcd_update() {
    #endif
    #ifdef ULTIPANEL
-      if (currentMenu != lcd_status_screen && millis() > return_to_status_ms) {
+      // Return to Status Screen after a timeout
+      if (currentMenu != lcd_status_screen &&
+        #if !defined(DELTA) && !defined(Z_SAFE_HOMING) && Z_HOME_DIR < 0
+          currentMenu != lcd_level_bed &&
+        #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--;
@@ -1775,7 +1744,7 @@ void lcd_finishstatus(bool persist=false) {
  #endif
  lcdDrawUpdate = 2;
-  #ifdef FILAMENT_LCD_DISPLAY
+  #if HAS_LCD_FILAMENT_SENSOR || HAS_LCD_POWER_SENSOR
    previous_lcd_status_ms = millis();  //get status message to show up for a while
  #endif
 }
@@ -1931,13 +1900,12 @@ void lcd_reset_alert_level() { lcd_status_message_level = 0; }
  void lcd_buzz(long duration, uint16_t freq) {
    if (freq > 0) {
-      #if BEEPER > 0
+      #ifdef LCD_USE_I2C_BUZZER
+        lcd.buzz(duration, freq);
+      #elif defined(BEEPER) && BEEPER >= 0
        SET_OUTPUT(BEEPER);
-        tone(BEEPER, freq);
+        tone(BEEPER, freq, duration);
        delay(duration);
-        noTone(BEEPER);
-      #elif defined(LCD_USE_I2C_BUZZER)
-        lcd.buzz(duration, freq);
      #else
        delay(duration);
      #endif
@@ -2226,5 +2194,66 @@ char *ftostr52(const float &x) {
  return conv;
 }
+#if !defined(DELTA) && !defined(Z_SAFE_HOMING) && Z_HOME_DIR < 0
+  static void lcd_level_bed() {
+    switch(pageShowInfo) {
+      case 0:
+        {
+          LCD_Printpos(0, 0); lcd_printPGM(PSTR(MSG_MBL_INTRO));
+          LCD_Printpos(0, 1); lcd_printPGM(PSTR(MSG_MBL_BUTTON));
+        }
+      break;
+      case 1:
+        {
+          LCD_Printpos(0, 0); lcd_printPGM(PSTR(MSG_MBL_1));
+          LCD_Printpos(0, 1); lcd_printPGM(PSTR(MSG_MBL_BUTTON));
+        }
+      break;
+      case 2:
+        {
+          LCD_Printpos(0, 0); lcd_printPGM(PSTR(MSG_MBL_2));
+          LCD_Printpos(0, 1); lcd_printPGM(PSTR(MSG_MBL_BUTTON));
+        }
+      break;
+      case 3:
+        {
+          LCD_Printpos(0, 0); lcd_printPGM(PSTR(MSG_MBL_3));
+          LCD_Printpos(0, 1); lcd_printPGM(PSTR(MSG_MBL_BUTTON));
+        }
+      break;
+      case 4:
+        {
+          LCD_Printpos(0, 0); lcd_printPGM(PSTR(MSG_MBL_4));
+          LCD_Printpos(0, 1); lcd_printPGM(PSTR(MSG_MBL_BUTTON));
+        }
+      break;
+      case 5:
+        {
+          LCD_Printpos(0, 0); lcd_printPGM(PSTR(MSG_MBL_5));
+          LCD_Printpos(0, 1); lcd_printPGM(PSTR(MSG_MBL_BUTTON));
+        }
+      break;
+      case 6:
+        {
+          LCD_Printpos(0, 0); lcd_printPGM(PSTR(MSG_MBL_6));
+          LCD_Printpos(0, 1); lcd_printPGM(PSTR("                  "));
+          delay(5000);
+          enqueuecommands_P(PSTR("G28"));
+          lcd_return_to_status();
+        }
+      break;
+    }
+  }
+  static void config_lcd_level_bed() {
+    ECHO_EM(MSG_MBL_SETTING);
+    enqueuecommands_P(PSTR("G28 M"));
+    pageShowInfo = 0;
+    lcd_goto_menu(lcd_level_bed);
+  }
+#endif
 #endif //ULTRA_LCD
--- a/MarlinKimbra/ultralcd.h
+++ b/MarlinKimbra/ultralcd.h
@@ -24,10 +24,9 @@
    void lcd_setcontrast(uint8_t value);
  #endif
+  #if !defined(DELTA) && !defined(Z_SAFE_HOMING) && Z_HOME_DIR < 0
    void set_pageShowInfo(int value);
-  void set_ChangeScreen(boolean state);
+  #endif
-  void config_lcd_level_bed(void);
-  void lcd_level_bed(void);
  #define LCD_MESSAGEPGM(x) lcd_setstatuspgm(PSTR(x))
  #define LCD_ALERTMESSAGEPGM(x) lcd_setalertstatuspgm(PSTR(x))
@@ -57,7 +56,7 @@
  extern bool cancel_heatup;
-  #if (HAS_FILAMENT_SENSOR && defined(FILAMENT_LCD_DISPLAY)) || (HAS_POWER_CONSUMPTION_SENSOR && defined(POWER_CONSUMPTION_LCD_DISPLAY))
+  #if HAS_LCD_FILAMENT_SENSOR || HAS_LCD_POWER_SENSOR
    extern millis_t previous_lcd_status_ms;
  #endif
@@ -118,7 +117,7 @@
  FORCE_INLINE void lcd_setstatuspgm(const char* message, const uint8_t level=0) {}
  FORCE_INLINE void lcd_buttons_update() {}
  FORCE_INLINE void lcd_reset_alert_level() {}
-  FORCE_INLINE void lcd_buzz(long duration,uint16_t freq) {}
+  FORCE_INLINE void lcd_buzz(long duration, uint16_t freq) {}
  FORCE_INLINE bool lcd_detected(void) { return true; }
  #define LCD_MESSAGEPGM(x) do{}while(0)

--- a/MarlinKimbra/ultralcd_implementation_hitachi_HD44780.h
+++ b/MarlinKimbra/ultralcd_implementation_hitachi_HD44780.h
@@ -6,11 +6,11 @@
 * When selecting the Russian language, a slightly different LCD implementation is used to handle UTF8 characters.
 **/
-#ifndef REPRAPWORLD_KEYPAD
+//#ifndef REPRAPWORLD_KEYPAD
-  extern volatile uint8_t buttons;  //the last checked buttons in a bit array.
+//  extern volatile uint8_t buttons;  //the last checked buttons in a bit array.
-#else
+//#else
-  extern volatile uint16_t buttons;  //an extended version of the last checked buttons in a bit array.
+  extern volatile uint8_t buttons;  //an extended version of the last checked buttons in a bit array.
-#endif
+//#endif
 ////////////////////////////////////
 // Setup button and encode mappings for each panel (into 'buttons' variable
@@ -82,13 +82,13 @@
    #define BLEN_REPRAPWORLD_KEYPAD_F3 0
    #define BLEN_REPRAPWORLD_KEYPAD_F2 1
    #define BLEN_REPRAPWORLD_KEYPAD_F1 2
-    #define BLEN_REPRAPWORLD_KEYPAD_UP 3
+    #define BLEN_REPRAPWORLD_KEYPAD_UP 6
    #define BLEN_REPRAPWORLD_KEYPAD_RIGHT 4
    #define BLEN_REPRAPWORLD_KEYPAD_MIDDLE 5
-    #define BLEN_REPRAPWORLD_KEYPAD_DOWN 6
+    #define BLEN_REPRAPWORLD_KEYPAD_DOWN 3
    #define BLEN_REPRAPWORLD_KEYPAD_LEFT 7
-    #define REPRAPWORLD_BTN_OFFSET 3 // bit offset into buttons for shift register values
+    #define REPRAPWORLD_BTN_OFFSET 0 // bit offset into buttons for shift register values
    #define EN_REPRAPWORLD_KEYPAD_F3 BIT((BLEN_REPRAPWORLD_KEYPAD_F3+REPRAPWORLD_BTN_OFFSET))
    #define EN_REPRAPWORLD_KEYPAD_F2 BIT((BLEN_REPRAPWORLD_KEYPAD_F2+REPRAPWORLD_BTN_OFFSET))
@@ -99,10 +99,10 @@
    #define EN_REPRAPWORLD_KEYPAD_DOWN BIT((BLEN_REPRAPWORLD_KEYPAD_DOWN+REPRAPWORLD_BTN_OFFSET))
    #define EN_REPRAPWORLD_KEYPAD_LEFT BIT((BLEN_REPRAPWORLD_KEYPAD_LEFT+REPRAPWORLD_BTN_OFFSET))
-    #define LCD_CLICKED ((buttons&EN_C) || (buttons&EN_REPRAPWORLD_KEYPAD_F1))
+    //#define LCD_CLICKED ((buttons&EN_C) || (buttons&EN_REPRAPWORLD_KEYPAD_F1))
-    #define REPRAPWORLD_KEYPAD_MOVE_Y_DOWN (buttons&EN_REPRAPWORLD_KEYPAD_DOWN)
+    //#define REPRAPWORLD_KEYPAD_MOVE_Y_DOWN (buttons&EN_REPRAPWORLD_KEYPAD_DOWN)
-    #define REPRAPWORLD_KEYPAD_MOVE_Y_UP (buttons&EN_REPRAPWORLD_KEYPAD_UP)
+    //#define REPRAPWORLD_KEYPAD_MOVE_Y_UP (buttons&EN_REPRAPWORLD_KEYPAD_UP)
-    #define REPRAPWORLD_KEYPAD_MOVE_HOME (buttons&EN_REPRAPWORLD_KEYPAD_MIDDLE)
+    //#define REPRAPWORLD_KEYPAD_MOVE_HOME (buttons&EN_REPRAPWORLD_KEYPAD_MIDDLE)
 #elif defined(NEWPANEL)
  #define LCD_CLICKED (buttons&EN_C)
@@ -378,34 +378,34 @@ static void lcd_implementation_init(
  #endif
 ) {
-#if defined(LCD_I2C_TYPE_PCF8575)
+  #if defined(LCD_I2C_TYPE_PCF8575)
    lcd.begin(LCD_WIDTH, LCD_HEIGHT);
    #ifdef LCD_I2C_PIN_BL
-    lcd.setBacklightPin(LCD_I2C_PIN_BL,POSITIVE);
+      lcd.setBacklightPin(LCD_I2C_PIN_BL, POSITIVE);
      lcd.setBacklight(HIGH);
    #endif
-#elif defined(LCD_I2C_TYPE_MCP23017)
+  #elif defined(LCD_I2C_TYPE_MCP23017)
    lcd.setMCPType(LTI_TYPE_MCP23017);
    lcd.begin(LCD_WIDTH, LCD_HEIGHT);
    lcd.setBacklight(0); //set all the LEDs off to begin with
-#elif defined(LCD_I2C_TYPE_MCP23008)
+  #elif defined(LCD_I2C_TYPE_MCP23008)
    lcd.setMCPType(LTI_TYPE_MCP23008);
    lcd.begin(LCD_WIDTH, LCD_HEIGHT);
-#elif defined(LCD_I2C_TYPE_PCA8574)
+  #elif defined(LCD_I2C_TYPE_PCA8574)
    lcd.init();
    lcd.backlight();
-#else
+  #else
    #if (LCD_PINS_RS != -1) && (LCD_PINS_ENABLE != -1)
      // required for RAMPS-FD, but does no harm for other targets
      SET_OUTPUT(LCD_PINS_RS);
      SET_OUTPUT(LCD_PINS_ENABLE);
 	  #endif
    lcd.begin(LCD_WIDTH, LCD_HEIGHT);
-#endif
+  #endif
  lcd_set_custom_characters(
    #ifdef LCD_PROGRESS_BAR
@@ -416,126 +416,120 @@ static void lcd_implementation_init(
  lcd.clear();
 }
-static void lcd_implementation_clear() {
+static void lcd_implementation_clear() { lcd.clear(); }
-  lcd.clear();
-}
 /* Arduino < 1.0.0 is missing a function to print PROGMEM strings, so we need to implement our own */
 char lcd_printPGM(const char* str) {
-  char c;
+  char c, n = 0;
-  char n = 0;
+  while ((c = pgm_read_byte(str++))) n += charset_mapper(c);
-  while((c = pgm_read_byte(str++))) {
-      n += charset_mapper(c);
-  }
  return n;
 }
 char lcd_print(char* str) {
-  char c, n = 0;;
+  char c, n = 0;
  unsigned char i = 0;
-  while((c = str[i++])) {
+  while ((c = str[i++])) n += charset_mapper(c);
-      n += charset_mapper(c);
-  }
  return n;
 }
-unsigned lcd_print(char c) {
+unsigned lcd_print(char c) { return charset_mapper(c); }
-    return charset_mapper(c);
-}
 /*
 Possible status screens:
-16x2   |0123456789012345|
+16x2   |000/000 B000/000|
-       |000/000 B000/000|
+       |0123456789012345|
-       |Status line.....|
-16x4   |0123456789012345|
+16x4   |000/000 B000/000|
-       |000/000 B000/000|
+       |SD100%  Z000.00 |
-       |SD100%    Z000.0|
       |F100%     T--:--|
-       |Status line.....|
+       |0123456789012345|
-20x2   |01234567890123456789|
+20x2   |T000/000D B000/000D |
-       |T000/000D B000/000D |
+       |01234567890123456789|
-       |Status line.........|
-20x4   |01234567890123456789|
+20x4   |T000/000D B000/000D |
-       |T000/000D B000/000D |
+       |X000  Y000  Z000.00 |
-       |X000  Y000   Z000.00|
       |F100%  SD100% T--:--|
-       |Status line.........|
+       |01234567890123456789|
-20x4   |01234567890123456789|
+20x4   |T000/000D B000/000D |
-       |T000/000D B000/000D |
+       |T000/000D   Z000.00 |
-       |T000/000D     Z000.0|
       |F100%  SD100% T--:--|
-       |Status line.........|
+       |01234567890123456789|
 */
 static void lcd_implementation_status_screen() {
-  int tHotend = int(degHotend(0) + 0.5);
-  int tTarget = int(degTargetHotend(0) + 0.5);
-  #if LCD_WIDTH < 20
+  #define LCD_TEMP_ONLY(T1,T2) \
+    lcd.print(itostr3(T1 + 0.5)); \
+    lcd.print('/'); \
+    lcd.print(itostr3left(T2 + 0.5))
+  #define LCD_TEMP(T1,T2,PREFIX) \
+    lcd.print(PREFIX); \
+    LCD_TEMP_ONLY(T1,T2); \
+    lcd_printPGM(PSTR(LCD_STR_DEGREE " ")); \
+    if (T2 < 10) lcd.print(' ')
+  //
+  // Line 1
+  //
  lcd.setCursor(0, 0);
-    lcd.print(itostr3(tHotend));
-    lcd.print('/');
-    lcd.print(itostr3left(tTarget));
+  #if LCD_WIDTH < 20
+    //
+    // Hotend 0 Temperature
+    //
+    LCD_TEMP_ONLY(degHotend(0), degTargetHotend(0));
+    //
+    // Hotend 1 or Bed Temperature
+    //
    #if HOTENDS > 1 || TEMP_SENSOR_BED != 0
      // If we have an 2nd extruder or heated bed, show that in the top right corner
      lcd.setCursor(8, 0);
      #if HOTENDS > 1
-        tHotend = int(degHotend(1) + 0.5);
-        tTarget = int(degTargetHotend(1) + 0.5);
        lcd.print(LCD_STR_THERMOMETER[0]);
+        LCD_TEMP_ONLY(degHotend(1), degTargetHotend(1));
      #else // Heated bed
-        tHotend = int(degBed() + 0.5);
-        tTarget = int(degTargetBed() + 0.5);
        lcd.print(LCD_STR_BEDTEMP[0]);
+        LCD_TEMP_ONLY(degBed(), degTargetBed());
      #endif
-      lcd.print(itostr3(tHotend));
-      lcd.print('/');
-      lcd.print(itostr3left(tTarget));
    #endif // HOTENDS > 1 || TEMP_SENSOR_BED != 0
-  #else // LCD_WIDTH > 19
+  #else // LCD_WIDTH >= 20
-    lcd.setCursor(0, 0);
+    //
-    lcd.print(LCD_STR_THERMOMETER[0]);
+    // Hotend 0 Temperature
-    lcd.print(itostr3(tHotend));
+    //
-    lcd.print('/');
+    LCD_TEMP(degHotend(0), degTargetHotend(0), LCD_STR_THERMOMETER[0]);
-    lcd.print(itostr3left(tTarget));
-    lcd_printPGM(PSTR(LCD_STR_DEGREE " "));
-    if (tTarget < 10) lcd.print(' ');
+    //
+    // Hotend 1 or Bed Temperature
+    //
    #if HOTENDS > 1 || TEMP_SENSOR_BED != 0
-      // If we have an 2nd extruder or heated bed, show that in the top right corner
      lcd.setCursor(10, 0);
      #if HOTENDS > 1
-        tHotend = int(degHotend(1) + 0.5);
+        LCD_TEMP(degHotend(1), degTargetHotend(1), LCD_STR_THERMOMETER[0]);
-        tTarget = int(degTargetHotend(1) + 0.5);
+      #else
-        lcd.print(LCD_STR_THERMOMETER[0]);
+        LCD_TEMP(degBed(), degTargetBed(), LCD_STR_BEDTEMP[0]);
-      #else // Heated bed
-        tHotend = int(degBed() + 0.5);
-        tTarget = int(degTargetBed() + 0.5);
-        lcd.print(LCD_STR_BEDTEMP[0]);
      #endif
-      lcd.print(itostr3(tHotend));
-      lcd.print('/');
-      lcd.print(itostr3left(tTarget));
-      lcd_printPGM(PSTR(LCD_STR_DEGREE " "));
-      if (tTarget < 10) lcd.print(' ');
    #endif // HOTENDS > 1 || TEMP_SENSOR_BED != 0
-  #endif // LCD_WIDTH > 19
+  #endif // LCD_WIDTH >= 20
+  //
+  // Line 2
+  //
  #if LCD_HEIGHT > 2
-    // Lines 2 for 4 line LCD
    #if LCD_WIDTH < 20
      #ifdef SDSUPPORT
        lcd.setCursor(0, 2);
        lcd_printPGM(PSTR("SD"));
@@ -546,22 +540,19 @@ static void lcd_implementation_status_screen() {
        lcd.print('%');
      #endif // SDSUPPORT
-    #else // LCD_WIDTH > 19
+    #else // LCD_WIDTH >= 20
+      lcd.setCursor(0, 1);
      #if HOTENDS > 1 && TEMP_SENSOR_BED != 0
-        // If we both have a 2nd extruder and a heated bed, show the heated bed temp on the 2nd line on the left, as the first line is filled with extruder temps
-        tHotend = int(degBed() + 0.5);
-        tTarget = int(degTargetBed() + 0.5);
-        lcd.setCursor(0, 1);
+        // If we both have a 2nd extruder and a heated bed,
-        lcd.print(LCD_STR_BEDTEMP[0]);
+        // show the heated bed temp on the left,
-        lcd.print(itostr3(tHotend));
+        // since the first line is filled with extruder temps
-        lcd.print('/');
+        LCD_TEMP(degBed(), degTargetBed(), LCD_STR_BEDTEMP[0]);
-        lcd.print(itostr3left(tTarget));
-        lcd_printPGM(PSTR(LCD_STR_DEGREE " "));
-        if (tTarget < 10) lcd.print(' ');
      #else
-        lcd.setCursor(0,1);
        #ifdef DELTA
          lcd.print('X');
          lcd.print(ftostr30(current_position[X_AXIS]));
@@ -575,7 +566,7 @@ static void lcd_implementation_status_screen() {
        #endif // DELTA
      #endif // HOTENDS > 1 || TEMP_SENSOR_BED != 0
-    #endif // LCD_WIDTH > 19
+    #endif // LCD_WIDTH >= 20
    lcd.setCursor(LCD_WIDTH - 8, 1);
    lcd.print('Z');
@@ -583,6 +574,10 @@ static void lcd_implementation_status_screen() {
  #endif // LCD_HEIGHT > 2
+  //
+  // Line 3
+  //
  #if LCD_HEIGHT > 3
    lcd.setCursor(0, 2);
@@ -630,9 +625,10 @@ static void lcd_implementation_status_screen() {
  #endif // LCD_HEIGHT > 3
-  /**
+  //
-   * Display Progress Bar, Filament display, and/or Status Message on the last line
+  // Last Line
-   */
+  // Status Message (which may be a Progress Bar or Filament display)
+  //
  lcd.setCursor(0, LCD_HEIGHT - 1);
@@ -797,7 +793,7 @@ static void lcd_implementation_drawmenu_sddirectory(bool sel, uint8_t row, const
  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 
+      // 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;
@@ -835,4 +831,4 @@ static void lcd_implementation_drawmenu_sddirectory(bool sel, uint8_t row, const
 #endif // LCD_HAS_SLOW_BUTTONS
-#endif //__ULTRALCD_IMPLEMENTATION_HITACHI_HD44780_H
+#endif // ULTRALCD_IMPLEMENTATION_HITACHI_HD44780_H
--- a/MarlinKimbra/ultralcd_st7920_u8glib_rrd.h
+++ b/MarlinKimbra/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 LCD_PIXEL_WIDTH 128
-#define HEIGHT 64
+#define LCD_PIXEL_HEIGHT 64
 #include <U8glib.h>
@@ -53,9 +53,9 @@ uint8_t u8g_dev_rrd_st7920_128x64_fn(u8g_t *u8g, u8g_dev_t *dev, uint8_t msg, vo
  {
    case U8G_DEV_MSG_INIT:
      {
-        OUT_WRITE(ST7920_CS_PIN, LOW);
+        OUT_WRITE(ST7920_CS_PIN,LOW);
-        OUT_WRITE(ST7920_DAT_PIN, LOW);
+        OUT_WRITE(ST7920_DAT_PIN,LOW);
-        OUT_WRITE(ST7920_CLK_PIN, HIGH);
+        OUT_WRITE(ST7920_CLK_PIN,HIGH);
        ST7920_CS();
        u8g_Delay(120);                 //initial delay for boot up
@@ -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;
+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,HEIGHT,0,0,0},WIDTH,u8g_dev_st7920_128x64_rrd_buf};
+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

--- a/MarlinKimbra/watchdog.cpp
+++ b/MarlinKimbra/watchdog.cpp
@@ -7,11 +7,11 @@
 #include "ultralcd.h"
 //===========================================================================
-//=============================private variables  ============================
+//============================ private variables ============================
 //===========================================================================
 //===========================================================================
-//=============================functinos         ============================
+//================================ functions ================================
 //===========================================================================
@@ -36,20 +36,16 @@ void watchdog_reset()
 }
 //===========================================================================
-//=============================ISR               ============================
+//=================================== ISR ===================================
 //===========================================================================
-//Watchdog timer interrupt, called if main program blocks >1sec and manual reset is enabled.
+// Watchdog timer interrupt, called if main program blocks >1sec and manual reset is enabled.
 #ifdef WATCHDOG_RESET_MANUAL
-ISR(WDT_vect)
+ISR(WDT_vect) { 
-{ 
+  ECHO_LM(ER, MSG_WATCHDOG_RESET);
-    //TODO: This message gets overwritten by the kill() call
+  kill(PSTR("ERR:Please Reset")); // kill blocks //16 characters so it fits on a 16x2 display
-    LCD_ALERTMESSAGEPGM("ERR:Please Reset");//16 characters so it fits on a 16x2 display
+  while(1); // wait for user or serial reset
-    lcd_update();
-    ECHO_LM(MSG_WATCHDOG_RESET);
-    kill(); //kill blocks
-    while(1); //wait for user or serial reset
 }
-#endif//RESET_MANUAL
+#endif // RESET_MANUAL
-#endif//USE_WATCHDOG
+#endif // USE_WATCHDOG