diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h
index ecdc833024..1ff8b6db2f 100644
--- a/Marlin/Configuration_adv.h
+++ b/Marlin/Configuration_adv.h
@@ -181,15 +181,14 @@
 #define SD_FINISHED_STEPPERRELEASE true  //if sd support and the file is finished: disable steppers?
 #define SD_FINISHED_RELEASECOMMAND "M84 X Y Z E" // no z because of layer shift.
 
-// The hardware watchdog should halt the Microcontroller, in case the firmware gets stuck somewhere. However:
-// the Watchdog is not working well, so please only enable this for testing
-// this enables the watchdog interrupt.
-//#define USE_WATCHDOG
-//#ifdef USE_WATCHDOG
-  // you cannot reboot on a mega2560 due to a bug in he bootloader. Hence, you have to reset manually, and this is done hereby:
+// 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
+
+#ifdef USE_WATCHDOG
+// you cannot watchdog reboot on Arduino mega2560 due to a bug in he bootloader. Hence we need to ask the user to reset.
+//  THIS FEATURE IS UNSAFE!, as it will only work if interrupts are disabled. And the code could hang in an interrupt routine with interrupts disabled.
 //#define RESET_MANUAL
-//#define WATCHDOG_TIMEOUT 4  //seconds
-//#endif
+#endif
 
 // extruder advance constant (s2/mm3)
 //
@@ -214,7 +213,7 @@
 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 
-const 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
 
 // If you are using a RAMPS board or cheap E-bay purchased boards that do not detect when an SD card is inserted
 // You can get round this by connecting a push button or single throw switch to the pin defined as SDCARDCARDDETECT 
diff --git a/Marlin/EEPROMwrite.h b/Marlin/EEPROMwrite.h
index 12674ef000..b833d71e63 100644
--- a/Marlin/EEPROMwrite.h
+++ b/Marlin/EEPROMwrite.h
@@ -6,14 +6,6 @@
 #include "temperature.h"
 //#include <EEPROM.h>
 
-int plaPreheatHotendTemp;
-int plaPreheatHPBTemp;
-int plaPreheatFanSpeed;
-
-int absPreheatHotendTemp;
-int absPreheatHPBTemp;
-int absPreheatFanSpeed;
-
 template <class T> int EEPROM_writeAnything(int &ee, const T& value)
 {
   const byte* p = (const byte*)(const void*)&value;
diff --git a/Marlin/LiquidCrystalRus.cpp b/Marlin/LiquidCrystalRus.cpp
index 9f3f5c055b..4bd8de781d 100644
--- a/Marlin/LiquidCrystalRus.cpp
+++ b/Marlin/LiquidCrystalRus.cpp
@@ -1,4 +1,3 @@
-#define __PROG_TYPES_COMPAT__
 #include "LiquidCrystalRus.h"
 
 #include <stdio.h>
@@ -14,7 +13,7 @@
 
 // it is a russian alphabet translation
 // except 0401 --> 0xa2 = ╗, 0451 --> 0xb5
-const PROGMEM prog_uchar utf_recode[] = 
+const PROGMEM uint8_t utf_recode[] = 
        { 0x41,0xa0,0x42,0xa1,0xe0,0x45,0xa3,0xa4,0xa5,0xa6,0x4b,0xa7,0x4d,0x48,0x4f,
          0xa8,0x50,0x43,0x54,0xa9,0xaa,0x58,0xe1,0xab,0xac,0xe2,0xad,0xae,0x62,0xaf,0xb0,0xb1,
          0x61,0xb2,0xb3,0xb4,0xe3,0x65,0xb6,0xb7,0xb8,0xb9,0xba,0xbb,0xbc,0xbd,0x6f,
diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h
index 5b59885cd3..bec9dfe91b 100644
--- a/Marlin/Marlin.h
+++ b/Marlin/Marlin.h
@@ -17,8 +17,8 @@
 #include <util/delay.h>
 #include <avr/pgmspace.h>
 #include <avr/eeprom.h>
-#include  <avr/wdt.h>
-#include  <avr/interrupt.h>
+#include <avr/wdt.h>
+#include <avr/interrupt.h>
 
 
 #include "fastio.h"
@@ -52,22 +52,11 @@
   #define MYSERIAL MSerial
 #endif
 
-//this is a unfinsihed attemp to removes a lot of warning messages, see:
-// http://www.avrfreaks.net/index.php?name=PNphpBB2&file=printview&t=57011
-//typedef char prog_char PROGMEM; 
-// //#define PSTR    (s )        ((const PROGMEM char *)(s))
-// //# define MYPGM(s) (__extension__({static prog_char __c[] = (s); &__c[0];})) 
-// //#define MYPGM(s) ((const prog_char *g PROGMEM=s))
-#define MYPGM(s) PSTR(s)
-//#define MYPGM(s)  (__extension__({static char __c[] __attribute__((__progmem__)) = (s); &__c[0];}))  //This is the normal behaviour
-//#define MYPGM(s)  (__extension__({static prog_char __c[]  = (s); &__c[0];})) //this does not work but hides the warnings
-
-
 #define SERIAL_PROTOCOL(x) MYSERIAL.print(x);
 #define SERIAL_PROTOCOL_F(x,y) MYSERIAL.print(x,y);
-#define SERIAL_PROTOCOLPGM(x) serialprintPGM(MYPGM(x));
+#define SERIAL_PROTOCOLPGM(x) serialprintPGM(PSTR(x));
 #define SERIAL_PROTOCOLLN(x) {MYSERIAL.print(x);MYSERIAL.write('\n');}
-#define SERIAL_PROTOCOLLNPGM(x) {serialprintPGM(MYPGM(x));MYSERIAL.write('\n');}
+#define SERIAL_PROTOCOLLNPGM(x) {serialprintPGM(PSTR(x));MYSERIAL.write('\n');}
 
 
 const char errormagic[] PROGMEM ="Error:";
@@ -92,7 +81,6 @@ void serial_echopair_P(const char *s_P, unsigned long v);
 
 
 //things to write to serial from Programmemory. saves 400 to 2k of RAM.
-#define SerialprintPGM(x) serialprintPGM(MYPGM(x))
 FORCE_INLINE void serialprintPGM(const char *str)
 {
   char ch=pgm_read_byte(str);
@@ -197,6 +185,9 @@ extern float min_pos[3];
 extern float max_pos[3];
 extern unsigned char FanSpeed;
 
+extern unsigned long starttime;
+extern unsigned long stoptime;
+
 // Handling multiple extruders pins
 extern uint8_t active_extruder;
 
diff --git a/Marlin/Marlin.pde b/Marlin/Marlin.pde
index bda479e3ca..5d91345868 100644
--- a/Marlin/Marlin.pde
+++ b/Marlin/Marlin.pde
@@ -27,1839 +27,10 @@
     http://reprap.org/pipermail/reprap-dev/2011-May/003323.html
  */
 
-#include "Marlin.h"
+/* All the implementation is done in *.cpp files to get better compatibility with avr-gcc without the Arduino IDE */
+/* Use this file to help the Arduino IDE find which Arduino libraries are needed and to keep documentation on GCode */
 
-#include "ultralcd.h"
-#include "planner.h"
-#include "stepper.h"
-#include "temperature.h"
-#include "motion_control.h"
-#include "cardreader.h"
-#include "watchdog.h"
-#include "EEPROMwrite.h"
-#include "language.h"
-#include "pins_arduino.h"
-
-#define VERSION_STRING  "1.0.0"
-
-// look here for descriptions of gcodes: http://linuxcnc.org/handbook/gcode/g-code.html
-// http://objects.reprap.org/wiki/Mendel_User_Manual:_RepRapGCodes
-
-//Implemented Codes
-//-------------------
-// G0  -> G1
-// G1  - Coordinated Movement X Y Z E
-// G2  - CW ARC
-// G3  - CCW ARC
-// G4  - Dwell S<seconds> or P<milliseconds>
-// G10 - retract filament according to settings of M207
-// G11 - retract recover filament according to settings of M208
-// G28 - Home all Axis
-// G90 - Use Absolute Coordinates
-// G91 - Use Relative Coordinates
-// G92 - Set current position to cordinates given
-
-//RepRap M Codes
-// M0   - Unconditional stop - Wait for user to press a button on the LCD (Only if ULTRA_LCD is enabled)
-// M1   - Same as M0
-// M104 - Set extruder target temp
-// M105 - Read current temp
-// M106 - Fan on
-// M107 - Fan off
-// M109 - Wait for extruder current temp to reach target temp.
-// M114 - Display current position
-
-//Custom M Codes
-// M17  - Enable/Power all stepper motors
-// M18  - Disable all stepper motors; same as M84
-// M20  - List SD card
-// M21  - Init SD card
-// M22  - Release SD card
-// M23  - Select SD file (M23 filename.g)
-// M24  - Start/resume SD print
-// M25  - Pause SD print
-// M26  - Set SD position in bytes (M26 S12345)
-// M27  - Report SD print status
-// M28  - Start SD write (M28 filename.g)
-// M29  - Stop SD write
-// M30  - Delete file from SD (M30 filename.g)
-// M31  - Output time since last M109 or SD card start to serial
-// M42  - Change pin status via gcode
-// M80  - Turn on Power Supply
-// M81  - Turn off Power Supply
-// M82  - Set E codes absolute (default)
-// M83  - Set E codes relative while in Absolute Coordinates (G90) mode
-// M84  - Disable steppers until next move, 
-//        or use S<seconds> to specify an inactivity timeout, after which the steppers will be disabled.  S0 to disable the timeout.
-// M85  - Set inactivity shutdown timer with parameter S<seconds>. To disable set zero (default)
-// M92  - Set axis_steps_per_unit - same syntax as G92
-// M114 - Output current position to serial port 
-// M115	- Capabilities string
-// M117 - display message
-// M119 - Output Endstop status to serial port
-// M140 - Set bed target temp
-// M190 - Wait for bed current temp to reach target temp.
-// M200 - Set filament diameter
-// M201 - Set max acceleration in units/s^2 for print moves (M201 X1000 Y1000)
-// M202 - Set max acceleration in units/s^2 for travel moves (M202 X1000 Y1000) Unused in Marlin!!
-// M203 - Set maximum feedrate that your machine can sustain (M203 X200 Y200 Z300 E10000) in mm/sec
-// M204 - Set default acceleration: S normal moves T filament only moves (M204 S3000 T7000) im mm/sec^2  also sets minimum segment time in ms (B20000) to prevent buffer underruns and M20 minimum feedrate
-// M205 -  advanced settings:  minimum travel speed S=while printing T=travel only,  B=minimum segment time X= maximum xy jerk, Z=maximum Z jerk, E=maximum E jerk
-// M206 - set additional homeing offset
-// M207 - set retract length S[positive mm] F[feedrate mm/sec] Z[additional zlift/hop]
-// M208 - set recover=unretract length S[positive mm surplus to the M207 S*] F[feedrate mm/sec]
-// M209 - S<1=true/0=false> enable automatic retract detect if the slicer did not support G10/11: every normal extrude-only move will be classified as retract depending on the direction.
-// M220 S<factor in percent>- set speed factor override percentage
-// M221 S<factor in percent>- set extrude factor override percentage
-// M240 - Trigger a camera to take a photograph
-// M301 - Set PID parameters P I and D
-// M302 - Allow cold extrudes
-// M303 - PID relay autotune S<temperature> sets the target temperature. (default target temperature = 150C)
-// M304 - Set bed PID parameters P I and D
-// M400 - Finish all moves
-// M500 - stores paramters in EEPROM
-// M501 - reads parameters from EEPROM (if you need reset them after you changed them temporarily).  
-// M502 - reverts to the default "factory settings".  You still need to store them in EEPROM afterwards if you want to.
-// M503 - print the current settings (from memory not from eeprom)
-// M999 - Restart after being stopped by error
-
-//Stepper Movement Variables
-
-//===========================================================================
-//=============================imported variables============================
-//===========================================================================
-
-
-//===========================================================================
-//=============================public variables=============================
-//===========================================================================
-#ifdef SDSUPPORT
-CardReader card;
-#endif
-float homing_feedrate[] = HOMING_FEEDRATE;
-bool axis_relative_modes[] = AXIS_RELATIVE_MODES;
-volatile int feedmultiply=100; //100->1 200->2
-int saved_feedmultiply;
-volatile bool feedmultiplychanged=false;
-volatile int extrudemultiply=100; //100->1 200->2
-float current_position[NUM_AXIS] = { 0.0, 0.0, 0.0, 0.0 };
-float add_homeing[3]={0,0,0};
-float min_pos[3] = { X_MIN_POS, Y_MIN_POS, Z_MIN_POS };
-float max_pos[3] = { X_MAX_POS, Y_MAX_POS, Z_MAX_POS };
-uint8_t active_extruder = 0;
-unsigned char FanSpeed=0;
-
-#ifdef FWRETRACT
-  bool autoretract_enabled=true;
-  bool retracted=false;
-  float retract_length=3, retract_feedrate=17*60, retract_zlift=0.8;
-  float retract_recover_length=0, retract_recover_feedrate=8*60;
-#endif
-
-//===========================================================================
-//=============================private variables=============================
-//===========================================================================
-const char axis_codes[NUM_AXIS] = {'X', 'Y', 'Z', 'E'};
-static float destination[NUM_AXIS] = {  0.0, 0.0, 0.0, 0.0};
-static float offset[3] = {0.0, 0.0, 0.0};
-static bool home_all_axis = true;
-static float feedrate = 1500.0, next_feedrate, saved_feedrate;
-static long gcode_N, gcode_LastN, Stopped_gcode_LastN = 0;
-
-static bool relative_mode = false;  //Determines Absolute or Relative Coordinates
-static bool relative_mode_e = false;  //Determines Absolute or Relative E Codes while in Absolute Coordinates mode. E is always relative in Relative Coordinates mode.
-
-static char cmdbuffer[BUFSIZE][MAX_CMD_SIZE];
-static bool fromsd[BUFSIZE];
-static int bufindr = 0;
-static int bufindw = 0;
-static int buflen = 0;
-//static int i = 0;
-static char serial_char;
-static int serial_count = 0;
-static boolean comment_mode = false;
-static char *strchr_pointer; // just a pointer to find chars in the cmd string like X, Y, Z, E, etc
-
-const int sensitive_pins[] = SENSITIVE_PINS; // Sensitive pin list for M42
-
-//static float tt = 0;
-//static float bt = 0;
-
-//Inactivity shutdown variables
-static unsigned long previous_millis_cmd = 0;
-static unsigned long max_inactive_time = 0;
-static unsigned long stepper_inactive_time = DEFAULT_STEPPER_DEACTIVE_TIME*1000l;
-
-static unsigned long starttime=0;
-static unsigned long stoptime=0;
-
-static uint8_t tmp_extruder;
-
-
-bool Stopped=false;
-
-//===========================================================================
-//=============================ROUTINES=============================
-//===========================================================================
-
-void get_arc_coordinates();
-bool setTargetedHotend(int code);
-
-void serial_echopair_P(const char *s_P, float v)
-    { serialprintPGM(s_P); SERIAL_ECHO(v); }
-void serial_echopair_P(const char *s_P, double v)
-    { serialprintPGM(s_P); SERIAL_ECHO(v); }
-void serial_echopair_P(const char *s_P, unsigned long v)
-    { serialprintPGM(s_P); SERIAL_ECHO(v); }
-
-extern "C"{
-  extern unsigned int __bss_end;
-  extern unsigned int __heap_start;
-  extern void *__brkval;
-
-  int freeMemory() {
-    int free_memory;
-
-    if((int)__brkval == 0)
-      free_memory = ((int)&free_memory) - ((int)&__bss_end);
-    else
-      free_memory = ((int)&free_memory) - ((int)__brkval);
-
-    return free_memory;
-  }
-}
-
-//adds an command to the main command buffer
-//thats really done in a non-safe way.
-//needs overworking someday
-void enquecommand(const char *cmd)
-{
-  if(buflen < BUFSIZE)
-  {
-    //this is dangerous if a mixing of serial and this happsens
-    strcpy(&(cmdbuffer[bufindw][0]),cmd);
-    SERIAL_ECHO_START;
-    SERIAL_ECHOPGM("enqueing \"");
-    SERIAL_ECHO(cmdbuffer[bufindw]);
-    SERIAL_ECHOLNPGM("\"");
-    bufindw= (bufindw + 1)%BUFSIZE;
-    buflen += 1;
-  }
-}
-
-void setup_killpin()
-{
-  #if( KILL_PIN>-1 )
-    pinMode(KILL_PIN,INPUT);
-    WRITE(KILL_PIN,HIGH);
-  #endif
-}
-    
-void setup_photpin()
-{
-  #ifdef PHOTOGRAPH_PIN
-    #if (PHOTOGRAPH_PIN > -1)
-    SET_OUTPUT(PHOTOGRAPH_PIN);
-    WRITE(PHOTOGRAPH_PIN, LOW);
-    #endif
-  #endif 
-}
-
-void setup_powerhold()
-{
- #ifdef SUICIDE_PIN
-   #if (SUICIDE_PIN> -1)
-      SET_OUTPUT(SUICIDE_PIN);
-      WRITE(SUICIDE_PIN, HIGH);
-   #endif
- #endif
-}
-
-void suicide()
-{
- #ifdef SUICIDE_PIN
-    #if (SUICIDE_PIN> -1) 
-      SET_OUTPUT(SUICIDE_PIN);
-      WRITE(SUICIDE_PIN, LOW);
-    #endif
-  #endif
-}
-
-void setup()
-{
-  setup_killpin(); 
-  setup_powerhold();
-  MYSERIAL.begin(BAUDRATE);
-  SERIAL_PROTOCOLLNPGM("start");
-  SERIAL_ECHO_START;
-
-  // Check startup - does nothing if bootloader sets MCUSR to 0
-  byte mcu = MCUSR;
-  if(mcu & 1) SERIAL_ECHOLNPGM(MSG_POWERUP);
-  if(mcu & 2) SERIAL_ECHOLNPGM(MSG_EXTERNAL_RESET);
-  if(mcu & 4) SERIAL_ECHOLNPGM(MSG_BROWNOUT_RESET);
-  if(mcu & 8) SERIAL_ECHOLNPGM(MSG_WATCHDOG_RESET);
-  if(mcu & 32) SERIAL_ECHOLNPGM(MSG_SOFTWARE_RESET);
-  MCUSR=0;
-
-  SERIAL_ECHOPGM(MSG_MARLIN);
-  SERIAL_ECHOLNPGM(VERSION_STRING);
-  #ifdef STRING_VERSION_CONFIG_H
-    #ifdef STRING_CONFIG_H_AUTHOR
-      SERIAL_ECHO_START;
-      SERIAL_ECHOPGM(MSG_CONFIGURATION_VER);
-      SERIAL_ECHOPGM(STRING_VERSION_CONFIG_H);
-      SERIAL_ECHOPGM(MSG_AUTHOR);
-      SERIAL_ECHOLNPGM(STRING_CONFIG_H_AUTHOR);
-    #endif
-  #endif
-  SERIAL_ECHO_START;
-  SERIAL_ECHOPGM(MSG_FREE_MEMORY);
-  SERIAL_ECHO(freeMemory());
-  SERIAL_ECHOPGM(MSG_PLANNER_BUFFER_BYTES);
-  SERIAL_ECHOLN((int)sizeof(block_t)*BLOCK_BUFFER_SIZE);
-  for(int8_t i = 0; i < BUFSIZE; i++)
-  {
-    fromsd[i] = false;
-  }
-  
-  EEPROM_RetrieveSettings(); // loads data from EEPROM if available
-
-  for(int8_t i=0; i < NUM_AXIS; i++)
-  {
-    axis_steps_per_sqr_second[i] = max_acceleration_units_per_sq_second[i] * axis_steps_per_unit[i];
-  }
-
-
-  tp_init();    // Initialize temperature loop 
-  plan_init();  // Initialize planner;
-  st_init();    // Initialize stepper;
-  wd_init();
-  setup_photpin();
-  
-  LCD_INIT;
-}
-
-
-void loop()
-{
-  if(buflen < (BUFSIZE-1))
-    get_command();
-  #ifdef SDSUPPORT
-  card.checkautostart(false);
-  #endif
-  if(buflen)
-  {
-    #ifdef SDSUPPORT
-      if(card.saving)
-      {
-	if(strstr(cmdbuffer[bufindr],"M29") == NULL)
-	{
-	  card.write_command(cmdbuffer[bufindr]);
-	  SERIAL_PROTOCOLLNPGM(MSG_OK);
-	}
-	else
-	{
-	  card.closefile();
-	  SERIAL_PROTOCOLLNPGM(MSG_FILE_SAVED);
-	}
-      }
-      else
-      {
-	process_commands();
-      }
-    #else
-      process_commands();
-    #endif //SDSUPPORT
-    buflen = (buflen-1);
-    bufindr = (bufindr + 1)%BUFSIZE;
-  }
-  //check heater every n milliseconds
-  manage_heater();
-  manage_inactivity();
-  checkHitEndstops();
-  LCD_STATUS;
-}
-
-void get_command() 
-{ 
-  while( MYSERIAL.available() > 0  && buflen < BUFSIZE) {
-    serial_char = MYSERIAL.read();
-    if(serial_char == '\n' || 
-       serial_char == '\r' || 
-       (serial_char == ':' && comment_mode == false) || 
-       serial_count >= (MAX_CMD_SIZE - 1) ) 
-    {
-      if(!serial_count) { //if empty line
-        comment_mode = false; //for new command
-        return;
-      }
-      cmdbuffer[bufindw][serial_count] = 0; //terminate string
-      if(!comment_mode){
-        comment_mode = false; //for new command
-        fromsd[bufindw] = false;
-        if(strstr(cmdbuffer[bufindw], "N") != NULL)
-        {
-          strchr_pointer = strchr(cmdbuffer[bufindw], 'N');
-          gcode_N = (strtol(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL, 10));
-          if(gcode_N != gcode_LastN+1 && (strstr(cmdbuffer[bufindw], "M110") == NULL) ) {
-            SERIAL_ERROR_START;
-            SERIAL_ERRORPGM(MSG_ERR_LINE_NO);
-            SERIAL_ERRORLN(gcode_LastN);
-            //Serial.println(gcode_N);
-            FlushSerialRequestResend();
-            serial_count = 0;
-            return;
-          }
-
-          if(strstr(cmdbuffer[bufindw], "*") != NULL)
-          {
-            byte checksum = 0;
-            byte count = 0;
-            while(cmdbuffer[bufindw][count] != '*') checksum = checksum^cmdbuffer[bufindw][count++];
-            strchr_pointer = strchr(cmdbuffer[bufindw], '*');
-
-            if( (int)(strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)) != checksum) {
-              SERIAL_ERROR_START;
-              SERIAL_ERRORPGM(MSG_ERR_CHECKSUM_MISMATCH);
-              SERIAL_ERRORLN(gcode_LastN);
-              FlushSerialRequestResend();
-              serial_count = 0;
-              return;
-            }
-            //if no errors, continue parsing
-          }
-          else 
-          {
-            SERIAL_ERROR_START;
-            SERIAL_ERRORPGM(MSG_ERR_NO_CHECKSUM);
-            SERIAL_ERRORLN(gcode_LastN);
-            FlushSerialRequestResend();
-            serial_count = 0;
-            return;
-          }
-
-          gcode_LastN = gcode_N;
-          //if no errors, continue parsing
-        }
-        else  // if we don't receive 'N' but still see '*'
-        {
-          if((strstr(cmdbuffer[bufindw], "*") != NULL))
-          {
-            SERIAL_ERROR_START;
-            SERIAL_ERRORPGM(MSG_ERR_NO_LINENUMBER_WITH_CHECKSUM);
-            SERIAL_ERRORLN(gcode_LastN);
-            serial_count = 0;
-            return;
-          }
-        }
-        if((strstr(cmdbuffer[bufindw], "G") != NULL)){
-          strchr_pointer = strchr(cmdbuffer[bufindw], 'G');
-          switch((int)((strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)))){
-          case 0:
-          case 1:
-          case 2:
-          case 3:
-            if(Stopped == false) { // If printer is stopped by an error the G[0-3] codes are ignored.
-	      #ifdef SDSUPPORT
-              if(card.saving)
-                break;
-	      #endif //SDSUPPORT
-              SERIAL_PROTOCOLLNPGM(MSG_OK); 
-            }
-            else {
-              SERIAL_ERRORLNPGM(MSG_ERR_STOPPED);
-              LCD_MESSAGEPGM(MSG_STOPPED);
-            }
-            break;
-          default:
-            break;
-          }
-
-        }
-        bufindw = (bufindw + 1)%BUFSIZE;
-        buflen += 1;
-      }
-      serial_count = 0; //clear buffer
-    }
-    else
-    {
-      if(serial_char == ';') comment_mode = true;
-      if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;
-    }
-  }
-  #ifdef SDSUPPORT
-  if(!card.sdprinting || serial_count!=0){
-    return;
-  }
-  while( !card.eof()  && buflen < BUFSIZE) {
-    int16_t n=card.get();
-    serial_char = (char)n;
-    if(serial_char == '\n' || 
-       serial_char == '\r' || 
-       (serial_char == ':' && comment_mode == false) || 
-       serial_count >= (MAX_CMD_SIZE - 1)||n==-1) 
-    {
-      if(card.eof()){
-        SERIAL_PROTOCOLLNPGM(MSG_FILE_PRINTED);
-        stoptime=millis();
-        char time[30];
-        unsigned long t=(stoptime-starttime)/1000;
-        int sec,min;
-        min=t/60;
-        sec=t%60;
-        sprintf(time,"%i min, %i sec",min,sec);
-        SERIAL_ECHO_START;
-        SERIAL_ECHOLN(time);
-        LCD_MESSAGE(time);
-        card.printingHasFinished();
-        card.checkautostart(true);
-        
-      }
-      if(!serial_count)
-      {
-        comment_mode = false; //for new command
-        return; //if empty line
-      }
-      cmdbuffer[bufindw][serial_count] = 0; //terminate string
-//      if(!comment_mode){
-        fromsd[bufindw] = true;
-        buflen += 1;
-        bufindw = (bufindw + 1)%BUFSIZE;
-//      }     
-      comment_mode = false; //for new command
-      serial_count = 0; //clear buffer
-    }
-    else
-    {
-      if(serial_char == ';') comment_mode = true;
-      if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;
-    }
-  }
-  
-  #endif //SDSUPPORT
-
-}
-
-
-float code_value() 
-{ 
-  return (strtod(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL)); 
-}
-
-long code_value_long() 
-{ 
-  return (strtol(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL, 10)); 
-}
-
-bool code_seen(char code_string[]) //Return True if the string was found
-{ 
-  return (strstr(cmdbuffer[bufindr], code_string) != NULL); 
-}  
-
-bool code_seen(char code)
-{
-  strchr_pointer = strchr(cmdbuffer[bufindr], code);
-  return (strchr_pointer != NULL);  //Return True if a character was found
-}
-
-#define DEFINE_PGM_READ_ANY(type, reader)		\
-    static inline type pgm_read_any(const type *p)	\
-	{ return pgm_read_##reader##_near(p); }
-
-DEFINE_PGM_READ_ANY(float,       float);
-DEFINE_PGM_READ_ANY(signed char, byte);
-
-#define XYZ_CONSTS_FROM_CONFIG(type, array, CONFIG)	\
-static const PROGMEM type array##_P[3] =		\
-    { X_##CONFIG, Y_##CONFIG, Z_##CONFIG };		\
-static inline type array(int axis)			\
-    { return pgm_read_any(&array##_P[axis]); }
-
-XYZ_CONSTS_FROM_CONFIG(float, base_min_pos,    MIN_POS);
-XYZ_CONSTS_FROM_CONFIG(float, base_max_pos,    MAX_POS);
-XYZ_CONSTS_FROM_CONFIG(float, base_home_pos,   HOME_POS);
-XYZ_CONSTS_FROM_CONFIG(float, max_length,      MAX_LENGTH);
-XYZ_CONSTS_FROM_CONFIG(float, home_retract_mm, HOME_RETRACT_MM);
-XYZ_CONSTS_FROM_CONFIG(signed char, home_dir,  HOME_DIR);
-
-static void axis_is_at_home(int axis) {
-  current_position[axis] = base_home_pos(axis) + add_homeing[axis];
-  min_pos[axis] =          base_min_pos(axis) + add_homeing[axis];
-  max_pos[axis] =          base_max_pos(axis) + add_homeing[axis];
-}
-
-static void homeaxis(int axis) {
-#define HOMEAXIS_DO(LETTER) \
-  ((LETTER##_MIN_PIN > -1 && LETTER##_HOME_DIR==-1) || (LETTER##_MAX_PIN > -1 && LETTER##_HOME_DIR==1))
-
-  if (axis==X_AXIS ? HOMEAXIS_DO(X) :
-      axis==Y_AXIS ? HOMEAXIS_DO(Y) :
-      axis==Z_AXIS ? HOMEAXIS_DO(Z) :
-      0) {
-    current_position[axis] = 0;
-    plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-    destination[axis] = 1.5 * max_length(axis) * home_dir(axis);
-    feedrate = homing_feedrate[axis];
-    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
-    st_synchronize();
-   
-    current_position[axis] = 0;
-    plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-    destination[axis] = -home_retract_mm(axis) * home_dir(axis);
-    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
-    st_synchronize();
-   
-    destination[axis] = 2*home_retract_mm(axis) * home_dir(axis);
-    feedrate = homing_feedrate[axis]/2 ; 
-    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
-    st_synchronize();
-   
-    axis_is_at_home(axis);					
-    destination[axis] = current_position[axis];
-    feedrate = 0.0;
-    endstops_hit_on_purpose();
-  }
-}
-#define HOMEAXIS(LETTER) homeaxis(LETTER##_AXIS)
-
-void process_commands()
-{
-  unsigned long codenum; //throw away variable
-  char *starpos = NULL;
-
-  if(code_seen('G'))
-  {
-    switch((int)code_value())
-    {
-    case 0: // G0 -> G1
-    case 1: // G1
-      if(Stopped == false) {
-        get_coordinates(); // For X Y Z E F
-        prepare_move();
-        //ClearToSend();
-        return;
-      }
-      //break;
-    case 2: // G2  - CW ARC
-      if(Stopped == false) {
-        get_arc_coordinates();
-        prepare_arc_move(true);
-        return;
-      }
-    case 3: // G3  - CCW ARC
-      if(Stopped == false) {
-        get_arc_coordinates();
-        prepare_arc_move(false);
-        return;
-      }
-    case 4: // G4 dwell
-      LCD_MESSAGEPGM(MSG_DWELL);
-      codenum = 0;
-      if(code_seen('P')) codenum = code_value(); // milliseconds to wait
-      if(code_seen('S')) codenum = code_value() * 1000; // seconds to wait
-      
-      st_synchronize();
-      codenum += millis();  // keep track of when we started waiting
-      previous_millis_cmd = millis();
-      while(millis()  < codenum ){
-        manage_heater();
-        manage_inactivity();
-        LCD_STATUS;
-      }
-      break;
-      #ifdef FWRETRACT  
-      case 10: // G10 retract
-      if(!retracted) 
-      {
-        destination[X_AXIS]=current_position[X_AXIS];
-        destination[Y_AXIS]=current_position[Y_AXIS];
-        destination[Z_AXIS]=current_position[Z_AXIS]; 
-        current_position[Z_AXIS]+=-retract_zlift;
-        destination[E_AXIS]=current_position[E_AXIS]-retract_length; 
-        feedrate=retract_feedrate;
-        retracted=true;
-        prepare_move();
-      }
-      
-      break;
-      case 11: // G10 retract_recover
-      if(!retracted) 
-      {
-        destination[X_AXIS]=current_position[X_AXIS];
-        destination[Y_AXIS]=current_position[Y_AXIS];
-        destination[Z_AXIS]=current_position[Z_AXIS]; 
-        
-        current_position[Z_AXIS]+=retract_zlift;
-        current_position[E_AXIS]+=-retract_recover_length; 
-        feedrate=retract_recover_feedrate;
-        retracted=false;
-        prepare_move();
-      }
-      break;
-      #endif //FWRETRACT
-    case 28: //G28 Home all Axis one at a time
-      saved_feedrate = feedrate;
-      saved_feedmultiply = feedmultiply;
-      feedmultiply = 100;
-      previous_millis_cmd = millis();
-      
-      enable_endstops(true);
-      
-      for(int8_t i=0; i < NUM_AXIS; i++) {
-        destination[i] = current_position[i];
-      }
-      feedrate = 0.0;
-      home_all_axis = !((code_seen(axis_codes[0])) || (code_seen(axis_codes[1])) || (code_seen(axis_codes[2])));
-      
-      #if Z_HOME_DIR > 0                      // If homing away from BED do Z first
-      if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) {
-        HOMEAXIS(Z);
-      }
-      #endif
-      
-      #ifdef QUICK_HOME
-      if((home_all_axis)||( code_seen(axis_codes[X_AXIS]) && code_seen(axis_codes[Y_AXIS])) )  //first diagonal move
-      {
-        current_position[X_AXIS] = 0;current_position[Y_AXIS] = 0;  
-
-        plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); 
-        destination[X_AXIS] = 1.5 * X_MAX_LENGTH * X_HOME_DIR;destination[Y_AXIS] = 1.5 * Y_MAX_LENGTH * Y_HOME_DIR;  
-        feedrate = homing_feedrate[X_AXIS]; 
-        if(homing_feedrate[Y_AXIS]<feedrate)
-          feedrate =homing_feedrate[Y_AXIS]; 
-        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
-        st_synchronize();
-    
-        axis_is_at_home(X_AXIS);
-        axis_is_at_home(Y_AXIS);
-        plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-        destination[X_AXIS] = current_position[X_AXIS];
-        destination[Y_AXIS] = current_position[Y_AXIS];
-        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
-        feedrate = 0.0;
-        st_synchronize();
-        endstops_hit_on_purpose();
-      }
-      #endif
-      
-      if((home_all_axis) || (code_seen(axis_codes[X_AXIS]))) 
-      {
-        HOMEAXIS(X);
-      }
-
-      if((home_all_axis) || (code_seen(axis_codes[Y_AXIS]))) {
-        HOMEAXIS(Y);
-      }
-      
-      #if Z_HOME_DIR < 0                      // If homing towards BED do Z last
-      if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) {
-        HOMEAXIS(Z);
-      }
-      #endif
-      
-      if(code_seen(axis_codes[X_AXIS])) 
-      {
-        if(code_value_long() != 0) {
-          current_position[X_AXIS]=code_value()+add_homeing[0];
-        }
-      }
-
-      if(code_seen(axis_codes[Y_AXIS])) {
-        if(code_value_long() != 0) {
-          current_position[Y_AXIS]=code_value()+add_homeing[1];
-        }
-      }
-
-      if(code_seen(axis_codes[Z_AXIS])) {
-        if(code_value_long() != 0) {
-          current_position[Z_AXIS]=code_value()+add_homeing[2];
-        }
-      }
-      plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-      
-      #ifdef ENDSTOPS_ONLY_FOR_HOMING
-        enable_endstops(false);
-      #endif
-      
-      feedrate = saved_feedrate;
-      feedmultiply = saved_feedmultiply;
-      previous_millis_cmd = millis();
-      endstops_hit_on_purpose();
-      break;
-    case 90: // G90
-      relative_mode = false;
-      break;
-    case 91: // G91
-      relative_mode = true;
-      break;
-    case 92: // G92
-      if(!code_seen(axis_codes[E_AXIS]))
-        st_synchronize();
-      for(int8_t i=0; i < NUM_AXIS; i++) {
-        if(code_seen(axis_codes[i])) { 
-           if(i == E_AXIS) {
-             current_position[i] = code_value();  
-             plan_set_e_position(current_position[E_AXIS]);
-           }
-           else {
-             current_position[i] = code_value()+add_homeing[i];  
-             plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-           }
-        }
-      }
-      break;
-    }
-  }
-
-  else if(code_seen('M'))
-  {
-    switch( (int)code_value() ) 
-    {
+#include "Configuration.h"
 #ifdef ULTRA_LCD
-    case 0: // M0 - Unconditional stop - Wait for user button press on LCD
-    case 1: // M1 - Conditional stop - Wait for user button press on LCD
-    {
-      LCD_MESSAGEPGM(MSG_USERWAIT);
-      codenum = 0;
-      if(code_seen('P')) codenum = code_value(); // milliseconds to wait
-      if(code_seen('S')) codenum = code_value() * 1000; // seconds to wait
-      
-      st_synchronize();
-      previous_millis_cmd = millis();
-      if (codenum > 0){
-        codenum += millis();  // keep track of when we started waiting
-        while(millis()  < codenum && !CLICKED){
-          manage_heater();
-          manage_inactivity();
-          LCD_STATUS;
-        }
-      }else{
-        while(!CLICKED){
-          manage_heater();
-          manage_inactivity();
-          LCD_STATUS;
-        }
-      }
-    }
-    break;
-#endif
-    case 17:
-        LCD_MESSAGEPGM(MSG_NO_MOVE);
-        enable_x(); 
-        enable_y(); 
-        enable_z(); 
-        enable_e0(); 
-        enable_e1(); 
-        enable_e2(); 
-      break;
-
-#ifdef SDSUPPORT
-    case 20: // M20 - list SD card
-      SERIAL_PROTOCOLLNPGM(MSG_BEGIN_FILE_LIST);
-      card.ls();
-      SERIAL_PROTOCOLLNPGM(MSG_END_FILE_LIST);
-      break;
-    case 21: // M21 - init SD card
-      
-      card.initsd();
-      
-      break;
-    case 22: //M22 - release SD card
-      card.release();
-
-      break;
-    case 23: //M23 - Select file
-      starpos = (strchr(strchr_pointer + 4,'*'));
-      if(starpos!=NULL)
-        *(starpos-1)='\0';
-      card.openFile(strchr_pointer + 4,true);
-      break;
-    case 24: //M24 - Start SD print
-      card.startFileprint();
-      starttime=millis();
-      break;
-    case 25: //M25 - Pause SD print
-      card.pauseSDPrint();
-      break;
-    case 26: //M26 - Set SD index
-      if(card.cardOK && code_seen('S')) {
-        card.setIndex(code_value_long());
-      }
-      break;
-    case 27: //M27 - Get SD status
-      card.getStatus();
-      break;
-    case 28: //M28 - Start SD write
-      starpos = (strchr(strchr_pointer + 4,'*'));
-      if(starpos != NULL){
-        char* npos = strchr(cmdbuffer[bufindr], 'N');
-        strchr_pointer = strchr(npos,' ') + 1;
-        *(starpos-1) = '\0';
-      }
-      card.openFile(strchr_pointer+4,false);
-      break;
-    case 29: //M29 - Stop SD write
-      //processed in write to file routine above
-      //card,saving = false;
-      break;
-    case 30: //M30 <filename> Delete File 
-	if (card.cardOK){
-		card.closefile();
-		starpos = (strchr(strchr_pointer + 4,'*'));
-                if(starpos != NULL){
-                char* npos = strchr(cmdbuffer[bufindr], 'N');
-                strchr_pointer = strchr(npos,' ') + 1;
-                *(starpos-1) = '\0';
-         }
-	 card.removeFile(strchr_pointer + 4);
-	}
-	break;
-	
-#endif //SDSUPPORT
-
-    case 31: //M31 take time since the start of the SD print or an M109 command
-      {
-      stoptime=millis();
-      char time[30];
-      unsigned long t=(stoptime-starttime)/1000;
-      int sec,min;
-      min=t/60;
-      sec=t%60;
-      sprintf(time,"%i min, %i sec",min,sec);
-      SERIAL_ECHO_START;
-      SERIAL_ECHOLN(time);
-      LCD_MESSAGE(time);
-      autotempShutdown();
-      }
-      break;
-    case 42: //M42 -Change pin status via gcode
-      if (code_seen('S'))
-      {
-        int pin_status = code_value();
-        if (code_seen('P') && pin_status >= 0 && pin_status <= 255)
-        {
-          int pin_number = code_value();
-          for(int8_t i = 0; i < (int8_t)sizeof(sensitive_pins); i++)
-          {
-            if (sensitive_pins[i] == pin_number)
-            {
-              pin_number = -1;
-              break;
-            }
-          }
-          
-          if (pin_number > -1)
-          {              
-            pinMode(pin_number, OUTPUT);
-            digitalWrite(pin_number, pin_status);
-            analogWrite(pin_number, pin_status);
-          }
-        }
-      }
-     break;
-    case 104: // M104
-      if(setTargetedHotend(104)){
-        break;
-      }
-      if (code_seen('S')) setTargetHotend(code_value(), tmp_extruder);
-      setWatch();
-      break;
-    case 140: // M140 set bed temp
-      if (code_seen('S')) setTargetBed(code_value());
-      break;
-    case 105 : // M105
-      if(setTargetedHotend(105)){
-        break;
-      }
-      #if (TEMP_0_PIN > -1)
-        SERIAL_PROTOCOLPGM("ok T:");
-        SERIAL_PROTOCOL_F(degHotend(tmp_extruder),1); 
-        SERIAL_PROTOCOLPGM(" /");
-        SERIAL_PROTOCOL_F(degTargetHotend(tmp_extruder),1); 
-        #if TEMP_BED_PIN > -1
-          SERIAL_PROTOCOLPGM(" B:");  
-          SERIAL_PROTOCOL_F(degBed(),1);
-          SERIAL_PROTOCOLPGM(" /");
-          SERIAL_PROTOCOL_F(degTargetBed(),1);
-        #endif //TEMP_BED_PIN
-      #else
-        SERIAL_ERROR_START;
-        SERIAL_ERRORLNPGM(MSG_ERR_NO_THERMISTORS);
-      #endif
-
-        SERIAL_PROTOCOLPGM(" @:");
-        SERIAL_PROTOCOL(getHeaterPower(tmp_extruder));  
-
-        SERIAL_PROTOCOLPGM(" B@:");
-        SERIAL_PROTOCOL(getHeaterPower(-1));  
-
-        SERIAL_PROTOCOLLN("");
-      return;
-      break;
-    case 109: 
-    {// M109 - Wait for extruder heater to reach target.
-      if(setTargetedHotend(109)){
-        break;
-      }
-      LCD_MESSAGEPGM(MSG_HEATING);   
-      #ifdef AUTOTEMP
-        autotemp_enabled=false;
-      #endif
-      if (code_seen('S')) setTargetHotend(code_value(), tmp_extruder);
-      #ifdef AUTOTEMP
-        if (code_seen('S')) autotemp_min=code_value();
-        if (code_seen('B')) autotemp_max=code_value();
-        if (code_seen('F')) 
-        {
-          autotemp_factor=code_value();
-          autotemp_enabled=true;
-        }
-      #endif
-      
-      setWatch();
-      codenum = millis(); 
-
-      /* See if we are heating up or cooling down */
-      bool target_direction = isHeatingHotend(tmp_extruder); // true if heating, false if cooling
-
-      #ifdef TEMP_RESIDENCY_TIME
-        long residencyStart;
-        residencyStart = -1;
-        /* continue to loop until we have reached the target temp   
-          _and_ until TEMP_RESIDENCY_TIME hasn't passed since we reached it */
-        while((residencyStart == -1) ||
-              (residencyStart >= 0 && (((unsigned int) (millis() - residencyStart)) < (TEMP_RESIDENCY_TIME * 1000UL))) ) {
-      #else
-        while ( target_direction ? (isHeatingHotend(tmp_extruder)) : (isCoolingHotend(tmp_extruder)&&(CooldownNoWait==false)) ) {
-      #endif //TEMP_RESIDENCY_TIME
-          if( (millis() - codenum) > 1000UL )
-          { //Print Temp Reading and remaining time every 1 second while heating up/cooling down
-            SERIAL_PROTOCOLPGM("T:");
-            SERIAL_PROTOCOL_F(degHotend(tmp_extruder),1); 
-            SERIAL_PROTOCOLPGM(" E:");
-            SERIAL_PROTOCOL((int)tmp_extruder); 
-            #ifdef TEMP_RESIDENCY_TIME
-              SERIAL_PROTOCOLPGM(" W:");
-              if(residencyStart > -1)
-              {
-                 codenum = ((TEMP_RESIDENCY_TIME * 1000UL) - (millis() - residencyStart)) / 1000UL;
-                 SERIAL_PROTOCOLLN( codenum );
-              }
-              else 
-              {
-                 SERIAL_PROTOCOLLN( "?" );
-              }
-            #else
-              SERIAL_PROTOCOLLN("");
-            #endif
-            codenum = millis();
-          }
-          manage_heater();
-          manage_inactivity();
-          LCD_STATUS;
-        #ifdef TEMP_RESIDENCY_TIME
-            /* start/restart the TEMP_RESIDENCY_TIME timer whenever we reach target temp for the first time
-              or when current temp falls outside the hysteresis after target temp was reached */
-          if ((residencyStart == -1 &&  target_direction && (degHotend(tmp_extruder) >= (degTargetHotend(tmp_extruder)-TEMP_WINDOW))) ||
-              (residencyStart == -1 && !target_direction && (degHotend(tmp_extruder) <= (degTargetHotend(tmp_extruder)+TEMP_WINDOW))) ||
-              (residencyStart > -1 && labs(degHotend(tmp_extruder) - degTargetHotend(tmp_extruder)) > TEMP_HYSTERESIS) ) 
-          {
-            residencyStart = millis();
-          }
-        #endif //TEMP_RESIDENCY_TIME
-        }
-        LCD_MESSAGEPGM(MSG_HEATING_COMPLETE);
-        starttime=millis();
-        previous_millis_cmd = millis();
-      }
-      break;
-    case 190: // M190 - Wait for bed heater to reach target.
-    #if TEMP_BED_PIN > -1
-        LCD_MESSAGEPGM(MSG_BED_HEATING);
-        if (code_seen('S')) setTargetBed(code_value());
-        codenum = millis(); 
-        while(isHeatingBed()) 
-        {
-          if(( millis() - codenum) > 1000 ) //Print Temp Reading every 1 second while heating up.
-          {
-            float tt=degHotend(active_extruder);
-            SERIAL_PROTOCOLPGM("T:");
-            SERIAL_PROTOCOL(tt);
-            SERIAL_PROTOCOLPGM(" E:");
-            SERIAL_PROTOCOL((int)active_extruder); 
-            SERIAL_PROTOCOLPGM(" B:");
-            SERIAL_PROTOCOL_F(degBed(),1); 
-            SERIAL_PROTOCOLLN(""); 
-            codenum = millis(); 
-          }
-          manage_heater();
-          manage_inactivity();
-          LCD_STATUS;
-        }
-        LCD_MESSAGEPGM(MSG_BED_DONE);
-        previous_millis_cmd = millis();
-    #endif
-        break;
-
-    #if FAN_PIN > -1
-      case 106: //M106 Fan On
-        if (code_seen('S')){
-           FanSpeed=constrain(code_value(),0,255);
-        }
-        else {
-          FanSpeed=255;			
-        }
-        break;
-      case 107: //M107 Fan Off
-        FanSpeed = 0;
-        break;
-    #endif //FAN_PIN
-
-    #if (PS_ON_PIN > -1)
-      case 80: // M80 - ATX Power On
-        SET_OUTPUT(PS_ON_PIN); //GND
-        WRITE(PS_ON_PIN, LOW);
-        break;
-      #endif
-      
-      case 81: // M81 - ATX Power Off
-      
-      #if defined SUICIDE_PIN && SUICIDE_PIN > -1
-        st_synchronize();
-        suicide();
-      #elif (PS_ON_PIN > -1)
-        SET_OUTPUT(PS_ON_PIN); 
-        WRITE(PS_ON_PIN, HIGH);
-      #endif
-		break;
-        
-    case 82:
-      axis_relative_modes[3] = false;
-      break;
-    case 83:
-      axis_relative_modes[3] = true;
-      break;
-    case 18: //compatibility
-    case 84: // M84
-      if(code_seen('S')){ 
-        stepper_inactive_time = code_value() * 1000; 
-      }
-      else
-      { 
-        bool all_axis = !((code_seen(axis_codes[0])) || (code_seen(axis_codes[1])) || (code_seen(axis_codes[2]))|| (code_seen(axis_codes[3])));
-        if(all_axis)
-        {
-          st_synchronize();
-          disable_e0();
-          disable_e1();
-          disable_e2();
-          finishAndDisableSteppers();
-        }
-        else
-        {
-          st_synchronize();
-          if(code_seen('X')) disable_x();
-          if(code_seen('Y')) disable_y();
-          if(code_seen('Z')) disable_z();
-          #if ((E0_ENABLE_PIN != X_ENABLE_PIN) && (E1_ENABLE_PIN != Y_ENABLE_PIN)) // Only enable on boards that have seperate ENABLE_PINS
-            if(code_seen('E')) {
-              disable_e0();
-              disable_e1();
-              disable_e2();
-            }
-          #endif 
-          LCD_MESSAGEPGM(MSG_PART_RELEASE);
-        }
-      }
-      break;
-    case 85: // M85
-      code_seen('S');
-      max_inactive_time = code_value() * 1000; 
-      break;
-    case 92: // M92
-      for(int8_t i=0; i < NUM_AXIS; i++) 
-      {
-        if(code_seen(axis_codes[i])) 
-          
-          if(i == 3) { // E
-            float value = code_value();
-            if(value < 20.0) {
-              float factor = axis_steps_per_unit[i] / value; // increase e constants if M92 E14 is given for netfab.
-              max_e_jerk *= factor;
-              max_feedrate[i] *= factor;
-              axis_steps_per_sqr_second[i] *= factor;
-            }
-            axis_steps_per_unit[i] = value;
-          }
-          else {
-            axis_steps_per_unit[i] = code_value();
-          }
-      }
-      break;
-    case 115: // M115
-      SerialprintPGM(MSG_M115_REPORT);
-      break;
-    case 117: // M117 display message
-      LCD_MESSAGE(cmdbuffer[bufindr]+5);
-      break;
-    case 114: // M114
-      SERIAL_PROTOCOLPGM("X:");
-      SERIAL_PROTOCOL(current_position[X_AXIS]);
-      SERIAL_PROTOCOLPGM("Y:");
-      SERIAL_PROTOCOL(current_position[Y_AXIS]);
-      SERIAL_PROTOCOLPGM("Z:");
-      SERIAL_PROTOCOL(current_position[Z_AXIS]);
-      SERIAL_PROTOCOLPGM("E:");      
-      SERIAL_PROTOCOL(current_position[E_AXIS]);
-      
-      SERIAL_PROTOCOLPGM(MSG_COUNT_X);
-      SERIAL_PROTOCOL(float(st_get_position(X_AXIS))/axis_steps_per_unit[X_AXIS]);
-      SERIAL_PROTOCOLPGM("Y:");
-      SERIAL_PROTOCOL(float(st_get_position(Y_AXIS))/axis_steps_per_unit[Y_AXIS]);
-      SERIAL_PROTOCOLPGM("Z:");
-      SERIAL_PROTOCOL(float(st_get_position(Z_AXIS))/axis_steps_per_unit[Z_AXIS]);
-      
-      SERIAL_PROTOCOLLN("");
-      break;
-    case 120: // M120
-      enable_endstops(false) ;
-      break;
-    case 121: // M121
-      enable_endstops(true) ;
-      break;
-    case 119: // M119
-    SERIAL_PROTOCOLLN(MSG_M119_REPORT);
-      #if (X_MIN_PIN > -1)
-        SERIAL_PROTOCOLPGM(MSG_X_MIN);
-        SERIAL_PROTOCOLLN(((READ(X_MIN_PIN)^X_ENDSTOPS_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
-      #endif
-      #if (X_MAX_PIN > -1)
-        SERIAL_PROTOCOLPGM(MSG_X_MAX);
-        SERIAL_PROTOCOLLN(((READ(X_MAX_PIN)^X_ENDSTOPS_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
-      #endif
-      #if (Y_MIN_PIN > -1)
-        SERIAL_PROTOCOLPGM(MSG_Y_MIN);
-        SERIAL_PROTOCOLLN(((READ(Y_MIN_PIN)^Y_ENDSTOPS_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
-      #endif
-      #if (Y_MAX_PIN > -1)
-        SERIAL_PROTOCOLPGM(MSG_Y_MAX);
-        SERIAL_PROTOCOLLN(((READ(Y_MAX_PIN)^Y_ENDSTOPS_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
-      #endif
-      #if (Z_MIN_PIN > -1)
-        SERIAL_PROTOCOLPGM(MSG_Z_MIN);
-        SERIAL_PROTOCOLLN(((READ(Z_MIN_PIN)^Z_ENDSTOPS_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
-      #endif
-      #if (Z_MAX_PIN > -1)
-        SERIAL_PROTOCOLPGM(MSG_Z_MAX);
-        SERIAL_PROTOCOLLN(((READ(Z_MAX_PIN)^Z_ENDSTOPS_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
-      #endif
-      break;
-      //TODO: update for all axis, use for loop
-    case 201: // M201
-      for(int8_t i=0; i < NUM_AXIS; i++) 
-      {
-        if(code_seen(axis_codes[i]))
-        {
-          max_acceleration_units_per_sq_second[i] = code_value();
-          axis_steps_per_sqr_second[i] = code_value() * axis_steps_per_unit[i];
-        }
-      }
-      break;
-    #if 0 // Not used for Sprinter/grbl gen6
-    case 202: // M202
-      for(int8_t i=0; i < NUM_AXIS; i++) {
-        if(code_seen(axis_codes[i])) axis_travel_steps_per_sqr_second[i] = code_value() * axis_steps_per_unit[i];
-      }
-      break;
-    #endif
-    case 203: // M203 max feedrate mm/sec
-      for(int8_t i=0; i < NUM_AXIS; i++) {
-        if(code_seen(axis_codes[i])) max_feedrate[i] = code_value();
-      }
-      break;
-    case 204: // M204 acclereration S normal moves T filmanent only moves
-      {
-        if(code_seen('S')) acceleration = code_value() ;
-        if(code_seen('T')) retract_acceleration = code_value() ;
-      }
-      break;
-    case 205: //M205 advanced settings:  minimum travel speed S=while printing T=travel only,  B=minimum segment time X= maximum xy jerk, Z=maximum Z jerk
-    {
-      if(code_seen('S')) minimumfeedrate = code_value();
-      if(code_seen('T')) mintravelfeedrate = code_value();
-      if(code_seen('B')) minsegmenttime = code_value() ;
-      if(code_seen('X')) max_xy_jerk = code_value() ;
-      if(code_seen('Z')) max_z_jerk = code_value() ;
-      if(code_seen('E')) max_e_jerk = code_value() ;
-    }
-    break;
-    case 206: // M206 additional homeing offset
-      for(int8_t i=0; i < 3; i++) 
-      {
-        if(code_seen(axis_codes[i])) add_homeing[i] = code_value();
-      }
-      break;
-    #ifdef FWRETRACT
-    case 207: //M207 - set retract length S[positive mm] F[feedrate mm/sec] Z[additional zlift/hop]
-    {
-      if(code_seen('S')) 
-      {
-        retract_length = code_value() ;
-      }
-      if(code_seen('F')) 
-      {
-        retract_feedrate = code_value() ;
-      }
-      if(code_seen('Z')) 
-      {
-        retract_zlift = code_value() ;
-      }
-    }break;
-    case 208: // M208 - set retract recover length S[positive mm surplus to the M207 S*] F[feedrate mm/sec]
-    {
-      if(code_seen('S')) 
-      {
-        retract_recover_length = code_value() ;
-      }
-      if(code_seen('F')) 
-      {
-        retract_recover_feedrate = code_value() ;
-      }
-    }break;
-    
-    case 209: // M209 - S<1=true/0=false> enable automatic retract detect if the slicer did not support G10/11: every normal extrude-only move will be classified as retract depending on the direction.
-    {
-      if(code_seen('S')) 
-      {
-        int t= code_value() ;
-        switch(t)
-        {
-          case 0: autoretract_enabled=false;retracted=false;break;
-          case 1: autoretract_enabled=true;retracted=false;break;
-          default: 
-            SERIAL_ECHO_START;
-            SERIAL_ECHOPGM(MSG_UNKNOWN_COMMAND);
-            SERIAL_ECHO(cmdbuffer[bufindr]);
-            SERIAL_ECHOLNPGM("\"");
-        }
-      }
-      
-    }break;
-    #endif
-    case 220: // M220 S<factor in percent>- set speed factor override percentage
-    {
-      if(code_seen('S')) 
-      {
-        feedmultiply = code_value() ;
-        feedmultiplychanged=true;
-      }
-    }
-    break;
-    case 221: // M221 S<factor in percent>- set extrude factor override percentage
-    {
-      if(code_seen('S')) 
-      {
-        extrudemultiply = code_value() ;
-      }
-    }
-    break;
-
-    #ifdef PIDTEMP
-    case 301: // M301
-      {
-        if(code_seen('P')) Kp = code_value();
-        if(code_seen('I')) Ki = code_value()*PID_dT;
-        if(code_seen('D')) Kd = code_value()/PID_dT;
-        #ifdef PID_ADD_EXTRUSION_RATE
-        if(code_seen('C')) Kc = code_value();
-        #endif
-        updatePID();
-        SERIAL_PROTOCOL(MSG_OK);
-		SERIAL_PROTOCOL(" p:");
-        SERIAL_PROTOCOL(Kp);
-        SERIAL_PROTOCOL(" i:");
-        SERIAL_PROTOCOL(Ki/PID_dT);
-        SERIAL_PROTOCOL(" d:");
-        SERIAL_PROTOCOL(Kd*PID_dT);
-        #ifdef PID_ADD_EXTRUSION_RATE
-        SERIAL_PROTOCOL(" c:");
-        SERIAL_PROTOCOL(Kc*PID_dT);
-        #endif
-        SERIAL_PROTOCOLLN("");
-      }
-      break;
-    #endif //PIDTEMP
-    #ifdef PIDTEMPBED
-    case 304: // M304
-      {
-        if(code_seen('P')) bedKp = code_value();
-        if(code_seen('I')) bedKi = code_value()*PID_dT;
-        if(code_seen('D')) bedKd = code_value()/PID_dT;
-        updatePID();
-        SERIAL_PROTOCOL(MSG_OK);
-		SERIAL_PROTOCOL(" p:");
-        SERIAL_PROTOCOL(bedKp);
-        SERIAL_PROTOCOL(" i:");
-        SERIAL_PROTOCOL(bedKi/PID_dT);
-        SERIAL_PROTOCOL(" d:");
-        SERIAL_PROTOCOL(bedKd*PID_dT);
-        SERIAL_PROTOCOLLN("");
-      }
-      break;
-    #endif //PIDTEMP
-    case 240: // M240  Triggers a camera by emulating a Canon RC-1 : http://www.doc-diy.net/photo/rc-1_hacked/
-     {
-      #ifdef PHOTOGRAPH_PIN
-        #if (PHOTOGRAPH_PIN > -1)
-        const uint8_t NUM_PULSES=16;
-        const float PULSE_LENGTH=0.01524;
-        for(int i=0; i < NUM_PULSES; i++) {
-          WRITE(PHOTOGRAPH_PIN, HIGH);
-          _delay_ms(PULSE_LENGTH);
-          WRITE(PHOTOGRAPH_PIN, LOW);
-          _delay_ms(PULSE_LENGTH);
-        }
-        delay(7.33);
-        for(int i=0; i < NUM_PULSES; i++) {
-          WRITE(PHOTOGRAPH_PIN, HIGH);
-          _delay_ms(PULSE_LENGTH);
-          WRITE(PHOTOGRAPH_PIN, LOW);
-          _delay_ms(PULSE_LENGTH);
-        }
-        #endif
-      #endif
-     }
-    break;
-      
-    case 302: // allow cold extrudes
-    {
-      allow_cold_extrudes(true);
-    }
-    break;
-    case 303: // M303 PID autotune
-    {
-      float temp = 150.0;
-      int e=0;
-      int c=5;
-      if (code_seen('E')) e=code_value();
-			if (e<0)
-				temp=70;
-      if (code_seen('S')) temp=code_value();
-      if (code_seen('C')) c=code_value();
-      PID_autotune(temp, e, c);
-    }
-    break;
-    case 400: // M400 finish all moves
-    {
-      st_synchronize();
-    }
-    break;
-    case 500: // Store settings in EEPROM
-    {
-        EEPROM_StoreSettings();
-    }
-    break;
-    case 501: // Read settings from EEPROM
-    {
-      EEPROM_RetrieveSettings();
-    }
-    break;
-    case 502: // Revert to default settings
-    {
-      EEPROM_RetrieveSettings(true);
-    }
-    break;
-    case 503: // print settings currently in memory
-    {
-      EEPROM_printSettings();
-    }
-    break;
-    case 999: // Restart after being stopped
-      Stopped = false;
-      gcode_LastN = Stopped_gcode_LastN;
-      FlushSerialRequestResend();
-    break;
-    }
-  }
-
-  else if(code_seen('T')) 
-  {
-    tmp_extruder = code_value();
-    if(tmp_extruder >= EXTRUDERS) {
-      SERIAL_ECHO_START;
-      SERIAL_ECHO("T");
-      SERIAL_ECHO(tmp_extruder);
-      SERIAL_ECHOLN(MSG_INVALID_EXTRUDER);
-    }
-    else {
-      active_extruder = tmp_extruder;
-      SERIAL_ECHO_START;
-      SERIAL_ECHO(MSG_ACTIVE_EXTRUDER);
-      SERIAL_PROTOCOLLN((int)active_extruder);
-    }
-  }
-
-  else
-  {
-    SERIAL_ECHO_START;
-    SERIAL_ECHOPGM(MSG_UNKNOWN_COMMAND);
-    SERIAL_ECHO(cmdbuffer[bufindr]);
-    SERIAL_ECHOLNPGM("\"");
-  }
-
-  ClearToSend();
-}
-
-void FlushSerialRequestResend()
-{
-  //char cmdbuffer[bufindr][100]="Resend:";
-  MYSERIAL.flush();
-  SERIAL_PROTOCOLPGM(MSG_RESEND);
-  SERIAL_PROTOCOLLN(gcode_LastN + 1);
-  ClearToSend();
-}
-
-void ClearToSend()
-{
-  previous_millis_cmd = millis();
-  #ifdef SDSUPPORT
-  if(fromsd[bufindr])
-    return;
-  #endif //SDSUPPORT
-  SERIAL_PROTOCOLLNPGM(MSG_OK); 
-}
-
-void get_coordinates()
-{
-  bool seen[4]={false,false,false,false};
-  for(int8_t i=0; i < NUM_AXIS; i++) {
-    if(code_seen(axis_codes[i])) 
-    {
-      destination[i] = (float)code_value() + (axis_relative_modes[i] || relative_mode)*current_position[i];
-      seen[i]=true;
-    }
-    else destination[i] = current_position[i]; //Are these else lines really needed?
-  }
-  if(code_seen('F')) {
-    next_feedrate = code_value();
-    if(next_feedrate > 0.0) feedrate = next_feedrate;
-  }
-  #ifdef FWRETRACT
-  if(autoretract_enabled)
-  if( !(seen[X_AXIS] || seen[Y_AXIS] || seen[Z_AXIS]) && seen[E_AXIS])
-  {
-    float echange=destination[E_AXIS]-current_position[E_AXIS];
-    if(echange<-MIN_RETRACT) //retract
-    {
-      if(!retracted) 
-      {
-      
-      destination[Z_AXIS]+=retract_zlift; //not sure why chaninging current_position negatively does not work.
-      //if slicer retracted by echange=-1mm and you want to retract 3mm, corrrectede=-2mm additionally
-      float correctede=-echange-retract_length;
-      //to generate the additional steps, not the destination is changed, but inversely the current position
-      current_position[E_AXIS]+=-correctede; 
-      feedrate=retract_feedrate;
-      retracted=true;
-      }
-      
-    }
-    else 
-      if(echange>MIN_RETRACT) //retract_recover
-    {
-      if(retracted) 
-      {
-      //current_position[Z_AXIS]+=-retract_zlift;
-      //if slicer retracted_recovered by echange=+1mm and you want to retract_recover 3mm, corrrectede=2mm additionally
-      float correctede=-echange+1*retract_length+retract_recover_length; //total unretract=retract_length+retract_recover_length[surplus]
-      current_position[E_AXIS]+=correctede; //to generate the additional steps, not the destination is changed, but inversely the current position
-      feedrate=retract_recover_feedrate;
-      retracted=false;
-      }
-    }
-    
-  }
-  #endif //FWRETRACT
-}
-
-void get_arc_coordinates()
-{
-#ifdef SF_ARC_FIX
-   bool relative_mode_backup = relative_mode;
-   relative_mode = true;
-#endif
-   get_coordinates();
-#ifdef SF_ARC_FIX
-   relative_mode=relative_mode_backup;
-#endif
-
-   if(code_seen('I')) {
-     offset[0] = code_value();
-   } 
-   else {
-     offset[0] = 0.0;
-   }
-   if(code_seen('J')) {
-     offset[1] = code_value();
-   }
-   else {
-     offset[1] = 0.0;
-   }
-}
-
-void clamp_to_software_endstops(float target[3])
-{
-  if (min_software_endstops) {
-    if (target[X_AXIS] < min_pos[X_AXIS]) target[X_AXIS] = min_pos[X_AXIS];
-    if (target[Y_AXIS] < min_pos[Y_AXIS]) target[Y_AXIS] = min_pos[Y_AXIS];
-    if (target[Z_AXIS] < min_pos[Z_AXIS]) target[Z_AXIS] = min_pos[Z_AXIS];
-  }
-
-  if (max_software_endstops) {
-    if (target[X_AXIS] > max_pos[X_AXIS]) target[X_AXIS] = max_pos[X_AXIS];
-    if (target[Y_AXIS] > max_pos[Y_AXIS]) target[Y_AXIS] = max_pos[Y_AXIS];
-    if (target[Z_AXIS] > max_pos[Z_AXIS]) target[Z_AXIS] = max_pos[Z_AXIS];
-  }
-}
-
-void prepare_move()
-{
-  clamp_to_software_endstops(destination);
-
-  previous_millis_cmd = millis(); 
-  // Do not use feedmultiply for E or Z only moves
-  if( (current_position[X_AXIS] == destination [X_AXIS]) && (current_position[Y_AXIS] == destination [Y_AXIS])) {
-      plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
-  }
-  else {
-    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder);
-  }
-  for(int8_t i=0; i < NUM_AXIS; i++) {
-    current_position[i] = destination[i];
-  }
-}
-
-void prepare_arc_move(char isclockwise) {
-  float r = hypot(offset[X_AXIS], offset[Y_AXIS]); // Compute arc radius for mc_arc
-
-  // Trace the arc
-  mc_arc(current_position, destination, offset, X_AXIS, Y_AXIS, Z_AXIS, feedrate*feedmultiply/60/100.0, r, isclockwise, active_extruder);
-  
-  // As far as the parser is concerned, the position is now == target. In reality the
-  // motion control system might still be processing the action and the real tool position
-  // in any intermediate location.
-  for(int8_t i=0; i < NUM_AXIS; i++) {
-    current_position[i] = destination[i];
-  }
-  previous_millis_cmd = millis();
-}
-
-#ifdef CONTROLLERFAN_PIN
-unsigned long lastMotor = 0; //Save the time for when a motor was turned on last
-unsigned long lastMotorCheck = 0;
-
-void controllerFan()
-{
-  if ((millis() - lastMotorCheck) >= 2500) //Not a time critical function, so we only check every 2500ms
-  {
-    lastMotorCheck = millis();
-    
-    if(!READ(X_ENABLE_PIN) || !READ(Y_ENABLE_PIN) || !READ(Z_ENABLE_PIN)
-    #if EXTRUDERS > 2
-       || !READ(E2_ENABLE_PIN)
-    #endif
-    #if EXTRUDER > 1
-       || !READ(E2_ENABLE_PIN)
-    #endif
-       || !READ(E0_ENABLE_PIN)) //If any of the drivers are enabled...    
-    {
-      lastMotor = millis(); //... set time to NOW so the fan will turn on
-    }
-    
-    if ((millis() - lastMotor) >= (CONTROLLERFAN_SEC*1000UL) || lastMotor == 0) //If the last time any driver was enabled, is longer since than CONTROLLERSEC...   
-    {
-      WRITE(CONTROLLERFAN_PIN, LOW); //... turn the fan off
-    }
-    else
-    {
-      WRITE(CONTROLLERFAN_PIN, HIGH); //... turn the fan on
-    }
-  }
-}
-#endif
-
-void manage_inactivity() 
-{ 
-  if( (millis() - previous_millis_cmd) >  max_inactive_time ) 
-    if(max_inactive_time) 
-      kill(); 
-  if(stepper_inactive_time)  {
-    if( (millis() - previous_millis_cmd) >  stepper_inactive_time ) 
-    {
-      if(blocks_queued() == false) {
-        disable_x();
-        disable_y();
-        disable_z();
-        disable_e0();
-        disable_e1();
-        disable_e2();
-      }
-    }
-  }
-  #if( KILL_PIN>-1 )
-    if( 0 == READ(KILL_PIN) )
-      kill();
-  #endif
-  #ifdef CONTROLLERFAN_PIN
-    controllerFan(); //Check if fan should be turned on to cool stepper drivers down
-  #endif
-  #ifdef EXTRUDER_RUNOUT_PREVENT
-    if( (millis() - previous_millis_cmd) >  EXTRUDER_RUNOUT_SECONDS*1000 ) 
-    if(degHotend(active_extruder)>EXTRUDER_RUNOUT_MINTEMP)
-    {
-     bool oldstatus=READ(E0_ENABLE_PIN);
-     enable_e0();
-     float oldepos=current_position[E_AXIS];
-     float oldedes=destination[E_AXIS];
-     plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], 
-                      current_position[E_AXIS]+EXTRUDER_RUNOUT_EXTRUDE*EXTRUDER_RUNOUT_ESTEPS/axis_steps_per_unit[E_AXIS], 
-                      EXTRUDER_RUNOUT_SPEED/60.*EXTRUDER_RUNOUT_ESTEPS/axis_steps_per_unit[E_AXIS], active_extruder);
-     current_position[E_AXIS]=oldepos;
-     destination[E_AXIS]=oldedes;
-     plan_set_e_position(oldepos);
-     previous_millis_cmd=millis();
-     st_synchronize();
-     WRITE(E0_ENABLE_PIN,oldstatus);
-    }
-  #endif
-  check_axes_activity();
-}
-
-void kill()
-{
-  cli(); // Stop interrupts
-  disable_heater();
-
-  disable_x();
-  disable_y();
-  disable_z();
-  disable_e0();
-  disable_e1();
-  disable_e2();
-  
-  if(PS_ON_PIN > -1) pinMode(PS_ON_PIN,INPUT);
-  SERIAL_ERROR_START;
-  SERIAL_ERRORLNPGM(MSG_ERR_KILLED);
-  LCD_ALERTMESSAGEPGM(MSG_KILLED);
-  suicide();
-  while(1); // Wait for reset
-}
-
-void Stop()
-{
-  disable_heater();
-  if(Stopped == false) {
-    Stopped = true;
-    Stopped_gcode_LastN = gcode_LastN; // Save last g_code for restart
-    SERIAL_ERROR_START;
-    SERIAL_ERRORLNPGM(MSG_ERR_STOPPED);
-    LCD_MESSAGEPGM(MSG_STOPPED);
-  }
-}
-
-bool IsStopped() { return Stopped; };
-
-#ifdef FAST_PWM_FAN
-void setPwmFrequency(uint8_t pin, int val)
-{
-  val &= 0x07;
-  switch(digitalPinToTimer(pin))
-  {
- 
-    #if defined(TCCR0A)
-    case TIMER0A:
-    case TIMER0B:
-//         TCCR0B &= ~(CS00 | CS01 | CS02);
-//         TCCR0B |= val;
-         break;
-    #endif
-
-    #if defined(TCCR1A)
-    case TIMER1A:
-    case TIMER1B:
-//         TCCR1B &= ~(CS10 | CS11 | CS12);
-//         TCCR1B |= val;
-         break;
-    #endif
-
-    #if defined(TCCR2)
-    case TIMER2:
-    case TIMER2:
-         TCCR2 &= ~(CS10 | CS11 | CS12);
-         TCCR2 |= val;
-         break;
-    #endif
-
-    #if defined(TCCR2A)
-    case TIMER2A:
-    case TIMER2B:
-         TCCR2B &= ~(CS20 | CS21 | CS22);
-         TCCR2B |= val;
-         break;
-    #endif
-
-    #if defined(TCCR3A)
-    case TIMER3A:
-    case TIMER3B:
-    case TIMER3C:
-         TCCR3B &= ~(CS30 | CS31 | CS32);
-         TCCR3B |= val;
-         break;
-    #endif
-
-    #if defined(TCCR4A) 
-    case TIMER4A:
-    case TIMER4B:
-    case TIMER4C:
-         TCCR4B &= ~(CS40 | CS41 | CS42);
-         TCCR4B |= val;
-         break;
-   #endif
-
-    #if defined(TCCR5A) 
-    case TIMER5A:
-    case TIMER5B:
-    case TIMER5C:
-         TCCR5B &= ~(CS50 | CS51 | CS52);
-         TCCR5B |= val;
-         break;
-   #endif
-
-  }
-}
-#endif //FAST_PWM_FAN
-
-bool setTargetedHotend(int code){
-  tmp_extruder = active_extruder;
-  if(code_seen('T')) {
-    tmp_extruder = code_value();
-    if(tmp_extruder >= EXTRUDERS) {
-      SERIAL_ECHO_START;
-      switch(code){
-        case 104:
-          SERIAL_ECHO(MSG_M104_INVALID_EXTRUDER);
-          break;
-        case 105:
-          SERIAL_ECHO(MSG_M105_INVALID_EXTRUDER);
-          break;
-        case 109:
-          SERIAL_ECHO(MSG_M109_INVALID_EXTRUDER);
-          break;
-      }
-      SERIAL_ECHOLN(tmp_extruder);
-      return true;
-    }
-  }
-  return false;
-}
+#include <LiquidCrystal.h>
+#endif
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
new file mode 100644
index 0000000000..ed222b8cf1
--- /dev/null
+++ b/Marlin/Marlin_main.cpp
@@ -0,0 +1,1865 @@
+/* -*- c++ -*- */
+
+/*
+    Reprap firmware based on Sprinter and grbl.
+ Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ 
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+ 
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ GNU General Public License for more details.
+ 
+ You should have received a copy of the GNU General Public License
+ along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ This firmware is a mashup between Sprinter and grbl.
+  (https://github.com/kliment/Sprinter)
+  (https://github.com/simen/grbl/tree)
+ 
+ It has preliminary support for Matthew Roberts advance algorithm 
+    http://reprap.org/pipermail/reprap-dev/2011-May/003323.html
+ */
+
+#include "Marlin.h"
+
+#include "ultralcd.h"
+#include "planner.h"
+#include "stepper.h"
+#include "temperature.h"
+#include "motion_control.h"
+#include "cardreader.h"
+#include "watchdog.h"
+#include "EEPROMwrite.h"
+#include "language.h"
+#include "pins_arduino.h"
+
+#define VERSION_STRING  "1.0.0"
+
+// look here for descriptions of gcodes: http://linuxcnc.org/handbook/gcode/g-code.html
+// http://objects.reprap.org/wiki/Mendel_User_Manual:_RepRapGCodes
+
+//Implemented Codes
+//-------------------
+// G0  -> G1
+// G1  - Coordinated Movement X Y Z E
+// G2  - CW ARC
+// G3  - CCW ARC
+// G4  - Dwell S<seconds> or P<milliseconds>
+// G10 - retract filament according to settings of M207
+// G11 - retract recover filament according to settings of M208
+// G28 - Home all Axis
+// G90 - Use Absolute Coordinates
+// G91 - Use Relative Coordinates
+// G92 - Set current position to cordinates given
+
+//RepRap M Codes
+// M0   - Unconditional stop - Wait for user to press a button on the LCD (Only if ULTRA_LCD is enabled)
+// M1   - Same as M0
+// M104 - Set extruder target temp
+// M105 - Read current temp
+// M106 - Fan on
+// M107 - Fan off
+// M109 - Wait for extruder current temp to reach target temp.
+// M114 - Display current position
+
+//Custom M Codes
+// M17  - Enable/Power all stepper motors
+// M18  - Disable all stepper motors; same as M84
+// M20  - List SD card
+// M21  - Init SD card
+// M22  - Release SD card
+// M23  - Select SD file (M23 filename.g)
+// M24  - Start/resume SD print
+// M25  - Pause SD print
+// M26  - Set SD position in bytes (M26 S12345)
+// M27  - Report SD print status
+// M28  - Start SD write (M28 filename.g)
+// M29  - Stop SD write
+// M30  - Delete file from SD (M30 filename.g)
+// M31  - Output time since last M109 or SD card start to serial
+// M42  - Change pin status via gcode
+// M80  - Turn on Power Supply
+// M81  - Turn off Power Supply
+// M82  - Set E codes absolute (default)
+// M83  - Set E codes relative while in Absolute Coordinates (G90) mode
+// M84  - Disable steppers until next move, 
+//        or use S<seconds> to specify an inactivity timeout, after which the steppers will be disabled.  S0 to disable the timeout.
+// M85  - Set inactivity shutdown timer with parameter S<seconds>. To disable set zero (default)
+// M92  - Set axis_steps_per_unit - same syntax as G92
+// M114 - Output current position to serial port 
+// M115	- Capabilities string
+// M117 - display message
+// M119 - Output Endstop status to serial port
+// M140 - Set bed target temp
+// M190 - Wait for bed current temp to reach target temp.
+// M200 - Set filament diameter
+// M201 - Set max acceleration in units/s^2 for print moves (M201 X1000 Y1000)
+// M202 - Set max acceleration in units/s^2 for travel moves (M202 X1000 Y1000) Unused in Marlin!!
+// M203 - Set maximum feedrate that your machine can sustain (M203 X200 Y200 Z300 E10000) in mm/sec
+// M204 - Set default acceleration: S normal moves T filament only moves (M204 S3000 T7000) im mm/sec^2  also sets minimum segment time in ms (B20000) to prevent buffer underruns and M20 minimum feedrate
+// M205 -  advanced settings:  minimum travel speed S=while printing T=travel only,  B=minimum segment time X= maximum xy jerk, Z=maximum Z jerk, E=maximum E jerk
+// M206 - set additional homeing offset
+// M207 - set retract length S[positive mm] F[feedrate mm/sec] Z[additional zlift/hop]
+// M208 - set recover=unretract length S[positive mm surplus to the M207 S*] F[feedrate mm/sec]
+// M209 - S<1=true/0=false> enable automatic retract detect if the slicer did not support G10/11: every normal extrude-only move will be classified as retract depending on the direction.
+// M220 S<factor in percent>- set speed factor override percentage
+// M221 S<factor in percent>- set extrude factor override percentage
+// M240 - Trigger a camera to take a photograph
+// M301 - Set PID parameters P I and D
+// M302 - Allow cold extrudes
+// M303 - PID relay autotune S<temperature> sets the target temperature. (default target temperature = 150C)
+// M304 - Set bed PID parameters P I and D
+// M400 - Finish all moves
+// M500 - stores paramters in EEPROM
+// M501 - reads parameters from EEPROM (if you need reset them after you changed them temporarily).  
+// M502 - reverts to the default "factory settings".  You still need to store them in EEPROM afterwards if you want to.
+// M503 - print the current settings (from memory not from eeprom)
+// M999 - Restart after being stopped by error
+
+//Stepper Movement Variables
+
+//===========================================================================
+//=============================imported variables============================
+//===========================================================================
+
+
+//===========================================================================
+//=============================public variables=============================
+//===========================================================================
+#ifdef SDSUPPORT
+CardReader card;
+#endif
+float homing_feedrate[] = HOMING_FEEDRATE;
+bool axis_relative_modes[] = AXIS_RELATIVE_MODES;
+volatile int feedmultiply=100; //100->1 200->2
+int saved_feedmultiply;
+volatile bool feedmultiplychanged=false;
+volatile int extrudemultiply=100; //100->1 200->2
+float current_position[NUM_AXIS] = { 0.0, 0.0, 0.0, 0.0 };
+float add_homeing[3]={0,0,0};
+float min_pos[3] = { X_MIN_POS, Y_MIN_POS, Z_MIN_POS };
+float max_pos[3] = { X_MAX_POS, Y_MAX_POS, Z_MAX_POS };
+uint8_t active_extruder = 0;
+unsigned char FanSpeed=0;
+
+#ifdef FWRETRACT
+  bool autoretract_enabled=true;
+  bool retracted=false;
+  float retract_length=3, retract_feedrate=17*60, retract_zlift=0.8;
+  float retract_recover_length=0, retract_recover_feedrate=8*60;
+#endif
+
+//===========================================================================
+//=============================private variables=============================
+//===========================================================================
+const char axis_codes[NUM_AXIS] = {'X', 'Y', 'Z', 'E'};
+static float destination[NUM_AXIS] = {  0.0, 0.0, 0.0, 0.0};
+static float offset[3] = {0.0, 0.0, 0.0};
+static bool home_all_axis = true;
+static float feedrate = 1500.0, next_feedrate, saved_feedrate;
+static long gcode_N, gcode_LastN, Stopped_gcode_LastN = 0;
+
+static bool relative_mode = false;  //Determines Absolute or Relative Coordinates
+
+static char cmdbuffer[BUFSIZE][MAX_CMD_SIZE];
+static bool fromsd[BUFSIZE];
+static int bufindr = 0;
+static int bufindw = 0;
+static int buflen = 0;
+//static int i = 0;
+static char serial_char;
+static int serial_count = 0;
+static boolean comment_mode = false;
+static char *strchr_pointer; // just a pointer to find chars in the cmd string like X, Y, Z, E, etc
+
+const int sensitive_pins[] = SENSITIVE_PINS; // Sensitive pin list for M42
+
+//static float tt = 0;
+//static float bt = 0;
+
+//Inactivity shutdown variables
+static unsigned long previous_millis_cmd = 0;
+static unsigned long max_inactive_time = 0;
+static unsigned long stepper_inactive_time = DEFAULT_STEPPER_DEACTIVE_TIME*1000l;
+
+unsigned long starttime=0;
+unsigned long stoptime=0;
+
+static uint8_t tmp_extruder;
+
+
+bool Stopped=false;
+
+//===========================================================================
+//=============================ROUTINES=============================
+//===========================================================================
+
+void get_arc_coordinates();
+bool setTargetedHotend(int code);
+
+void serial_echopair_P(const char *s_P, float v)
+    { serialprintPGM(s_P); SERIAL_ECHO(v); }
+void serial_echopair_P(const char *s_P, double v)
+    { serialprintPGM(s_P); SERIAL_ECHO(v); }
+void serial_echopair_P(const char *s_P, unsigned long v)
+    { serialprintPGM(s_P); SERIAL_ECHO(v); }
+
+extern "C"{
+  extern unsigned int __bss_end;
+  extern unsigned int __heap_start;
+  extern void *__brkval;
+
+  int freeMemory() {
+    int free_memory;
+
+    if((int)__brkval == 0)
+      free_memory = ((int)&free_memory) - ((int)&__bss_end);
+    else
+      free_memory = ((int)&free_memory) - ((int)__brkval);
+
+    return free_memory;
+  }
+}
+
+//adds an command to the main command buffer
+//thats really done in a non-safe way.
+//needs overworking someday
+void enquecommand(const char *cmd)
+{
+  if(buflen < BUFSIZE)
+  {
+    //this is dangerous if a mixing of serial and this happsens
+    strcpy(&(cmdbuffer[bufindw][0]),cmd);
+    SERIAL_ECHO_START;
+    SERIAL_ECHOPGM("enqueing \"");
+    SERIAL_ECHO(cmdbuffer[bufindw]);
+    SERIAL_ECHOLNPGM("\"");
+    bufindw= (bufindw + 1)%BUFSIZE;
+    buflen += 1;
+  }
+}
+
+void setup_killpin()
+{
+  #if( KILL_PIN>-1 )
+    pinMode(KILL_PIN,INPUT);
+    WRITE(KILL_PIN,HIGH);
+  #endif
+}
+    
+void setup_photpin()
+{
+  #ifdef PHOTOGRAPH_PIN
+    #if (PHOTOGRAPH_PIN > -1)
+    SET_OUTPUT(PHOTOGRAPH_PIN);
+    WRITE(PHOTOGRAPH_PIN, LOW);
+    #endif
+  #endif 
+}
+
+void setup_powerhold()
+{
+ #ifdef SUICIDE_PIN
+   #if (SUICIDE_PIN> -1)
+      SET_OUTPUT(SUICIDE_PIN);
+      WRITE(SUICIDE_PIN, HIGH);
+   #endif
+ #endif
+}
+
+void suicide()
+{
+ #ifdef SUICIDE_PIN
+    #if (SUICIDE_PIN> -1) 
+      SET_OUTPUT(SUICIDE_PIN);
+      WRITE(SUICIDE_PIN, LOW);
+    #endif
+  #endif
+}
+
+void setup()
+{
+  setup_killpin(); 
+  setup_powerhold();
+  MYSERIAL.begin(BAUDRATE);
+  SERIAL_PROTOCOLLNPGM("start");
+  SERIAL_ECHO_START;
+
+  // Check startup - does nothing if bootloader sets MCUSR to 0
+  byte mcu = MCUSR;
+  if(mcu & 1) SERIAL_ECHOLNPGM(MSG_POWERUP);
+  if(mcu & 2) SERIAL_ECHOLNPGM(MSG_EXTERNAL_RESET);
+  if(mcu & 4) SERIAL_ECHOLNPGM(MSG_BROWNOUT_RESET);
+  if(mcu & 8) SERIAL_ECHOLNPGM(MSG_WATCHDOG_RESET);
+  if(mcu & 32) SERIAL_ECHOLNPGM(MSG_SOFTWARE_RESET);
+  MCUSR=0;
+
+  SERIAL_ECHOPGM(MSG_MARLIN);
+  SERIAL_ECHOLNPGM(VERSION_STRING);
+  #ifdef STRING_VERSION_CONFIG_H
+    #ifdef STRING_CONFIG_H_AUTHOR
+      SERIAL_ECHO_START;
+      SERIAL_ECHOPGM(MSG_CONFIGURATION_VER);
+      SERIAL_ECHOPGM(STRING_VERSION_CONFIG_H);
+      SERIAL_ECHOPGM(MSG_AUTHOR);
+      SERIAL_ECHOLNPGM(STRING_CONFIG_H_AUTHOR);
+    #endif
+  #endif
+  SERIAL_ECHO_START;
+  SERIAL_ECHOPGM(MSG_FREE_MEMORY);
+  SERIAL_ECHO(freeMemory());
+  SERIAL_ECHOPGM(MSG_PLANNER_BUFFER_BYTES);
+  SERIAL_ECHOLN((int)sizeof(block_t)*BLOCK_BUFFER_SIZE);
+  for(int8_t i = 0; i < BUFSIZE; i++)
+  {
+    fromsd[i] = false;
+  }
+  
+  EEPROM_RetrieveSettings(); // loads data from EEPROM if available
+
+  for(int8_t i=0; i < NUM_AXIS; i++)
+  {
+    axis_steps_per_sqr_second[i] = max_acceleration_units_per_sq_second[i] * axis_steps_per_unit[i];
+  }
+
+
+  tp_init();    // Initialize temperature loop 
+  plan_init();  // Initialize planner;
+  watchdog_init();
+  st_init();    // Initialize stepper, this enables interrupts!
+  setup_photpin();
+  
+  LCD_INIT;
+}
+
+
+void loop()
+{
+  if(buflen < (BUFSIZE-1))
+    get_command();
+  #ifdef SDSUPPORT
+  card.checkautostart(false);
+  #endif
+  if(buflen)
+  {
+    #ifdef SDSUPPORT
+      if(card.saving)
+      {
+	if(strstr(cmdbuffer[bufindr],"M29") == NULL)
+	{
+	  card.write_command(cmdbuffer[bufindr]);
+	  SERIAL_PROTOCOLLNPGM(MSG_OK);
+	}
+	else
+	{
+	  card.closefile();
+	  SERIAL_PROTOCOLLNPGM(MSG_FILE_SAVED);
+	}
+      }
+      else
+      {
+	process_commands();
+      }
+    #else
+      process_commands();
+    #endif //SDSUPPORT
+    buflen = (buflen-1);
+    bufindr = (bufindr + 1)%BUFSIZE;
+  }
+  //check heater every n milliseconds
+  manage_heater();
+  manage_inactivity();
+  checkHitEndstops();
+  LCD_STATUS;
+}
+
+void get_command() 
+{ 
+  while( MYSERIAL.available() > 0  && buflen < BUFSIZE) {
+    serial_char = MYSERIAL.read();
+    if(serial_char == '\n' || 
+       serial_char == '\r' || 
+       (serial_char == ':' && comment_mode == false) || 
+       serial_count >= (MAX_CMD_SIZE - 1) ) 
+    {
+      if(!serial_count) { //if empty line
+        comment_mode = false; //for new command
+        return;
+      }
+      cmdbuffer[bufindw][serial_count] = 0; //terminate string
+      if(!comment_mode){
+        comment_mode = false; //for new command
+        fromsd[bufindw] = false;
+        if(strstr(cmdbuffer[bufindw], "N") != NULL)
+        {
+          strchr_pointer = strchr(cmdbuffer[bufindw], 'N');
+          gcode_N = (strtol(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL, 10));
+          if(gcode_N != gcode_LastN+1 && (strstr(cmdbuffer[bufindw], "M110") == NULL) ) {
+            SERIAL_ERROR_START;
+            SERIAL_ERRORPGM(MSG_ERR_LINE_NO);
+            SERIAL_ERRORLN(gcode_LastN);
+            //Serial.println(gcode_N);
+            FlushSerialRequestResend();
+            serial_count = 0;
+            return;
+          }
+
+          if(strstr(cmdbuffer[bufindw], "*") != NULL)
+          {
+            byte checksum = 0;
+            byte count = 0;
+            while(cmdbuffer[bufindw][count] != '*') checksum = checksum^cmdbuffer[bufindw][count++];
+            strchr_pointer = strchr(cmdbuffer[bufindw], '*');
+
+            if( (int)(strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)) != checksum) {
+              SERIAL_ERROR_START;
+              SERIAL_ERRORPGM(MSG_ERR_CHECKSUM_MISMATCH);
+              SERIAL_ERRORLN(gcode_LastN);
+              FlushSerialRequestResend();
+              serial_count = 0;
+              return;
+            }
+            //if no errors, continue parsing
+          }
+          else 
+          {
+            SERIAL_ERROR_START;
+            SERIAL_ERRORPGM(MSG_ERR_NO_CHECKSUM);
+            SERIAL_ERRORLN(gcode_LastN);
+            FlushSerialRequestResend();
+            serial_count = 0;
+            return;
+          }
+
+          gcode_LastN = gcode_N;
+          //if no errors, continue parsing
+        }
+        else  // if we don't receive 'N' but still see '*'
+        {
+          if((strstr(cmdbuffer[bufindw], "*") != NULL))
+          {
+            SERIAL_ERROR_START;
+            SERIAL_ERRORPGM(MSG_ERR_NO_LINENUMBER_WITH_CHECKSUM);
+            SERIAL_ERRORLN(gcode_LastN);
+            serial_count = 0;
+            return;
+          }
+        }
+        if((strstr(cmdbuffer[bufindw], "G") != NULL)){
+          strchr_pointer = strchr(cmdbuffer[bufindw], 'G');
+          switch((int)((strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)))){
+          case 0:
+          case 1:
+          case 2:
+          case 3:
+            if(Stopped == false) { // If printer is stopped by an error the G[0-3] codes are ignored.
+	      #ifdef SDSUPPORT
+              if(card.saving)
+                break;
+	      #endif //SDSUPPORT
+              SERIAL_PROTOCOLLNPGM(MSG_OK); 
+            }
+            else {
+              SERIAL_ERRORLNPGM(MSG_ERR_STOPPED);
+              LCD_MESSAGEPGM(MSG_STOPPED);
+            }
+            break;
+          default:
+            break;
+          }
+
+        }
+        bufindw = (bufindw + 1)%BUFSIZE;
+        buflen += 1;
+      }
+      serial_count = 0; //clear buffer
+    }
+    else
+    {
+      if(serial_char == ';') comment_mode = true;
+      if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;
+    }
+  }
+  #ifdef SDSUPPORT
+  if(!card.sdprinting || serial_count!=0){
+    return;
+  }
+  while( !card.eof()  && buflen < BUFSIZE) {
+    int16_t n=card.get();
+    serial_char = (char)n;
+    if(serial_char == '\n' || 
+       serial_char == '\r' || 
+       (serial_char == ':' && comment_mode == false) || 
+       serial_count >= (MAX_CMD_SIZE - 1)||n==-1) 
+    {
+      if(card.eof()){
+        SERIAL_PROTOCOLLNPGM(MSG_FILE_PRINTED);
+        stoptime=millis();
+        char time[30];
+        unsigned long t=(stoptime-starttime)/1000;
+        int sec,min;
+        min=t/60;
+        sec=t%60;
+        sprintf(time,"%i min, %i sec",min,sec);
+        SERIAL_ECHO_START;
+        SERIAL_ECHOLN(time);
+        LCD_MESSAGE(time);
+        card.printingHasFinished();
+        card.checkautostart(true);
+        
+      }
+      if(!serial_count)
+      {
+        comment_mode = false; //for new command
+        return; //if empty line
+      }
+      cmdbuffer[bufindw][serial_count] = 0; //terminate string
+//      if(!comment_mode){
+        fromsd[bufindw] = true;
+        buflen += 1;
+        bufindw = (bufindw + 1)%BUFSIZE;
+//      }     
+      comment_mode = false; //for new command
+      serial_count = 0; //clear buffer
+    }
+    else
+    {
+      if(serial_char == ';') comment_mode = true;
+      if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;
+    }
+  }
+  
+  #endif //SDSUPPORT
+
+}
+
+
+float code_value() 
+{ 
+  return (strtod(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL)); 
+}
+
+long code_value_long() 
+{ 
+  return (strtol(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL, 10)); 
+}
+
+bool code_seen(char code_string[]) //Return True if the string was found
+{ 
+  return (strstr(cmdbuffer[bufindr], code_string) != NULL); 
+}  
+
+bool code_seen(char code)
+{
+  strchr_pointer = strchr(cmdbuffer[bufindr], code);
+  return (strchr_pointer != NULL);  //Return True if a character was found
+}
+
+#define DEFINE_PGM_READ_ANY(type, reader)		\
+    static inline type pgm_read_any(const type *p)	\
+	{ return pgm_read_##reader##_near(p); }
+
+DEFINE_PGM_READ_ANY(float,       float);
+DEFINE_PGM_READ_ANY(signed char, byte);
+
+#define XYZ_CONSTS_FROM_CONFIG(type, array, CONFIG)	\
+static const PROGMEM type array##_P[3] =		\
+    { X_##CONFIG, Y_##CONFIG, Z_##CONFIG };		\
+static inline type array(int axis)			\
+    { return pgm_read_any(&array##_P[axis]); }
+
+XYZ_CONSTS_FROM_CONFIG(float, base_min_pos,    MIN_POS);
+XYZ_CONSTS_FROM_CONFIG(float, base_max_pos,    MAX_POS);
+XYZ_CONSTS_FROM_CONFIG(float, base_home_pos,   HOME_POS);
+XYZ_CONSTS_FROM_CONFIG(float, max_length,      MAX_LENGTH);
+XYZ_CONSTS_FROM_CONFIG(float, home_retract_mm, HOME_RETRACT_MM);
+XYZ_CONSTS_FROM_CONFIG(signed char, home_dir,  HOME_DIR);
+
+static void axis_is_at_home(int axis) {
+  current_position[axis] = base_home_pos(axis) + add_homeing[axis];
+  min_pos[axis] =          base_min_pos(axis) + add_homeing[axis];
+  max_pos[axis] =          base_max_pos(axis) + add_homeing[axis];
+}
+
+static void homeaxis(int axis) {
+#define HOMEAXIS_DO(LETTER) \
+  ((LETTER##_MIN_PIN > -1 && LETTER##_HOME_DIR==-1) || (LETTER##_MAX_PIN > -1 && LETTER##_HOME_DIR==1))
+
+  if (axis==X_AXIS ? HOMEAXIS_DO(X) :
+      axis==Y_AXIS ? HOMEAXIS_DO(Y) :
+      axis==Z_AXIS ? HOMEAXIS_DO(Z) :
+      0) {
+    current_position[axis] = 0;
+    plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+    destination[axis] = 1.5 * max_length(axis) * home_dir(axis);
+    feedrate = homing_feedrate[axis];
+    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+    st_synchronize();
+   
+    current_position[axis] = 0;
+    plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+    destination[axis] = -home_retract_mm(axis) * home_dir(axis);
+    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+    st_synchronize();
+   
+    destination[axis] = 2*home_retract_mm(axis) * home_dir(axis);
+    feedrate = homing_feedrate[axis]/2 ; 
+    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+    st_synchronize();
+   
+    axis_is_at_home(axis);					
+    destination[axis] = current_position[axis];
+    feedrate = 0.0;
+    endstops_hit_on_purpose();
+  }
+}
+#define HOMEAXIS(LETTER) homeaxis(LETTER##_AXIS)
+
+void process_commands()
+{
+  unsigned long codenum; //throw away variable
+  char *starpos = NULL;
+
+  if(code_seen('G'))
+  {
+    switch((int)code_value())
+    {
+    case 0: // G0 -> G1
+    case 1: // G1
+      if(Stopped == false) {
+        get_coordinates(); // For X Y Z E F
+        prepare_move();
+        //ClearToSend();
+        return;
+      }
+      //break;
+    case 2: // G2  - CW ARC
+      if(Stopped == false) {
+        get_arc_coordinates();
+        prepare_arc_move(true);
+        return;
+      }
+    case 3: // G3  - CCW ARC
+      if(Stopped == false) {
+        get_arc_coordinates();
+        prepare_arc_move(false);
+        return;
+      }
+    case 4: // G4 dwell
+      LCD_MESSAGEPGM(MSG_DWELL);
+      codenum = 0;
+      if(code_seen('P')) codenum = code_value(); // milliseconds to wait
+      if(code_seen('S')) codenum = code_value() * 1000; // seconds to wait
+      
+      st_synchronize();
+      codenum += millis();  // keep track of when we started waiting
+      previous_millis_cmd = millis();
+      while(millis()  < codenum ){
+        manage_heater();
+        manage_inactivity();
+        LCD_STATUS;
+      }
+      break;
+      #ifdef FWRETRACT  
+      case 10: // G10 retract
+      if(!retracted) 
+      {
+        destination[X_AXIS]=current_position[X_AXIS];
+        destination[Y_AXIS]=current_position[Y_AXIS];
+        destination[Z_AXIS]=current_position[Z_AXIS]; 
+        current_position[Z_AXIS]+=-retract_zlift;
+        destination[E_AXIS]=current_position[E_AXIS]-retract_length; 
+        feedrate=retract_feedrate;
+        retracted=true;
+        prepare_move();
+      }
+      
+      break;
+      case 11: // G10 retract_recover
+      if(!retracted) 
+      {
+        destination[X_AXIS]=current_position[X_AXIS];
+        destination[Y_AXIS]=current_position[Y_AXIS];
+        destination[Z_AXIS]=current_position[Z_AXIS]; 
+        
+        current_position[Z_AXIS]+=retract_zlift;
+        current_position[E_AXIS]+=-retract_recover_length; 
+        feedrate=retract_recover_feedrate;
+        retracted=false;
+        prepare_move();
+      }
+      break;
+      #endif //FWRETRACT
+    case 28: //G28 Home all Axis one at a time
+      saved_feedrate = feedrate;
+      saved_feedmultiply = feedmultiply;
+      feedmultiply = 100;
+      previous_millis_cmd = millis();
+      
+      enable_endstops(true);
+      
+      for(int8_t i=0; i < NUM_AXIS; i++) {
+        destination[i] = current_position[i];
+      }
+      feedrate = 0.0;
+      home_all_axis = !((code_seen(axis_codes[0])) || (code_seen(axis_codes[1])) || (code_seen(axis_codes[2])));
+      
+      #if Z_HOME_DIR > 0                      // If homing away from BED do Z first
+      if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) {
+        HOMEAXIS(Z);
+      }
+      #endif
+      
+      #ifdef QUICK_HOME
+      if((home_all_axis)||( code_seen(axis_codes[X_AXIS]) && code_seen(axis_codes[Y_AXIS])) )  //first diagonal move
+      {
+        current_position[X_AXIS] = 0;current_position[Y_AXIS] = 0;  
+
+        plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); 
+        destination[X_AXIS] = 1.5 * X_MAX_LENGTH * X_HOME_DIR;destination[Y_AXIS] = 1.5 * Y_MAX_LENGTH * Y_HOME_DIR;  
+        feedrate = homing_feedrate[X_AXIS]; 
+        if(homing_feedrate[Y_AXIS]<feedrate)
+          feedrate =homing_feedrate[Y_AXIS]; 
+        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+        st_synchronize();
+    
+        axis_is_at_home(X_AXIS);
+        axis_is_at_home(Y_AXIS);
+        plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+        destination[X_AXIS] = current_position[X_AXIS];
+        destination[Y_AXIS] = current_position[Y_AXIS];
+        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+        feedrate = 0.0;
+        st_synchronize();
+        endstops_hit_on_purpose();
+      }
+      #endif
+      
+      if((home_all_axis) || (code_seen(axis_codes[X_AXIS]))) 
+      {
+        HOMEAXIS(X);
+      }
+
+      if((home_all_axis) || (code_seen(axis_codes[Y_AXIS]))) {
+        HOMEAXIS(Y);
+      }
+      
+      #if Z_HOME_DIR < 0                      // If homing towards BED do Z last
+      if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) {
+        HOMEAXIS(Z);
+      }
+      #endif
+      
+      if(code_seen(axis_codes[X_AXIS])) 
+      {
+        if(code_value_long() != 0) {
+          current_position[X_AXIS]=code_value()+add_homeing[0];
+        }
+      }
+
+      if(code_seen(axis_codes[Y_AXIS])) {
+        if(code_value_long() != 0) {
+          current_position[Y_AXIS]=code_value()+add_homeing[1];
+        }
+      }
+
+      if(code_seen(axis_codes[Z_AXIS])) {
+        if(code_value_long() != 0) {
+          current_position[Z_AXIS]=code_value()+add_homeing[2];
+        }
+      }
+      plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+      
+      #ifdef ENDSTOPS_ONLY_FOR_HOMING
+        enable_endstops(false);
+      #endif
+      
+      feedrate = saved_feedrate;
+      feedmultiply = saved_feedmultiply;
+      previous_millis_cmd = millis();
+      endstops_hit_on_purpose();
+      break;
+    case 90: // G90
+      relative_mode = false;
+      break;
+    case 91: // G91
+      relative_mode = true;
+      break;
+    case 92: // G92
+      if(!code_seen(axis_codes[E_AXIS]))
+        st_synchronize();
+      for(int8_t i=0; i < NUM_AXIS; i++) {
+        if(code_seen(axis_codes[i])) { 
+           if(i == E_AXIS) {
+             current_position[i] = code_value();  
+             plan_set_e_position(current_position[E_AXIS]);
+           }
+           else {
+             current_position[i] = code_value()+add_homeing[i];  
+             plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+           }
+        }
+      }
+      break;
+    }
+  }
+
+  else if(code_seen('M'))
+  {
+    switch( (int)code_value() ) 
+    {
+#ifdef ULTRA_LCD
+    case 0: // M0 - Unconditional stop - Wait for user button press on LCD
+    case 1: // M1 - Conditional stop - Wait for user button press on LCD
+    {
+      LCD_MESSAGEPGM(MSG_USERWAIT);
+      codenum = 0;
+      if(code_seen('P')) codenum = code_value(); // milliseconds to wait
+      if(code_seen('S')) codenum = code_value() * 1000; // seconds to wait
+      
+      st_synchronize();
+      previous_millis_cmd = millis();
+      if (codenum > 0){
+        codenum += millis();  // keep track of when we started waiting
+        while(millis()  < codenum && !CLICKED){
+          manage_heater();
+          manage_inactivity();
+          LCD_STATUS;
+        }
+      }else{
+        while(!CLICKED){
+          manage_heater();
+          manage_inactivity();
+          LCD_STATUS;
+        }
+      }
+    }
+    break;
+#endif
+    case 17:
+        LCD_MESSAGEPGM(MSG_NO_MOVE);
+        enable_x(); 
+        enable_y(); 
+        enable_z(); 
+        enable_e0(); 
+        enable_e1(); 
+        enable_e2(); 
+      break;
+
+#ifdef SDSUPPORT
+    case 20: // M20 - list SD card
+      SERIAL_PROTOCOLLNPGM(MSG_BEGIN_FILE_LIST);
+      card.ls();
+      SERIAL_PROTOCOLLNPGM(MSG_END_FILE_LIST);
+      break;
+    case 21: // M21 - init SD card
+      
+      card.initsd();
+      
+      break;
+    case 22: //M22 - release SD card
+      card.release();
+
+      break;
+    case 23: //M23 - Select file
+      starpos = (strchr(strchr_pointer + 4,'*'));
+      if(starpos!=NULL)
+        *(starpos-1)='\0';
+      card.openFile(strchr_pointer + 4,true);
+      break;
+    case 24: //M24 - Start SD print
+      card.startFileprint();
+      starttime=millis();
+      break;
+    case 25: //M25 - Pause SD print
+      card.pauseSDPrint();
+      break;
+    case 26: //M26 - Set SD index
+      if(card.cardOK && code_seen('S')) {
+        card.setIndex(code_value_long());
+      }
+      break;
+    case 27: //M27 - Get SD status
+      card.getStatus();
+      break;
+    case 28: //M28 - Start SD write
+      starpos = (strchr(strchr_pointer + 4,'*'));
+      if(starpos != NULL){
+        char* npos = strchr(cmdbuffer[bufindr], 'N');
+        strchr_pointer = strchr(npos,' ') + 1;
+        *(starpos-1) = '\0';
+      }
+      card.openFile(strchr_pointer+4,false);
+      break;
+    case 29: //M29 - Stop SD write
+      //processed in write to file routine above
+      //card,saving = false;
+      break;
+    case 30: //M30 <filename> Delete File 
+	if (card.cardOK){
+		card.closefile();
+		starpos = (strchr(strchr_pointer + 4,'*'));
+                if(starpos != NULL){
+                char* npos = strchr(cmdbuffer[bufindr], 'N');
+                strchr_pointer = strchr(npos,' ') + 1;
+                *(starpos-1) = '\0';
+         }
+	 card.removeFile(strchr_pointer + 4);
+	}
+	break;
+	
+#endif //SDSUPPORT
+
+    case 31: //M31 take time since the start of the SD print or an M109 command
+      {
+      stoptime=millis();
+      char time[30];
+      unsigned long t=(stoptime-starttime)/1000;
+      int sec,min;
+      min=t/60;
+      sec=t%60;
+      sprintf(time,"%i min, %i sec",min,sec);
+      SERIAL_ECHO_START;
+      SERIAL_ECHOLN(time);
+      LCD_MESSAGE(time);
+      autotempShutdown();
+      }
+      break;
+    case 42: //M42 -Change pin status via gcode
+      if (code_seen('S'))
+      {
+        int pin_status = code_value();
+        if (code_seen('P') && pin_status >= 0 && pin_status <= 255)
+        {
+          int pin_number = code_value();
+          for(int8_t i = 0; i < (int8_t)sizeof(sensitive_pins); i++)
+          {
+            if (sensitive_pins[i] == pin_number)
+            {
+              pin_number = -1;
+              break;
+            }
+          }
+          
+          if (pin_number > -1)
+          {              
+            pinMode(pin_number, OUTPUT);
+            digitalWrite(pin_number, pin_status);
+            analogWrite(pin_number, pin_status);
+          }
+        }
+      }
+     break;
+    case 104: // M104
+      if(setTargetedHotend(104)){
+        break;
+      }
+      if (code_seen('S')) setTargetHotend(code_value(), tmp_extruder);
+      setWatch();
+      break;
+    case 140: // M140 set bed temp
+      if (code_seen('S')) setTargetBed(code_value());
+      break;
+    case 105 : // M105
+      if(setTargetedHotend(105)){
+        break;
+      }
+      #if (TEMP_0_PIN > -1)
+        SERIAL_PROTOCOLPGM("ok T:");
+        SERIAL_PROTOCOL_F(degHotend(tmp_extruder),1); 
+        SERIAL_PROTOCOLPGM(" /");
+        SERIAL_PROTOCOL_F(degTargetHotend(tmp_extruder),1); 
+        #if TEMP_BED_PIN > -1
+          SERIAL_PROTOCOLPGM(" B:");  
+          SERIAL_PROTOCOL_F(degBed(),1);
+          SERIAL_PROTOCOLPGM(" /");
+          SERIAL_PROTOCOL_F(degTargetBed(),1);
+        #endif //TEMP_BED_PIN
+      #else
+        SERIAL_ERROR_START;
+        SERIAL_ERRORLNPGM(MSG_ERR_NO_THERMISTORS);
+      #endif
+
+        SERIAL_PROTOCOLPGM(" @:");
+        SERIAL_PROTOCOL(getHeaterPower(tmp_extruder));  
+
+        SERIAL_PROTOCOLPGM(" B@:");
+        SERIAL_PROTOCOL(getHeaterPower(-1));  
+
+        SERIAL_PROTOCOLLN("");
+      return;
+      break;
+    case 109: 
+    {// M109 - Wait for extruder heater to reach target.
+      if(setTargetedHotend(109)){
+        break;
+      }
+      LCD_MESSAGEPGM(MSG_HEATING);   
+      #ifdef AUTOTEMP
+        autotemp_enabled=false;
+      #endif
+      if (code_seen('S')) setTargetHotend(code_value(), tmp_extruder);
+      #ifdef AUTOTEMP
+        if (code_seen('S')) autotemp_min=code_value();
+        if (code_seen('B')) autotemp_max=code_value();
+        if (code_seen('F')) 
+        {
+          autotemp_factor=code_value();
+          autotemp_enabled=true;
+        }
+      #endif
+      
+      setWatch();
+      codenum = millis(); 
+
+      /* See if we are heating up or cooling down */
+      bool target_direction = isHeatingHotend(tmp_extruder); // true if heating, false if cooling
+
+      #ifdef TEMP_RESIDENCY_TIME
+        long residencyStart;
+        residencyStart = -1;
+        /* continue to loop until we have reached the target temp   
+          _and_ until TEMP_RESIDENCY_TIME hasn't passed since we reached it */
+        while((residencyStart == -1) ||
+              (residencyStart >= 0 && (((unsigned int) (millis() - residencyStart)) < (TEMP_RESIDENCY_TIME * 1000UL))) ) {
+      #else
+        while ( target_direction ? (isHeatingHotend(tmp_extruder)) : (isCoolingHotend(tmp_extruder)&&(CooldownNoWait==false)) ) {
+      #endif //TEMP_RESIDENCY_TIME
+          if( (millis() - codenum) > 1000UL )
+          { //Print Temp Reading and remaining time every 1 second while heating up/cooling down
+            SERIAL_PROTOCOLPGM("T:");
+            SERIAL_PROTOCOL_F(degHotend(tmp_extruder),1); 
+            SERIAL_PROTOCOLPGM(" E:");
+            SERIAL_PROTOCOL((int)tmp_extruder); 
+            #ifdef TEMP_RESIDENCY_TIME
+              SERIAL_PROTOCOLPGM(" W:");
+              if(residencyStart > -1)
+              {
+                 codenum = ((TEMP_RESIDENCY_TIME * 1000UL) - (millis() - residencyStart)) / 1000UL;
+                 SERIAL_PROTOCOLLN( codenum );
+              }
+              else 
+              {
+                 SERIAL_PROTOCOLLN( "?" );
+              }
+            #else
+              SERIAL_PROTOCOLLN("");
+            #endif
+            codenum = millis();
+          }
+          manage_heater();
+          manage_inactivity();
+          LCD_STATUS;
+        #ifdef TEMP_RESIDENCY_TIME
+            /* start/restart the TEMP_RESIDENCY_TIME timer whenever we reach target temp for the first time
+              or when current temp falls outside the hysteresis after target temp was reached */
+          if ((residencyStart == -1 &&  target_direction && (degHotend(tmp_extruder) >= (degTargetHotend(tmp_extruder)-TEMP_WINDOW))) ||
+              (residencyStart == -1 && !target_direction && (degHotend(tmp_extruder) <= (degTargetHotend(tmp_extruder)+TEMP_WINDOW))) ||
+              (residencyStart > -1 && labs(degHotend(tmp_extruder) - degTargetHotend(tmp_extruder)) > TEMP_HYSTERESIS) ) 
+          {
+            residencyStart = millis();
+          }
+        #endif //TEMP_RESIDENCY_TIME
+        }
+        LCD_MESSAGEPGM(MSG_HEATING_COMPLETE);
+        starttime=millis();
+        previous_millis_cmd = millis();
+      }
+      break;
+    case 190: // M190 - Wait for bed heater to reach target.
+    #if TEMP_BED_PIN > -1
+        LCD_MESSAGEPGM(MSG_BED_HEATING);
+        if (code_seen('S')) setTargetBed(code_value());
+        codenum = millis(); 
+        while(isHeatingBed()) 
+        {
+          if(( millis() - codenum) > 1000 ) //Print Temp Reading every 1 second while heating up.
+          {
+            float tt=degHotend(active_extruder);
+            SERIAL_PROTOCOLPGM("T:");
+            SERIAL_PROTOCOL(tt);
+            SERIAL_PROTOCOLPGM(" E:");
+            SERIAL_PROTOCOL((int)active_extruder); 
+            SERIAL_PROTOCOLPGM(" B:");
+            SERIAL_PROTOCOL_F(degBed(),1); 
+            SERIAL_PROTOCOLLN(""); 
+            codenum = millis(); 
+          }
+          manage_heater();
+          manage_inactivity();
+          LCD_STATUS;
+        }
+        LCD_MESSAGEPGM(MSG_BED_DONE);
+        previous_millis_cmd = millis();
+    #endif
+        break;
+
+    #if FAN_PIN > -1
+      case 106: //M106 Fan On
+        if (code_seen('S')){
+           FanSpeed=constrain(code_value(),0,255);
+        }
+        else {
+          FanSpeed=255;			
+        }
+        break;
+      case 107: //M107 Fan Off
+        FanSpeed = 0;
+        break;
+    #endif //FAN_PIN
+
+    #if (PS_ON_PIN > -1)
+      case 80: // M80 - ATX Power On
+        SET_OUTPUT(PS_ON_PIN); //GND
+        WRITE(PS_ON_PIN, LOW);
+        break;
+      #endif
+      
+      case 81: // M81 - ATX Power Off
+      
+      #if defined SUICIDE_PIN && SUICIDE_PIN > -1
+        st_synchronize();
+        suicide();
+      #elif (PS_ON_PIN > -1)
+        SET_OUTPUT(PS_ON_PIN); 
+        WRITE(PS_ON_PIN, HIGH);
+      #endif
+		break;
+        
+    case 82:
+      axis_relative_modes[3] = false;
+      break;
+    case 83:
+      axis_relative_modes[3] = true;
+      break;
+    case 18: //compatibility
+    case 84: // M84
+      if(code_seen('S')){ 
+        stepper_inactive_time = code_value() * 1000; 
+      }
+      else
+      { 
+        bool all_axis = !((code_seen(axis_codes[0])) || (code_seen(axis_codes[1])) || (code_seen(axis_codes[2]))|| (code_seen(axis_codes[3])));
+        if(all_axis)
+        {
+          st_synchronize();
+          disable_e0();
+          disable_e1();
+          disable_e2();
+          finishAndDisableSteppers();
+        }
+        else
+        {
+          st_synchronize();
+          if(code_seen('X')) disable_x();
+          if(code_seen('Y')) disable_y();
+          if(code_seen('Z')) disable_z();
+          #if ((E0_ENABLE_PIN != X_ENABLE_PIN) && (E1_ENABLE_PIN != Y_ENABLE_PIN)) // Only enable on boards that have seperate ENABLE_PINS
+            if(code_seen('E')) {
+              disable_e0();
+              disable_e1();
+              disable_e2();
+            }
+          #endif 
+          LCD_MESSAGEPGM(MSG_PART_RELEASE);
+        }
+      }
+      break;
+    case 85: // M85
+      code_seen('S');
+      max_inactive_time = code_value() * 1000; 
+      break;
+    case 92: // M92
+      for(int8_t i=0; i < NUM_AXIS; i++) 
+      {
+        if(code_seen(axis_codes[i])) 
+        {
+          if(i == 3) { // E
+            float value = code_value();
+            if(value < 20.0) {
+              float factor = axis_steps_per_unit[i] / value; // increase e constants if M92 E14 is given for netfab.
+              max_e_jerk *= factor;
+              max_feedrate[i] *= factor;
+              axis_steps_per_sqr_second[i] *= factor;
+            }
+            axis_steps_per_unit[i] = value;
+          }
+          else {
+            axis_steps_per_unit[i] = code_value();
+          }
+        }
+      }
+      break;
+    case 115: // M115
+      SERIAL_PROTOCOLPGM(MSG_M115_REPORT);
+      break;
+    case 117: // M117 display message
+      LCD_MESSAGE(cmdbuffer[bufindr]+5);
+      break;
+    case 114: // M114
+      SERIAL_PROTOCOLPGM("X:");
+      SERIAL_PROTOCOL(current_position[X_AXIS]);
+      SERIAL_PROTOCOLPGM("Y:");
+      SERIAL_PROTOCOL(current_position[Y_AXIS]);
+      SERIAL_PROTOCOLPGM("Z:");
+      SERIAL_PROTOCOL(current_position[Z_AXIS]);
+      SERIAL_PROTOCOLPGM("E:");      
+      SERIAL_PROTOCOL(current_position[E_AXIS]);
+      
+      SERIAL_PROTOCOLPGM(MSG_COUNT_X);
+      SERIAL_PROTOCOL(float(st_get_position(X_AXIS))/axis_steps_per_unit[X_AXIS]);
+      SERIAL_PROTOCOLPGM("Y:");
+      SERIAL_PROTOCOL(float(st_get_position(Y_AXIS))/axis_steps_per_unit[Y_AXIS]);
+      SERIAL_PROTOCOLPGM("Z:");
+      SERIAL_PROTOCOL(float(st_get_position(Z_AXIS))/axis_steps_per_unit[Z_AXIS]);
+      
+      SERIAL_PROTOCOLLN("");
+      break;
+    case 120: // M120
+      enable_endstops(false) ;
+      break;
+    case 121: // M121
+      enable_endstops(true) ;
+      break;
+    case 119: // M119
+    SERIAL_PROTOCOLLN(MSG_M119_REPORT);
+      #if (X_MIN_PIN > -1)
+        SERIAL_PROTOCOLPGM(MSG_X_MIN);
+        SERIAL_PROTOCOLLN(((READ(X_MIN_PIN)^X_ENDSTOPS_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
+      #endif
+      #if (X_MAX_PIN > -1)
+        SERIAL_PROTOCOLPGM(MSG_X_MAX);
+        SERIAL_PROTOCOLLN(((READ(X_MAX_PIN)^X_ENDSTOPS_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
+      #endif
+      #if (Y_MIN_PIN > -1)
+        SERIAL_PROTOCOLPGM(MSG_Y_MIN);
+        SERIAL_PROTOCOLLN(((READ(Y_MIN_PIN)^Y_ENDSTOPS_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
+      #endif
+      #if (Y_MAX_PIN > -1)
+        SERIAL_PROTOCOLPGM(MSG_Y_MAX);
+        SERIAL_PROTOCOLLN(((READ(Y_MAX_PIN)^Y_ENDSTOPS_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
+      #endif
+      #if (Z_MIN_PIN > -1)
+        SERIAL_PROTOCOLPGM(MSG_Z_MIN);
+        SERIAL_PROTOCOLLN(((READ(Z_MIN_PIN)^Z_ENDSTOPS_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
+      #endif
+      #if (Z_MAX_PIN > -1)
+        SERIAL_PROTOCOLPGM(MSG_Z_MAX);
+        SERIAL_PROTOCOLLN(((READ(Z_MAX_PIN)^Z_ENDSTOPS_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
+      #endif
+      break;
+      //TODO: update for all axis, use for loop
+    case 201: // M201
+      for(int8_t i=0; i < NUM_AXIS; i++) 
+      {
+        if(code_seen(axis_codes[i]))
+        {
+          max_acceleration_units_per_sq_second[i] = code_value();
+          axis_steps_per_sqr_second[i] = code_value() * axis_steps_per_unit[i];
+        }
+      }
+      break;
+    #if 0 // Not used for Sprinter/grbl gen6
+    case 202: // M202
+      for(int8_t i=0; i < NUM_AXIS; i++) {
+        if(code_seen(axis_codes[i])) axis_travel_steps_per_sqr_second[i] = code_value() * axis_steps_per_unit[i];
+      }
+      break;
+    #endif
+    case 203: // M203 max feedrate mm/sec
+      for(int8_t i=0; i < NUM_AXIS; i++) {
+        if(code_seen(axis_codes[i])) max_feedrate[i] = code_value();
+      }
+      break;
+    case 204: // M204 acclereration S normal moves T filmanent only moves
+      {
+        if(code_seen('S')) acceleration = code_value() ;
+        if(code_seen('T')) retract_acceleration = code_value() ;
+      }
+      break;
+    case 205: //M205 advanced settings:  minimum travel speed S=while printing T=travel only,  B=minimum segment time X= maximum xy jerk, Z=maximum Z jerk
+    {
+      if(code_seen('S')) minimumfeedrate = code_value();
+      if(code_seen('T')) mintravelfeedrate = code_value();
+      if(code_seen('B')) minsegmenttime = code_value() ;
+      if(code_seen('X')) max_xy_jerk = code_value() ;
+      if(code_seen('Z')) max_z_jerk = code_value() ;
+      if(code_seen('E')) max_e_jerk = code_value() ;
+    }
+    break;
+    case 206: // M206 additional homeing offset
+      for(int8_t i=0; i < 3; i++) 
+      {
+        if(code_seen(axis_codes[i])) add_homeing[i] = code_value();
+      }
+      break;
+    #ifdef FWRETRACT
+    case 207: //M207 - set retract length S[positive mm] F[feedrate mm/sec] Z[additional zlift/hop]
+    {
+      if(code_seen('S')) 
+      {
+        retract_length = code_value() ;
+      }
+      if(code_seen('F')) 
+      {
+        retract_feedrate = code_value() ;
+      }
+      if(code_seen('Z')) 
+      {
+        retract_zlift = code_value() ;
+      }
+    }break;
+    case 208: // M208 - set retract recover length S[positive mm surplus to the M207 S*] F[feedrate mm/sec]
+    {
+      if(code_seen('S')) 
+      {
+        retract_recover_length = code_value() ;
+      }
+      if(code_seen('F')) 
+      {
+        retract_recover_feedrate = code_value() ;
+      }
+    }break;
+    
+    case 209: // M209 - S<1=true/0=false> enable automatic retract detect if the slicer did not support G10/11: every normal extrude-only move will be classified as retract depending on the direction.
+    {
+      if(code_seen('S')) 
+      {
+        int t= code_value() ;
+        switch(t)
+        {
+          case 0: autoretract_enabled=false;retracted=false;break;
+          case 1: autoretract_enabled=true;retracted=false;break;
+          default: 
+            SERIAL_ECHO_START;
+            SERIAL_ECHOPGM(MSG_UNKNOWN_COMMAND);
+            SERIAL_ECHO(cmdbuffer[bufindr]);
+            SERIAL_ECHOLNPGM("\"");
+        }
+      }
+      
+    }break;
+    #endif
+    case 220: // M220 S<factor in percent>- set speed factor override percentage
+    {
+      if(code_seen('S')) 
+      {
+        feedmultiply = code_value() ;
+        feedmultiplychanged=true;
+      }
+    }
+    break;
+    case 221: // M221 S<factor in percent>- set extrude factor override percentage
+    {
+      if(code_seen('S')) 
+      {
+        extrudemultiply = code_value() ;
+      }
+    }
+    break;
+
+    #ifdef PIDTEMP
+    case 301: // M301
+      {
+        if(code_seen('P')) Kp = code_value();
+        if(code_seen('I')) Ki = code_value()*PID_dT;
+        if(code_seen('D')) Kd = code_value()/PID_dT;
+        #ifdef PID_ADD_EXTRUSION_RATE
+        if(code_seen('C')) Kc = code_value();
+        #endif
+        updatePID();
+        SERIAL_PROTOCOL(MSG_OK);
+		SERIAL_PROTOCOL(" p:");
+        SERIAL_PROTOCOL(Kp);
+        SERIAL_PROTOCOL(" i:");
+        SERIAL_PROTOCOL(Ki/PID_dT);
+        SERIAL_PROTOCOL(" d:");
+        SERIAL_PROTOCOL(Kd*PID_dT);
+        #ifdef PID_ADD_EXTRUSION_RATE
+        SERIAL_PROTOCOL(" c:");
+        SERIAL_PROTOCOL(Kc*PID_dT);
+        #endif
+        SERIAL_PROTOCOLLN("");
+      }
+      break;
+    #endif //PIDTEMP
+    #ifdef PIDTEMPBED
+    case 304: // M304
+      {
+        if(code_seen('P')) bedKp = code_value();
+        if(code_seen('I')) bedKi = code_value()*PID_dT;
+        if(code_seen('D')) bedKd = code_value()/PID_dT;
+        updatePID();
+        SERIAL_PROTOCOL(MSG_OK);
+		SERIAL_PROTOCOL(" p:");
+        SERIAL_PROTOCOL(bedKp);
+        SERIAL_PROTOCOL(" i:");
+        SERIAL_PROTOCOL(bedKi/PID_dT);
+        SERIAL_PROTOCOL(" d:");
+        SERIAL_PROTOCOL(bedKd*PID_dT);
+        SERIAL_PROTOCOLLN("");
+      }
+      break;
+    #endif //PIDTEMP
+    case 240: // M240  Triggers a camera by emulating a Canon RC-1 : http://www.doc-diy.net/photo/rc-1_hacked/
+     {
+      #ifdef PHOTOGRAPH_PIN
+        #if (PHOTOGRAPH_PIN > -1)
+        const uint8_t NUM_PULSES=16;
+        const float PULSE_LENGTH=0.01524;
+        for(int i=0; i < NUM_PULSES; i++) {
+          WRITE(PHOTOGRAPH_PIN, HIGH);
+          _delay_ms(PULSE_LENGTH);
+          WRITE(PHOTOGRAPH_PIN, LOW);
+          _delay_ms(PULSE_LENGTH);
+        }
+        delay(7.33);
+        for(int i=0; i < NUM_PULSES; i++) {
+          WRITE(PHOTOGRAPH_PIN, HIGH);
+          _delay_ms(PULSE_LENGTH);
+          WRITE(PHOTOGRAPH_PIN, LOW);
+          _delay_ms(PULSE_LENGTH);
+        }
+        #endif
+      #endif
+     }
+    break;
+      
+    case 302: // allow cold extrudes
+    {
+      allow_cold_extrudes(true);
+    }
+    break;
+    case 303: // M303 PID autotune
+    {
+      float temp = 150.0;
+      int e=0;
+      int c=5;
+      if (code_seen('E')) e=code_value();
+			if (e<0)
+				temp=70;
+      if (code_seen('S')) temp=code_value();
+      if (code_seen('C')) c=code_value();
+      PID_autotune(temp, e, c);
+    }
+    break;
+    case 400: // M400 finish all moves
+    {
+      st_synchronize();
+    }
+    break;
+    case 500: // Store settings in EEPROM
+    {
+        EEPROM_StoreSettings();
+    }
+    break;
+    case 501: // Read settings from EEPROM
+    {
+      EEPROM_RetrieveSettings();
+    }
+    break;
+    case 502: // Revert to default settings
+    {
+      EEPROM_RetrieveSettings(true);
+    }
+    break;
+    case 503: // print settings currently in memory
+    {
+      EEPROM_printSettings();
+    }
+    break;
+    case 999: // Restart after being stopped
+      Stopped = false;
+      gcode_LastN = Stopped_gcode_LastN;
+      FlushSerialRequestResend();
+    break;
+    }
+  }
+
+  else if(code_seen('T')) 
+  {
+    tmp_extruder = code_value();
+    if(tmp_extruder >= EXTRUDERS) {
+      SERIAL_ECHO_START;
+      SERIAL_ECHO("T");
+      SERIAL_ECHO(tmp_extruder);
+      SERIAL_ECHOLN(MSG_INVALID_EXTRUDER);
+    }
+    else {
+      active_extruder = tmp_extruder;
+      SERIAL_ECHO_START;
+      SERIAL_ECHO(MSG_ACTIVE_EXTRUDER);
+      SERIAL_PROTOCOLLN((int)active_extruder);
+    }
+  }
+
+  else
+  {
+    SERIAL_ECHO_START;
+    SERIAL_ECHOPGM(MSG_UNKNOWN_COMMAND);
+    SERIAL_ECHO(cmdbuffer[bufindr]);
+    SERIAL_ECHOLNPGM("\"");
+  }
+
+  ClearToSend();
+}
+
+void FlushSerialRequestResend()
+{
+  //char cmdbuffer[bufindr][100]="Resend:";
+  MYSERIAL.flush();
+  SERIAL_PROTOCOLPGM(MSG_RESEND);
+  SERIAL_PROTOCOLLN(gcode_LastN + 1);
+  ClearToSend();
+}
+
+void ClearToSend()
+{
+  previous_millis_cmd = millis();
+  #ifdef SDSUPPORT
+  if(fromsd[bufindr])
+    return;
+  #endif //SDSUPPORT
+  SERIAL_PROTOCOLLNPGM(MSG_OK); 
+}
+
+void get_coordinates()
+{
+  bool seen[4]={false,false,false,false};
+  for(int8_t i=0; i < NUM_AXIS; i++) {
+    if(code_seen(axis_codes[i])) 
+    {
+      destination[i] = (float)code_value() + (axis_relative_modes[i] || relative_mode)*current_position[i];
+      seen[i]=true;
+    }
+    else destination[i] = current_position[i]; //Are these else lines really needed?
+  }
+  if(code_seen('F')) {
+    next_feedrate = code_value();
+    if(next_feedrate > 0.0) feedrate = next_feedrate;
+  }
+  #ifdef FWRETRACT
+  if(autoretract_enabled)
+  if( !(seen[X_AXIS] || seen[Y_AXIS] || seen[Z_AXIS]) && seen[E_AXIS])
+  {
+    float echange=destination[E_AXIS]-current_position[E_AXIS];
+    if(echange<-MIN_RETRACT) //retract
+    {
+      if(!retracted) 
+      {
+      
+      destination[Z_AXIS]+=retract_zlift; //not sure why chaninging current_position negatively does not work.
+      //if slicer retracted by echange=-1mm and you want to retract 3mm, corrrectede=-2mm additionally
+      float correctede=-echange-retract_length;
+      //to generate the additional steps, not the destination is changed, but inversely the current position
+      current_position[E_AXIS]+=-correctede; 
+      feedrate=retract_feedrate;
+      retracted=true;
+      }
+      
+    }
+    else 
+      if(echange>MIN_RETRACT) //retract_recover
+    {
+      if(retracted) 
+      {
+      //current_position[Z_AXIS]+=-retract_zlift;
+      //if slicer retracted_recovered by echange=+1mm and you want to retract_recover 3mm, corrrectede=2mm additionally
+      float correctede=-echange+1*retract_length+retract_recover_length; //total unretract=retract_length+retract_recover_length[surplus]
+      current_position[E_AXIS]+=correctede; //to generate the additional steps, not the destination is changed, but inversely the current position
+      feedrate=retract_recover_feedrate;
+      retracted=false;
+      }
+    }
+    
+  }
+  #endif //FWRETRACT
+}
+
+void get_arc_coordinates()
+{
+#ifdef SF_ARC_FIX
+   bool relative_mode_backup = relative_mode;
+   relative_mode = true;
+#endif
+   get_coordinates();
+#ifdef SF_ARC_FIX
+   relative_mode=relative_mode_backup;
+#endif
+
+   if(code_seen('I')) {
+     offset[0] = code_value();
+   } 
+   else {
+     offset[0] = 0.0;
+   }
+   if(code_seen('J')) {
+     offset[1] = code_value();
+   }
+   else {
+     offset[1] = 0.0;
+   }
+}
+
+void clamp_to_software_endstops(float target[3])
+{
+  if (min_software_endstops) {
+    if (target[X_AXIS] < min_pos[X_AXIS]) target[X_AXIS] = min_pos[X_AXIS];
+    if (target[Y_AXIS] < min_pos[Y_AXIS]) target[Y_AXIS] = min_pos[Y_AXIS];
+    if (target[Z_AXIS] < min_pos[Z_AXIS]) target[Z_AXIS] = min_pos[Z_AXIS];
+  }
+
+  if (max_software_endstops) {
+    if (target[X_AXIS] > max_pos[X_AXIS]) target[X_AXIS] = max_pos[X_AXIS];
+    if (target[Y_AXIS] > max_pos[Y_AXIS]) target[Y_AXIS] = max_pos[Y_AXIS];
+    if (target[Z_AXIS] > max_pos[Z_AXIS]) target[Z_AXIS] = max_pos[Z_AXIS];
+  }
+}
+
+void prepare_move()
+{
+  clamp_to_software_endstops(destination);
+
+  previous_millis_cmd = millis(); 
+  // Do not use feedmultiply for E or Z only moves
+  if( (current_position[X_AXIS] == destination [X_AXIS]) && (current_position[Y_AXIS] == destination [Y_AXIS])) {
+      plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+  }
+  else {
+    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder);
+  }
+  for(int8_t i=0; i < NUM_AXIS; i++) {
+    current_position[i] = destination[i];
+  }
+}
+
+void prepare_arc_move(char isclockwise) {
+  float r = hypot(offset[X_AXIS], offset[Y_AXIS]); // Compute arc radius for mc_arc
+
+  // Trace the arc
+  mc_arc(current_position, destination, offset, X_AXIS, Y_AXIS, Z_AXIS, feedrate*feedmultiply/60/100.0, r, isclockwise, active_extruder);
+  
+  // As far as the parser is concerned, the position is now == target. In reality the
+  // motion control system might still be processing the action and the real tool position
+  // in any intermediate location.
+  for(int8_t i=0; i < NUM_AXIS; i++) {
+    current_position[i] = destination[i];
+  }
+  previous_millis_cmd = millis();
+}
+
+#ifdef CONTROLLERFAN_PIN
+unsigned long lastMotor = 0; //Save the time for when a motor was turned on last
+unsigned long lastMotorCheck = 0;
+
+void controllerFan()
+{
+  if ((millis() - lastMotorCheck) >= 2500) //Not a time critical function, so we only check every 2500ms
+  {
+    lastMotorCheck = millis();
+    
+    if(!READ(X_ENABLE_PIN) || !READ(Y_ENABLE_PIN) || !READ(Z_ENABLE_PIN)
+    #if EXTRUDERS > 2
+       || !READ(E2_ENABLE_PIN)
+    #endif
+    #if EXTRUDER > 1
+       || !READ(E2_ENABLE_PIN)
+    #endif
+       || !READ(E0_ENABLE_PIN)) //If any of the drivers are enabled...    
+    {
+      lastMotor = millis(); //... set time to NOW so the fan will turn on
+    }
+    
+    if ((millis() - lastMotor) >= (CONTROLLERFAN_SEC*1000UL) || lastMotor == 0) //If the last time any driver was enabled, is longer since than CONTROLLERSEC...   
+    {
+      WRITE(CONTROLLERFAN_PIN, LOW); //... turn the fan off
+    }
+    else
+    {
+      WRITE(CONTROLLERFAN_PIN, HIGH); //... turn the fan on
+    }
+  }
+}
+#endif
+
+void manage_inactivity() 
+{ 
+  if( (millis() - previous_millis_cmd) >  max_inactive_time ) 
+    if(max_inactive_time) 
+      kill(); 
+  if(stepper_inactive_time)  {
+    if( (millis() - previous_millis_cmd) >  stepper_inactive_time ) 
+    {
+      if(blocks_queued() == false) {
+        disable_x();
+        disable_y();
+        disable_z();
+        disable_e0();
+        disable_e1();
+        disable_e2();
+      }
+    }
+  }
+  #if( KILL_PIN>-1 )
+    if( 0 == READ(KILL_PIN) )
+      kill();
+  #endif
+  #ifdef CONTROLLERFAN_PIN
+    controllerFan(); //Check if fan should be turned on to cool stepper drivers down
+  #endif
+  #ifdef EXTRUDER_RUNOUT_PREVENT
+    if( (millis() - previous_millis_cmd) >  EXTRUDER_RUNOUT_SECONDS*1000 ) 
+    if(degHotend(active_extruder)>EXTRUDER_RUNOUT_MINTEMP)
+    {
+     bool oldstatus=READ(E0_ENABLE_PIN);
+     enable_e0();
+     float oldepos=current_position[E_AXIS];
+     float oldedes=destination[E_AXIS];
+     plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], 
+                      current_position[E_AXIS]+EXTRUDER_RUNOUT_EXTRUDE*EXTRUDER_RUNOUT_ESTEPS/axis_steps_per_unit[E_AXIS], 
+                      EXTRUDER_RUNOUT_SPEED/60.*EXTRUDER_RUNOUT_ESTEPS/axis_steps_per_unit[E_AXIS], active_extruder);
+     current_position[E_AXIS]=oldepos;
+     destination[E_AXIS]=oldedes;
+     plan_set_e_position(oldepos);
+     previous_millis_cmd=millis();
+     st_synchronize();
+     WRITE(E0_ENABLE_PIN,oldstatus);
+    }
+  #endif
+  check_axes_activity();
+}
+
+void kill()
+{
+  cli(); // Stop interrupts
+  disable_heater();
+
+  disable_x();
+  disable_y();
+  disable_z();
+  disable_e0();
+  disable_e1();
+  disable_e2();
+  
+  if(PS_ON_PIN > -1) pinMode(PS_ON_PIN,INPUT);
+  SERIAL_ERROR_START;
+  SERIAL_ERRORLNPGM(MSG_ERR_KILLED);
+  LCD_ALERTMESSAGEPGM(MSG_KILLED);
+  suicide();
+  while(1); // Wait for reset
+}
+
+void Stop()
+{
+  disable_heater();
+  if(Stopped == false) {
+    Stopped = true;
+    Stopped_gcode_LastN = gcode_LastN; // Save last g_code for restart
+    SERIAL_ERROR_START;
+    SERIAL_ERRORLNPGM(MSG_ERR_STOPPED);
+    LCD_MESSAGEPGM(MSG_STOPPED);
+  }
+}
+
+bool IsStopped() { return Stopped; };
+
+#ifdef FAST_PWM_FAN
+void setPwmFrequency(uint8_t pin, int val)
+{
+  val &= 0x07;
+  switch(digitalPinToTimer(pin))
+  {
+ 
+    #if defined(TCCR0A)
+    case TIMER0A:
+    case TIMER0B:
+//         TCCR0B &= ~(CS00 | CS01 | CS02);
+//         TCCR0B |= val;
+         break;
+    #endif
+
+    #if defined(TCCR1A)
+    case TIMER1A:
+    case TIMER1B:
+//         TCCR1B &= ~(CS10 | CS11 | CS12);
+//         TCCR1B |= val;
+         break;
+    #endif
+
+    #if defined(TCCR2)
+    case TIMER2:
+    case TIMER2:
+         TCCR2 &= ~(CS10 | CS11 | CS12);
+         TCCR2 |= val;
+         break;
+    #endif
+
+    #if defined(TCCR2A)
+    case TIMER2A:
+    case TIMER2B:
+         TCCR2B &= ~(CS20 | CS21 | CS22);
+         TCCR2B |= val;
+         break;
+    #endif
+
+    #if defined(TCCR3A)
+    case TIMER3A:
+    case TIMER3B:
+    case TIMER3C:
+         TCCR3B &= ~(CS30 | CS31 | CS32);
+         TCCR3B |= val;
+         break;
+    #endif
+
+    #if defined(TCCR4A) 
+    case TIMER4A:
+    case TIMER4B:
+    case TIMER4C:
+         TCCR4B &= ~(CS40 | CS41 | CS42);
+         TCCR4B |= val;
+         break;
+   #endif
+
+    #if defined(TCCR5A) 
+    case TIMER5A:
+    case TIMER5B:
+    case TIMER5C:
+         TCCR5B &= ~(CS50 | CS51 | CS52);
+         TCCR5B |= val;
+         break;
+   #endif
+
+  }
+}
+#endif //FAST_PWM_FAN
+
+bool setTargetedHotend(int code){
+  tmp_extruder = active_extruder;
+  if(code_seen('T')) {
+    tmp_extruder = code_value();
+    if(tmp_extruder >= EXTRUDERS) {
+      SERIAL_ECHO_START;
+      switch(code){
+        case 104:
+          SERIAL_ECHO(MSG_M104_INVALID_EXTRUDER);
+          break;
+        case 105:
+          SERIAL_ECHO(MSG_M105_INVALID_EXTRUDER);
+          break;
+        case 109:
+          SERIAL_ECHO(MSG_M109_INVALID_EXTRUDER);
+          break;
+      }
+      SERIAL_ECHOLN(tmp_extruder);
+      return true;
+    }
+  }
+  return false;
+}
diff --git a/Marlin/SdFile.cpp b/Marlin/SdFile.cpp
index e9439e3b50..29f5efadc1 100644
--- a/Marlin/SdFile.cpp
+++ b/Marlin/SdFile.cpp
@@ -55,13 +55,16 @@ int16_t SdFile::write(const void* buf, uint16_t nbyte) {
  * Use writeError to check for errors.
  */
 #if ARDUINO >= 100
-    size_t SdFile::write(uint8_t b)
-#else
-  void SdFile::write(uint8_t b)
-#endif
+size_t SdFile::write(uint8_t b)
 {
-  SdBaseFile::write(&b, 1);
+    return SdBaseFile::write(&b, 1);
 }
+#else
+void SdFile::write(uint8_t b)
+{
+    SdBaseFile::write(&b, 1);
+}
+#endif
 //------------------------------------------------------------------------------
 /** Write a string to a file. Used by the Arduino Print class.
  * \param[in] str Pointer to the string.
@@ -89,4 +92,4 @@ void SdFile::writeln_P(PGM_P str) {
 }
 
 
-#endif
\ No newline at end of file
+#endif
diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index 6c47e298d5..caee9915be 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -191,8 +191,7 @@ void calculate_trapezoid_for_block(block_t *block, float entry_factor, float exi
   // have to use intersection_distance() to calculate when to abort acceleration and start braking
   // in order to reach the final_rate exactly at the end of this block.
   if (plateau_steps < 0) {
-    accelerate_steps = ceil(
-    intersection_distance(block->initial_rate, block->final_rate, acceleration, block->step_event_count));
+    accelerate_steps = ceil(intersection_distance(block->initial_rate, block->final_rate, acceleration, block->step_event_count));
     accelerate_steps = max(accelerate_steps,0); // Check limits due to numerical round-off
     accelerate_steps = min(accelerate_steps,block->step_event_count);
     plateau_steps = 0;
diff --git a/Marlin/temperature.cpp b/Marlin/temperature.cpp
index 66293c7f59..9bc0953609 100644
--- a/Marlin/temperature.cpp
+++ b/Marlin/temperature.cpp
@@ -145,20 +145,20 @@ static volatile bool temp_meas_ready = false;
 
 void PID_autotune(float temp, int extruder, int ncycles)
 {
-  float input;
+  float input = 0.0;
   int cycles=0;
   bool heating = true;
 
   unsigned long temp_millis = millis();
   unsigned long t1=temp_millis;
   unsigned long t2=temp_millis;
-  long t_high;
-  long t_low;
+  long t_high = 0;
+  long t_low = 0;
 
   long bias, d;
   float Ku, Tu;
   float Kp, Ki, Kd;
-  float max, min;
+  float max = 0, min = 10000;
 
 	if ((extruder > EXTRUDERS)
   #if (TEMP_BED_PIN <= -1)
@@ -315,17 +315,15 @@ int getHeaterPower(int heater) {
 
 void manage_heater()
 {
-
-  #ifdef USE_WATCHDOG
-    wd_reset();
-  #endif
-  
   float pid_input;
   float pid_output;
 
   if(temp_meas_ready != true)   //better readability
     return; 
 
+  //Reset the watchdog after we know we have a temperature measurement.
+  watchdog_reset();
+
   CRITICAL_SECTION_START;
   temp_meas_ready = false;
   CRITICAL_SECTION_END;
@@ -548,7 +546,6 @@ int temp2analogBed(int celsius) {
 #elif defined BED_USES_AD595
     return lround(((celsius-TEMP_SENSOR_AD595_OFFSET)/TEMP_SENSOR_AD595_GAIN) * (1024.0 * OVERSAMPLENR/ (5.0 * 100.0) ) );
 #else
-    #warning No heater-type defined for the bed.
     return 0;
 #endif
 }
@@ -625,7 +622,6 @@ float analog2tempBed(int raw) {
   #elif defined BED_USES_AD595
     return ((raw * ((5.0 * 100.0) / 1024.0) / OVERSAMPLENR) * TEMP_SENSOR_AD595_GAIN) + TEMP_SENSOR_AD595_OFFSET;
   #else
-    #warning No heater-type defined for the bed.
     return 0;
   #endif
 }
diff --git a/Marlin/ultralcd.pde b/Marlin/ultralcd.cpp
similarity index 88%
rename from Marlin/ultralcd.pde
rename to Marlin/ultralcd.cpp
index 8a6b8463ff..6245e728dd 100644
--- a/Marlin/ultralcd.pde
+++ b/Marlin/ultralcd.cpp
@@ -6,11 +6,7 @@
 #include "language.h"
 #include "temperature.h"
 #include "EEPROMwrite.h"
-#if LANGUAGE_CHOICE == 6
-#include "LiquidCrystalRus.h"
-#else
-#include <LiquidCrystal.h>
-#endif
+
 //===========================================================================
 //=============================imported variables============================
 //===========================================================================
@@ -33,6 +29,14 @@ volatile char buttons=0;  //the last checked buttons in a bit array.
 long encoderpos=0;
 short lastenc=0;
 
+//TODO: This should be in a preferences file.
+int plaPreheatHotendTemp;
+int plaPreheatHPBTemp;
+int plaPreheatFanSpeed;
+
+int absPreheatHotendTemp;
+int absPreheatHPBTemp;
+int absPreheatFanSpeed;
 
 //===========================================================================
 //=============================private  variables============================
@@ -42,11 +46,7 @@ static char messagetext[LCD_WIDTH]="";
 //return for string conversion routines
 static char conv[8];
 
-#if LANGUAGE_CHOICE == 6
-LiquidCrystalRus lcd(LCD_PINS_RS, LCD_PINS_ENABLE, LCD_PINS_D4, LCD_PINS_D5,LCD_PINS_D6,LCD_PINS_D7);  //RS,Enable,D4,D5,D6,D7
-#else
-LiquidCrystal lcd(LCD_PINS_RS, LCD_PINS_ENABLE, LCD_PINS_D4, LCD_PINS_D5,LCD_PINS_D6,LCD_PINS_D7);  //RS,Enable,D4,D5,D6,D7 
-#endif
+LCD_CLASS lcd(LCD_PINS_RS, LCD_PINS_ENABLE, LCD_PINS_D4, LCD_PINS_D5,LCD_PINS_D6,LCD_PINS_D7);  //RS,Enable,D4,D5,D6,D7
 
 static unsigned long previous_millis_lcd=0;
 //static long previous_millis_buttons=0;
@@ -70,7 +70,7 @@ void lcdProgMemprint(const char *str)
     ch=pgm_read_byte(++str);
   }
 }
-#define lcdprintPGM(x) lcdProgMemprint(MYPGM(x))
+#define LCD_PRINT_PGM(x) lcdProgMemprint(PSTR(x))
 
 
 //===========================================================================
@@ -366,11 +366,11 @@ void MainMenu::showStatus()
   {
     encoderpos=feedmultiply;
     clear();
-    lcd.setCursor(0,0);lcdprintPGM("\002000/000\001 ");
+    lcd.setCursor(0,0);LCD_PRINT_PGM("\002000/000\001 ");
     #if defined BED_USES_THERMISTOR || defined BED_USES_AD595 
-      lcd.setCursor(10,0);lcdprintPGM("B000/000\001 ");
+      lcd.setCursor(10,0);LCD_PRINT_PGM("B000/000\001 ");
     #elif EXTRUDERS > 1
-      lcd.setCursor(10,0);lcdprintPGM("\002000/000\001 ");
+      lcd.setCursor(10,0);LCD_PRINT_PGM("\002000/000\001 ");
     #endif
   }
     
@@ -432,7 +432,7 @@ void MainMenu::showStatus()
     
     if(starttime!=oldtime)
     {
-      lcd.print(itostr2(time/60));lcdprintPGM("h ");lcd.print(itostr2(time%60));lcdprintPGM("m");
+      lcd.print(itostr2(time/60));LCD_PRINT_PGM("h ");lcd.print(itostr2(time%60));LCD_PRINT_PGM("m");
       oldtime=time;
     }
   }
@@ -441,7 +441,7 @@ void MainMenu::showStatus()
   if((currentz!=oldzpos)||force_lcd_update)
   {
     lcd.setCursor(10,1);
-    lcdprintPGM("Z:");lcd.print(ftostr52(current_position[2]));
+    LCD_PRINT_PGM("Z:");lcd.print(ftostr52(current_position[2]));
     oldzpos=currentz;
   }
   
@@ -468,7 +468,7 @@ void MainMenu::showStatus()
   {
    oldfeedmultiply=curfeedmultiply;
    lcd.setCursor(0,2);
-   lcd.print(itostr3(curfeedmultiply));lcdprintPGM("% ");
+   lcd.print(itostr3(curfeedmultiply));LCD_PRINT_PGM("% ");
   }
   
   if(messagetext[0]!='\0')
@@ -487,7 +487,7 @@ void MainMenu::showStatus()
   {
      lcd.setCursor(10,2);
     lcd.print(itostr3((int)percent));
-    lcdprintPGM("%SD");
+    LCD_PRINT_PGM("%SD");
   }
 #endif
 #else //smaller LCDS----------------------------------
@@ -496,7 +496,7 @@ void MainMenu::showStatus()
   if(force_lcd_update)  //initial display of content
   {
     encoderpos=feedmultiply;
-    lcd.setCursor(0,0);lcdprintPGM("\002---/---\001 ");
+    lcd.setCursor(0,0);LCD_PRINT_PGM("\002---/---\001 ");
   }
     
   int tHotEnd0=intround(degHotend0());
@@ -550,26 +550,26 @@ void MainMenu::showPrepare()
   switch(i)
   {
     case ItemP_exit:
-      MENUITEM(  lcdprintPGM(MSG_MAIN)  ,  BLOCK;status=Main_Menu;beepshort(); ) ;
+      MENUITEM(  LCD_PRINT_PGM(MSG_MAIN)  ,  BLOCK;status=Main_Menu;beepshort(); ) ;
       break;
     case ItemP_autostart:
-      MENUITEM(  lcdprintPGM(MSG_AUTOSTART)  ,  BLOCK;
+      MENUITEM(  LCD_PRINT_PGM(MSG_AUTOSTART)  ,  BLOCK;
 #ifdef SDSUPPORT
           card.lastnr=0;card.setroot();card.checkautostart(true);
 #endif
           beepshort(); ) ;
       break;
     case ItemP_disstep:
-      MENUITEM(  lcdprintPGM(MSG_DISABLE_STEPPERS)  ,  BLOCK;enquecommand("M84");beepshort(); ) ;
+      MENUITEM(  LCD_PRINT_PGM(MSG_DISABLE_STEPPERS)  ,  BLOCK;enquecommand("M84");beepshort(); ) ;
       break;
     case ItemP_home:
-      MENUITEM(  lcdprintPGM(MSG_AUTO_HOME)  ,  BLOCK;enquecommand("G28");beepshort(); ) ;
+      MENUITEM(  LCD_PRINT_PGM(MSG_AUTO_HOME)  ,  BLOCK;enquecommand("G28");beepshort(); ) ;
       break;
     case ItemP_origin:
-      MENUITEM(  lcdprintPGM(MSG_SET_ORIGIN)  ,  BLOCK;enquecommand("G92 X0 Y0 Z0");beepshort(); ) ;
+      MENUITEM(  LCD_PRINT_PGM(MSG_SET_ORIGIN)  ,  BLOCK;enquecommand("G92 X0 Y0 Z0");beepshort(); ) ;
       break;
     case ItemP_preheat_pla:
-		MENUITEM(  lcdprintPGM(MSG_PREHEAT_PLA)  ,  BLOCK;setTargetHotend0(plaPreheatHotendTemp);setTargetBed(plaPreheatHPBTemp);
+		MENUITEM(  LCD_PRINT_PGM(MSG_PREHEAT_PLA)  ,  BLOCK;setTargetHotend0(plaPreheatHotendTemp);setTargetBed(plaPreheatHPBTemp);
       #if FAN_PIN > -1
 		FanSpeed = plaPreheatFanSpeed;
         analogWrite(FAN_PIN,  FanSpeed);
@@ -577,7 +577,7 @@ void MainMenu::showPrepare()
       beepshort(); );
       break;
     case ItemP_preheat_abs:
-      MENUITEM(  lcdprintPGM(MSG_PREHEAT_ABS)  ,  BLOCK;setTargetHotend0(absPreheatHotendTemp);setTargetBed(absPreheatHPBTemp); 
+      MENUITEM(  LCD_PRINT_PGM(MSG_PREHEAT_ABS)  ,  BLOCK;setTargetHotend0(absPreheatHotendTemp);setTargetBed(absPreheatHPBTemp); 
       #if FAN_PIN > -1
 	  	FanSpeed = absPreheatFanSpeed;
         analogWrite(FAN_PIN,  FanSpeed);
@@ -585,13 +585,13 @@ void MainMenu::showPrepare()
       beepshort(); );
       break;
     case ItemP_cooldown:
-      MENUITEM(  lcdprintPGM(MSG_COOLDOWN)  ,  BLOCK;setTargetHotend0(0);setTargetHotend1(0);setTargetHotend2(0);setTargetBed(0);beepshort(); ) ;
+      MENUITEM(  LCD_PRINT_PGM(MSG_COOLDOWN)  ,  BLOCK;setTargetHotend0(0);setTargetHotend1(0);setTargetHotend2(0);setTargetBed(0);beepshort(); ) ;
       break;
 //    case ItemP_extrude:
-  //    MENUITEM(  lcdprintPGM(" Extrude")  ,  BLOCK;enquecommand("G92 E0");enquecommand("G1 F700 E50");beepshort(); ) ;
+  //    MENUITEM(  LCD_PRINT_PGM(" Extrude")  ,  BLOCK;enquecommand("G92 E0");enquecommand("G1 F700 E50");beepshort(); ) ;
     //  break;
     case ItemP_move:
-      MENUITEM(  lcdprintPGM(MSG_MOVE_AXIS) , BLOCK;status=Sub_PrepareMove;beepshort(); );
+      MENUITEM(  LCD_PRINT_PGM(MSG_MOVE_AXIS) , BLOCK;status=Sub_PrepareMove;beepshort(); );
       break;
         default:   
       break;
@@ -617,14 +617,14 @@ void MainMenu::showAxisMove()
      switch(i)
       {
           case ItemAM_exit:
-          MENUITEM(  lcdprintPGM(MSG_PREPARE_ALT)  ,  BLOCK;status=Main_Prepare;beepshort(); ) ;
+          MENUITEM(  LCD_PRINT_PGM(MSG_PREPARE_ALT)  ,  BLOCK;status=Main_Prepare;beepshort(); ) ;
           break;
           case ItemAM_X:
           {
 	 	  //oldencoderpos=0;
                   if(force_lcd_update)
                   {
-                    lcd.setCursor(0,line);lcdprintPGM(" X:");
+                    lcd.setCursor(0,line);LCD_PRINT_PGM(" X:");
                     lcd.setCursor(11,line);lcd.print(ftostr52(current_position[X_AXIS]));
                   }
       
@@ -669,7 +669,7 @@ void MainMenu::showAxisMove()
             {
                   if(force_lcd_update)
                   {
-                    lcd.setCursor(0,line);lcdprintPGM(" Y:");
+                    lcd.setCursor(0,line);LCD_PRINT_PGM(" Y:");
                     lcd.setCursor(11,line);lcd.print(ftostr52(current_position[Y_AXIS]));
                   }
       
@@ -714,7 +714,7 @@ void MainMenu::showAxisMove()
           {
                   if(force_lcd_update)
                   {
-                    lcd.setCursor(0,line);lcdprintPGM(" Z:");
+                    lcd.setCursor(0,line);LCD_PRINT_PGM(" Z:");
                     lcd.setCursor(11,line);lcd.print(ftostr52(current_position[Z_AXIS]));
                   }
       
@@ -757,11 +757,11 @@ void MainMenu::showAxisMove()
           break;
           case ItemAM_E:
           // ErikDB: TODO: this length should be changed for volumetric.
-          MENUITEM(  lcdprintPGM(MSG_EXTRUDE)  ,  BLOCK;enquecommand("G92 E0");enquecommand("G1 F70 E1");beepshort(); ) ;
+          MENUITEM(  LCD_PRINT_PGM(MSG_EXTRUDE)  ,  BLOCK;enquecommand("G92 E0");enquecommand("G1 F70 E1");beepshort(); ) ;
           break;
           case ItemAM_ERetract:
               // ErikDB: TODO: this length should be changed for volumetric.
-              MENUITEM(  lcdprintPGM(MSG_RETRACT)  ,  BLOCK;enquecommand("G92 E0");enquecommand("G1 F700 E-1");beepshort(); ) ;
+              MENUITEM(  LCD_PRINT_PGM(MSG_RETRACT)  ,  BLOCK;enquecommand("G92 E0");enquecommand("G1 F700 E-1");beepshort(); ) ;
               break;
           default:
           break;
@@ -787,13 +787,13 @@ void MainMenu::showTune()
   switch(i)
   {
   case ItemT_exit:
-      MENUITEM(  lcdprintPGM(MSG_MAIN)  ,  BLOCK;status=Main_Menu;beepshort(); ) ;
+      MENUITEM(  LCD_PRINT_PGM(MSG_MAIN)  ,  BLOCK;status=Main_Menu;beepshort(); ) ;
       break;
   case ItemT_speed:
     {
       if(force_lcd_update)
       {
-        lcd.setCursor(0,line);lcdprintPGM(MSG_SPEED);
+        lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_SPEED);
         lcd.setCursor(13,line);lcd.print(ftostr3(feedmultiply));
       }
       
@@ -828,7 +828,7 @@ void MainMenu::showTune()
       {
         if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_NOZZLE);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_NOZZLE);
           lcd.setCursor(13,line);lcd.print(ftostr3(intround(degTargetHotend0())));
         } 
         
@@ -862,7 +862,7 @@ void MainMenu::showTune()
       {
         if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_BED);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_BED);
           lcd.setCursor(13,line);lcd.print(ftostr3(intround(degTargetBed())));
         }
         
@@ -898,7 +898,7 @@ void MainMenu::showTune()
       {
         if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_FAN_SPEED);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_FAN_SPEED);
           lcd.setCursor(13,line);lcd.print(ftostr3(FanSpeed));
         }
         
@@ -933,7 +933,7 @@ void MainMenu::showTune()
          {
       if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_FLOW);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_FLOW);
           lcd.setCursor(13,line);lcd.print(ftostr52(axis_steps_per_unit[E_AXIS]));
         }
         
@@ -975,22 +975,22 @@ void MainMenu::showTune()
  updateActiveLines(ItemT_fan,encoderpos);
 }
 
-//does not work
-// #define MENUCHANGEITEM(repaint_action,  enter_action, accept_action,  change_action) \
-//   {\
-//     if(force_lcd_update)  { lcd.setCursor(0,line);  repaint_action; } \
-//     if(activeline==line)  \
-//     { \
-//       if(CLICKED) \
-//       { \
-//         linechanging=!linechanging; \
-//         if(linechanging)  {enter_action;} \
-//         else {accept_action;} \
-//       }  \
-//       else \
-//       if(linechanging) {change_action};}\
-//   }
-//   
+/*does not work
+#define MENUCHANGEITEM(repaint_action,  enter_action, accept_action,  change_action) \
+   {\
+     if(force_lcd_update)  { lcd.setCursor(0,line);  repaint_action; } \
+     if(activeline==line)  \
+     { \
+       if(CLICKED) \
+       { \
+         linechanging=!linechanging; \
+         if(linechanging)  {enter_action;} \
+         else {accept_action;} \
+       }  \
+       else \
+       if(linechanging) {change_action};}\
+   }
+*/   
 
 enum {
   ItemCT_exit,ItemCT_nozzle0,
@@ -1022,13 +1022,13 @@ void MainMenu::showControlTemp()
   switch(i)
   {
     case ItemCT_exit:
-      MENUITEM(  lcdprintPGM(MSG_CONTROL)  ,  BLOCK;status=Main_Control;beepshort(); ) ;
+      MENUITEM(  LCD_PRINT_PGM(MSG_CONTROL)  ,  BLOCK;status=Main_Control;beepshort(); ) ;
       break;
     case ItemCT_nozzle0:
       {
         if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_NOZZLE);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_NOZZLE);
           lcd.setCursor(13,line);lcd.print(ftostr3(intround(degTargetHotend0())));
         }
         
@@ -1063,7 +1063,7 @@ void MainMenu::showControlTemp()
       {
         if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_NOZZLE1);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_NOZZLE1);
           lcd.setCursor(13,line);lcd.print(ftostr3(intround(degTargetHotend1())));
         }
         
@@ -1099,7 +1099,7 @@ void MainMenu::showControlTemp()
       {
         if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_NOZZLE2);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_NOZZLE2);
           lcd.setCursor(13,line);lcd.print(ftostr3(intround(degTargetHotend2())));
         }
         
@@ -1135,7 +1135,7 @@ void MainMenu::showControlTemp()
       {
         if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_MIN);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_MIN);
           lcd.setCursor(13,line);lcd.print(ftostr3(autotemp_min));
         }
         
@@ -1169,7 +1169,7 @@ void MainMenu::showControlTemp()
       {
         if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_MAX);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_MAX);
           lcd.setCursor(13,line);lcd.print(ftostr3(autotemp_max));
         }
         
@@ -1203,7 +1203,7 @@ void MainMenu::showControlTemp()
       {
         if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_FACTOR);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_FACTOR);
           lcd.setCursor(13,line);lcd.print(ftostr32(autotemp_factor));
         }
         
@@ -1237,12 +1237,12 @@ void MainMenu::showControlTemp()
       {
         if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_AUTOTEMP);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_AUTOTEMP);
           lcd.setCursor(13,line);
           if(autotemp_enabled)
-            lcdprintPGM(MSG_ON);
+            LCD_PRINT_PGM(MSG_ON);
           else
-            lcdprintPGM(MSG_OFF);
+            LCD_PRINT_PGM(MSG_OFF);
         }
         
         if((activeline!=line) )
@@ -1253,9 +1253,9 @@ void MainMenu::showControlTemp()
           autotemp_enabled=!autotemp_enabled;
           lcd.setCursor(13,line);
           if(autotemp_enabled)
-            lcdprintPGM(MSG_ON);
+            LCD_PRINT_PGM(MSG_ON);
           else
-            lcdprintPGM(MSG_OFF);
+            LCD_PRINT_PGM(MSG_OFF);
           BLOCK;
         }
         
@@ -1266,7 +1266,7 @@ void MainMenu::showControlTemp()
       {
         if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_BED);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_BED);
           lcd.setCursor(13,line);lcd.print(ftostr3(intround(degTargetBed())));
         }
         
@@ -1300,7 +1300,7 @@ void MainMenu::showControlTemp()
       {
         if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_FAN_SPEED);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_FAN_SPEED);
           lcd.setCursor(13,line);lcd.print(ftostr3(FanSpeed));
         }
         
@@ -1336,7 +1336,7 @@ void MainMenu::showControlTemp()
       {
       if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(" PID-P: ");
+          lcd.setCursor(0,line);LCD_PRINT_PGM(" PID-P: ");
           lcd.setCursor(13,line);lcd.print(itostr4(Kp));
         }
         
@@ -1371,7 +1371,7 @@ void MainMenu::showControlTemp()
       {
       if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_PID_I);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_PID_I);
           lcd.setCursor(13,line);lcd.print(ftostr51(Ki/PID_dT));
         }
         
@@ -1406,7 +1406,7 @@ void MainMenu::showControlTemp()
       {
       if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_PID_D);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_PID_D);
           lcd.setCursor(13,line);lcd.print(itostr4(Kd*PID_dT));
         }
         
@@ -1443,7 +1443,7 @@ void MainMenu::showControlTemp()
       {
       if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_PID_C);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_PID_C);
           lcd.setCursor(13,line);lcd.print(itostr3(Kc));
         }
         
@@ -1478,10 +1478,10 @@ void MainMenu::showControlTemp()
     #endif
       break;
 	  case ItemCT_PLA_PreHeat_Setting:
-        MENUITEM(  lcdprintPGM(MSG_PREHEAT_PLA_SETTINGS)  ,  BLOCK;status=Sub_PreheatPLASettings;beepshort(); ) ;
+        MENUITEM(  LCD_PRINT_PGM(MSG_PREHEAT_PLA_SETTINGS)  ,  BLOCK;status=Sub_PreheatPLASettings;beepshort(); ) ;
 	  break;
 	  case ItemCT_ABS_PreHeat_Setting:
-        MENUITEM(  lcdprintPGM(MSG_PREHEAT_ABS_SETTINGS)  ,  BLOCK;status=Sub_PreheatABSSettings;beepshort(); ) ;
+        MENUITEM(  LCD_PRINT_PGM(MSG_PREHEAT_ABS_SETTINGS)  ,  BLOCK;status=Sub_PreheatABSSettings;beepshort(); ) ;
 	  break;
     default:   
       break;
@@ -1513,14 +1513,14 @@ void MainMenu::showControlMotion()
   switch(i)
   {
     case ItemCM_exit:
-      MENUITEM(  lcdprintPGM(MSG_CONTROL)  ,  BLOCK;status=Main_Control;beepshort(); ) ;
+      MENUITEM(  LCD_PRINT_PGM(MSG_CONTROL)  ,  BLOCK;status=Main_Control;beepshort(); ) ;
       break;
     case ItemCM_acc:
     {
       if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_ACC);
-          lcd.setCursor(13,line);lcd.print(itostr3(acceleration/100));lcdprintPGM("00");
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_ACC);
+          lcd.setCursor(13,line);lcd.print(itostr3(acceleration/100));LCD_PRINT_PGM("00");
         }
         
         if((activeline!=line) )
@@ -1545,7 +1545,7 @@ void MainMenu::showControlMotion()
         {
           if(encoderpos<5) encoderpos=5;
           if(encoderpos>990) encoderpos=990;
-          lcd.setCursor(13,line);lcd.print(itostr3(encoderpos));lcdprintPGM("00");
+          lcd.setCursor(13,line);lcd.print(itostr3(encoderpos));LCD_PRINT_PGM("00");
         }
         
       }break;
@@ -1553,7 +1553,7 @@ void MainMenu::showControlMotion()
       {
       if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_VXY_JERK);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_VXY_JERK);
           lcd.setCursor(13,line);lcd.print(itostr3(max_xy_jerk));
         }
         
@@ -1592,11 +1592,11 @@ void MainMenu::showControlMotion()
       {
       if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_VMAX);
-          if(i==ItemCM_vmaxx)lcdprintPGM(MSG_X);
-          if(i==ItemCM_vmaxy)lcdprintPGM(MSG_Y);
-          if(i==ItemCM_vmaxz)lcdprintPGM(MSG_Z);
-          if(i==ItemCM_vmaxe)lcdprintPGM(MSG_E);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_VMAX);
+          if(i==ItemCM_vmaxx)LCD_PRINT_PGM(MSG_X);
+          if(i==ItemCM_vmaxy)LCD_PRINT_PGM(MSG_Y);
+          if(i==ItemCM_vmaxz)LCD_PRINT_PGM(MSG_Z);
+          if(i==ItemCM_vmaxe)LCD_PRINT_PGM(MSG_E);
           lcd.setCursor(13,line);lcd.print(itostr3(max_feedrate[i-ItemCM_vmaxx]));
         }
         
@@ -1632,7 +1632,7 @@ void MainMenu::showControlMotion()
     {
       if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_VMIN);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_VMIN);
           lcd.setCursor(13,line);lcd.print(itostr3(minimumfeedrate));
         }
         
@@ -1667,7 +1667,7 @@ void MainMenu::showControlMotion()
     {
       if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_VTRAV_MIN);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_VTRAV_MIN);
           lcd.setCursor(13,line);lcd.print(itostr3(mintravelfeedrate));
         }
         
@@ -1706,12 +1706,12 @@ void MainMenu::showControlMotion()
     {
       if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(" Amax ");
-          if(i==ItemCM_amaxx)lcdprintPGM(MSG_X);
-          if(i==ItemCM_amaxy)lcdprintPGM(MSG_Y);
-          if(i==ItemCM_amaxz)lcdprintPGM(MSG_Z);
-          if(i==ItemCM_amaxe)lcdprintPGM(MSG_E);
-          lcd.setCursor(13,line);lcd.print(itostr3(max_acceleration_units_per_sq_second[i-ItemCM_amaxx]/100));lcdprintPGM("00");
+          lcd.setCursor(0,line);LCD_PRINT_PGM(" Amax ");
+          if(i==ItemCM_amaxx)LCD_PRINT_PGM(MSG_X);
+          if(i==ItemCM_amaxy)LCD_PRINT_PGM(MSG_Y);
+          if(i==ItemCM_amaxz)LCD_PRINT_PGM(MSG_Z);
+          if(i==ItemCM_amaxe)LCD_PRINT_PGM(MSG_E);
+          lcd.setCursor(13,line);lcd.print(itostr3(max_acceleration_units_per_sq_second[i-ItemCM_amaxx]/100));LCD_PRINT_PGM("00");
         }
         
         if((activeline!=line) )
@@ -1736,7 +1736,7 @@ void MainMenu::showControlMotion()
         {
           if(encoderpos<1) encoderpos=1;
           if(encoderpos>990) encoderpos=990;
-          lcd.setCursor(13,line);lcd.print(itostr3(encoderpos));lcdprintPGM("00");
+          lcd.setCursor(13,line);lcd.print(itostr3(encoderpos));LCD_PRINT_PGM("00");
         }
         
       }break;
@@ -1746,8 +1746,8 @@ void MainMenu::showControlMotion()
     {
         if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_A_RETRACT);
-          lcd.setCursor(13,line);lcd.print(ftostr3(retract_acceleration/100));lcdprintPGM("00");
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_A_RETRACT);
+          lcd.setCursor(13,line);lcd.print(ftostr3(retract_acceleration/100));LCD_PRINT_PGM("00");
         }
         
         if((activeline!=line) )
@@ -1773,7 +1773,7 @@ void MainMenu::showControlMotion()
         {
           if(encoderpos<10) encoderpos=10;
           if(encoderpos>990) encoderpos=990;
-          lcd.setCursor(13,line);lcd.print(itostr3(encoderpos));lcdprintPGM("00");
+          lcd.setCursor(13,line);lcd.print(itostr3(encoderpos));LCD_PRINT_PGM("00");
         }
         
       }break;
@@ -1781,7 +1781,7 @@ void MainMenu::showControlMotion()
          {
       if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_XSTEPS);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_XSTEPS);
           lcd.setCursor(11,line);lcd.print(ftostr52(axis_steps_per_unit[X_AXIS]));
         }
         
@@ -1818,7 +1818,7 @@ void MainMenu::showControlMotion()
          {
       if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_YSTEPS);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_YSTEPS);
           lcd.setCursor(11,line);lcd.print(ftostr52(axis_steps_per_unit[Y_AXIS]));
         }
         
@@ -1856,7 +1856,7 @@ void MainMenu::showControlMotion()
          {
       if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_ZSTEPS);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_ZSTEPS);
           lcd.setCursor(11,line);lcd.print(ftostr51(axis_steps_per_unit[Z_AXIS]));
         }
         
@@ -1895,7 +1895,7 @@ void MainMenu::showControlMotion()
          {
       if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_ESTEPS);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_ESTEPS);
           lcd.setCursor(11,line);lcd.print(ftostr51(axis_steps_per_unit[E_AXIS]));
         }
         
@@ -1958,7 +1958,7 @@ void MainMenu::showControlRetract()
   switch(i)
   {
     case ItemR_exit:
-      MENUITEM(  lcdprintPGM(MSG_CONTROL)  ,  BLOCK;status=Main_Control;beepshort(); ) ;
+      MENUITEM(  LCD_PRINT_PGM(MSG_CONTROL)  ,  BLOCK;status=Main_Control;beepshort(); ) ;
       break;
     
       //float retract_length=2, retract_feedrate=1200, retract_zlift=0.4;
@@ -1967,12 +1967,12 @@ void MainMenu::showControlRetract()
       {
         if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_AUTORETRACT);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_AUTORETRACT);
           lcd.setCursor(13,line);
           if(autoretract_enabled)
-            lcdprintPGM(MSG_ON);
+            LCD_PRINT_PGM(MSG_ON);
           else
-            lcdprintPGM(MSG_OFF);
+            LCD_PRINT_PGM(MSG_OFF);
         }
         
         if((activeline!=line) )
@@ -1983,9 +1983,9 @@ void MainMenu::showControlRetract()
           autoretract_enabled=!autoretract_enabled;
           lcd.setCursor(13,line);
           if(autoretract_enabled)
-            lcdprintPGM(MSG_ON);
+            LCD_PRINT_PGM(MSG_ON);
           else
-            lcdprintPGM(MSG_OFF);
+            LCD_PRINT_PGM(MSG_OFF);
           BLOCK;
         }
         
@@ -1995,7 +1995,7 @@ void MainMenu::showControlRetract()
     {
         if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_CONTROL_RETRACT);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_CONTROL_RETRACT);
           lcd.setCursor(13,line);lcd.print(ftostr52(retract_length));
         }
         
@@ -2030,7 +2030,7 @@ void MainMenu::showControlRetract()
     {
         if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_CONTROL_RETRACTF);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_CONTROL_RETRACTF);
           lcd.setCursor(13,line);lcd.print(itostr4(retract_feedrate));
         }
         
@@ -2065,7 +2065,7 @@ void MainMenu::showControlRetract()
     {
         if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_CONTROL_RETRACT_ZLIFT);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_CONTROL_RETRACT_ZLIFT);
           lcd.setCursor(13,line);lcd.print(ftostr52(retract_zlift));;
         }
         
@@ -2100,7 +2100,7 @@ void MainMenu::showControlRetract()
     {
         if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_CONTROL_RETRACT_RECOVER);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_CONTROL_RETRACT_RECOVER);
           lcd.setCursor(13,line);lcd.print(ftostr52(retract_recover_length));;
         }
         
@@ -2136,7 +2136,7 @@ void MainMenu::showControlRetract()
     {
         if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_CONTROL_RETRACT_RECOVERF);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_CONTROL_RETRACT_RECOVERF);
           lcd.setCursor(13,line);lcd.print(itostr4(retract_recover_feedrate));
         }
         
@@ -2196,24 +2196,24 @@ void MainMenu::showControl()
   switch(i)
   {
     case ItemC_exit:
-      MENUITEM(  lcdprintPGM(MSG_MAIN_WIDE)  ,  BLOCK;status=Main_Menu;beepshort(); ) ;
+      MENUITEM(  LCD_PRINT_PGM(MSG_MAIN_WIDE)  ,  BLOCK;status=Main_Menu;beepshort(); ) ;
       break;
     case ItemC_temp:
-      MENUITEM(  lcdprintPGM(MSG_TEMPERATURE_WIDE)  ,  BLOCK;status=Sub_TempControl;beepshort(); ) ;
+      MENUITEM(  LCD_PRINT_PGM(MSG_TEMPERATURE_WIDE)  ,  BLOCK;status=Sub_TempControl;beepshort(); ) ;
       break;
    case ItemC_move:
-      MENUITEM(  lcdprintPGM(MSG_MOTION_WIDE)  ,  BLOCK;status=Sub_MotionControl;beepshort(); ) ;
+      MENUITEM(  LCD_PRINT_PGM(MSG_MOTION_WIDE)  ,  BLOCK;status=Sub_MotionControl;beepshort(); ) ;
       break;
 #ifdef FWRETRACT
     case ItemC_rectract:
-      MENUITEM(  lcdprintPGM(MSG_RECTRACT_WIDE)  ,  BLOCK;status=Sub_RetractControl;beepshort(); ) ;
+      MENUITEM(  LCD_PRINT_PGM(MSG_RECTRACT_WIDE)  ,  BLOCK;status=Sub_RetractControl;beepshort(); ) ;
       break;
 #endif
     case ItemC_store:
     {
       if(force_lcd_update)
       {
-        lcd.setCursor(0,line);lcdprintPGM(MSG_STORE_EPROM);
+        lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_STORE_EPROM);
       }
       if((activeline==line) && CLICKED)
       {
@@ -2227,7 +2227,7 @@ void MainMenu::showControl()
     {
       if(force_lcd_update)
       {
-        lcd.setCursor(0,line);lcdprintPGM(MSG_LOAD_EPROM);
+        lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_LOAD_EPROM);
       }
       if((activeline==line) && CLICKED)
       {
@@ -2241,7 +2241,7 @@ void MainMenu::showControl()
     {
       if(force_lcd_update)
       {
-        lcd.setCursor(0,line);lcdprintPGM(MSG_RESTORE_FAILSAFE);
+        lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_RESTORE_FAILSAFE);
       }
       if((activeline==line) && CLICKED)
       {
@@ -2288,7 +2288,7 @@ void MainMenu::showSD()
   switch(i)
   {
     case 0:
-      MENUITEM(  lcdprintPGM(MSG_MAIN)  ,  BLOCK;status=Main_Menu;beepshort(); ) ;
+      MENUITEM(  LCD_PRINT_PGM(MSG_MAIN)  ,  BLOCK;status=Main_Menu;beepshort(); ) ;
       break;
 //     case 1:
 //       {
@@ -2301,11 +2301,11 @@ void MainMenu::showSD()
 //           if(true)
 //           #endif
 //           {
-//             lcdprintPGM(" \004Refresh");
+//             LCD_PRINT_PGM(" \004Refresh");
 //           }
 //           else
 //           {
-//             lcdprintPGM(" \004Insert Card");
+//             LCD_PRINT_PGM(" \004Insert Card");
 //           }
 //           
 //         }
@@ -2320,7 +2320,7 @@ void MainMenu::showSD()
 //       }break;
     case 1:
       MENUITEM(  lcd.print(" ");card.getWorkDirName();
-	  if(card.filename[0]=='/') lcdprintPGM(MSG_REFRESH);
+	  if(card.filename[0]=='/') LCD_PRINT_PGM(MSG_REFRESH);
 	  else {
 		  lcd.print("\005");
 		  lcd.print(card.filename);
@@ -2343,8 +2343,12 @@ void MainMenu::showSD()
         {
           card.getfilename(i-FIRSTITEM);
           //Serial.print("Filenr:");Serial.println(i-2);
-          lcd.setCursor(0,line);lcdprintPGM(" ");
-          if(card.filenameIsDir) lcd.print("\005");
+          lcd.setCursor(0,line);LCD_PRINT_PGM(" ");
+          if(card.filenameIsDir)
+          {
+            lcd.print("\005");
+            card.longFilename[LCD_WIDTH-2] = '\0';
+          }
           if (card.longFilename[0])
           {
             card.longFilename[LCD_WIDTH-1] = '\0';
@@ -2436,14 +2440,14 @@ void MainMenu::showMainMenu()
     switch(i)
     { 
       case ItemM_watch:
-        MENUITEM(  lcdprintPGM(MSG_WATCH)  ,  BLOCK;status=Main_Status;beepshort(); ) ;
+        MENUITEM(  LCD_PRINT_PGM(MSG_WATCH)  ,  BLOCK;status=Main_Status;beepshort(); ) ;
        break;
       case ItemM_prepare:
-        MENUITEM(  if(!tune) lcdprintPGM(MSG_PREPARE);else  lcdprintPGM(MSG_TUNE); ,  BLOCK;status=Main_Prepare;beepshort(); ) ;
+        MENUITEM(  if(!tune) LCD_PRINT_PGM(MSG_PREPARE);else  LCD_PRINT_PGM(MSG_TUNE); ,  BLOCK;status=Main_Prepare;beepshort(); ) ;
       break;
        
       case ItemM_control:
-        MENUITEM(  lcdprintPGM(MSG_CONTROL_ARROW)  ,  BLOCK;status=Main_Control;beepshort(); ) ;
+        MENUITEM(  LCD_PRINT_PGM(MSG_CONTROL_ARROW)  ,  BLOCK;status=Main_Control;beepshort(); ) ;
       break;
       #ifdef SDSUPPORT
       case ItemM_file:    
@@ -2458,13 +2462,13 @@ void MainMenu::showMainMenu()
           #endif
           {
             if(card.sdprinting)
-              lcdprintPGM(MSG_STOP_PRINT);
+              LCD_PRINT_PGM(MSG_STOP_PRINT);
             else
-              lcdprintPGM(MSG_CARD_MENU);
+              LCD_PRINT_PGM(MSG_CARD_MENU);
           }
           else
           {
-           lcdprintPGM(MSG_NO_CARD); 
+           LCD_PRINT_PGM(MSG_NO_CARD); 
           }
         }
         #ifdef CARDINSERTED
@@ -2490,13 +2494,13 @@ void MainMenu::showMainMenu()
 #endif
                     {
                         if(card.sdprinting)
-                            lcdprintPGM(MSG_PAUSE_PRINT);
+                            LCD_PRINT_PGM(MSG_PAUSE_PRINT);
                         else
-                            lcdprintPGM(MSG_RESUME_PRINT);
+                            LCD_PRINT_PGM(MSG_RESUME_PRINT);
                     }
                     else
                     {
-                        //lcdprintPGM(MSG_NO_CARD);
+                        //LCD_PRINT_PGM(MSG_NO_CARD);
                     }
             }
 #ifdef CARDINSERTED
@@ -2666,14 +2670,14 @@ void MainMenu::showPLAsettings()
   {
 
 	case ItemPLAPreHeat_Exit:
-      MENUITEM(  lcdprintPGM(MSG_TEMPERATURE_RTN)  ,  BLOCK;status=Sub_TempControl;beepshort(); ) ;
+      MENUITEM(  LCD_PRINT_PGM(MSG_TEMPERATURE_RTN)  ,  BLOCK;status=Sub_TempControl;beepshort(); ) ;
       break;
 
     case ItemPLAPreHeat_set_PLA_FanSpeed:
        {
         if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_FAN_SPEED);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_FAN_SPEED);
           lcd.setCursor(13,line);lcd.print(ftostr3(plaPreheatFanSpeed));
         }
         
@@ -2707,7 +2711,7 @@ void MainMenu::showPLAsettings()
       {
         if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_NOZZLE);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_NOZZLE);
           lcd.setCursor(13,line);lcd.print(ftostr3(plaPreheatHotendTemp));
         } 
         
@@ -2741,7 +2745,7 @@ void MainMenu::showPLAsettings()
       {
         if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_BED);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_BED);
           lcd.setCursor(13,line);lcd.print(ftostr3(plaPreheatHPBTemp));
         } 
         
@@ -2774,7 +2778,7 @@ void MainMenu::showPLAsettings()
     {
       if(force_lcd_update)
       {
-        lcd.setCursor(0,line);lcdprintPGM(MSG_STORE_EPROM);
+        lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_STORE_EPROM);
       }
       if((activeline==line) && CLICKED)
       {
@@ -2812,14 +2816,14 @@ void MainMenu::showABSsettings()
   {
 
 	case ItemABSPreHeat_Exit:
-      MENUITEM(  lcdprintPGM(MSG_TEMPERATURE_RTN)  ,  BLOCK;status=Sub_TempControl;beepshort(); ) ;
+      MENUITEM(  LCD_PRINT_PGM(MSG_TEMPERATURE_RTN)  ,  BLOCK;status=Sub_TempControl;beepshort(); ) ;
       break;
 
     case ItemABSPreHeat_set_FanSpeed:
        {
         if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_FAN_SPEED);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_FAN_SPEED);
           lcd.setCursor(13,line);lcd.print(ftostr3(absPreheatFanSpeed));
         }
         
@@ -2853,7 +2857,7 @@ void MainMenu::showABSsettings()
       {
         if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_NOZZLE);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_NOZZLE);
           lcd.setCursor(13,line);lcd.print(ftostr3(absPreheatHotendTemp));
         } 
         
@@ -2887,7 +2891,7 @@ void MainMenu::showABSsettings()
       {
         if(force_lcd_update)
         {
-          lcd.setCursor(0,line);lcdprintPGM(MSG_BED);
+          lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_BED);
           lcd.setCursor(13,line);lcd.print(ftostr3(absPreheatHPBTemp));
         } 
         
@@ -2920,7 +2924,7 @@ void MainMenu::showABSsettings()
     {
       if(force_lcd_update)
       {
-        lcd.setCursor(0,line);lcdprintPGM(MSG_STORE_EPROM);
+        lcd.setCursor(0,line);LCD_PRINT_PGM(MSG_STORE_EPROM);
       }
       if((activeline==line) && CLICKED)
       {
diff --git a/Marlin/ultralcd.h b/Marlin/ultralcd.h
index fbc12e987f..8b9feeacbc 100644
--- a/Marlin/ultralcd.h
+++ b/Marlin/ultralcd.h
@@ -1,46 +1,41 @@
 #ifndef ULTRALCD_H
 #define ULTRALCD_H
+
 #include "Marlin.h"
+
 #ifdef ULTRA_LCD
 #include "language.h"
+
 #if LANGUAGE_CHOICE == 6
 #include "LiquidCrystalRus.h"
+#define LCD_CLASS LiquidCrystalRus
 #else
 #include <LiquidCrystal.h>
+#define LCD_CLASS LiquidCrystal
 #endif
-  void lcd_status();
-  void lcd_init();
-  void lcd_status(const char* message);
-  void beep();
-  void buttons_init();
-  void buttons_check();
 
-  #define LCD_UPDATE_INTERVAL 100
-  #define STATUSTIMEOUT 15000
-#if LANGUAGE_CHOICE == 6
-  extern LiquidCrystalRus lcd;
-#else
-  extern LiquidCrystal lcd;
-#endif
-  extern volatile char buttons;  //the last checked buttons in a bit array.
+void lcd_status();
+void lcd_init();
+void lcd_status(const char* message);
+void beep();
+void buttons_init();
+void buttons_check();
+
+#define LCD_UPDATE_INTERVAL 100
+#define STATUSTIMEOUT 15000
+
+extern LCD_CLASS lcd;
+
+extern volatile char buttons;  //the last checked buttons in a bit array.
   
-  #ifdef NEWPANEL
+#ifdef NEWPANEL
     #define EN_C (1<<BLEN_C)
     #define EN_B (1<<BLEN_B)
     #define EN_A (1<<BLEN_A)
-    
+
     #define CLICKED (buttons&EN_C)
     #define BLOCK {blocking=millis()+blocktime;}
-    #if (SDCARDDETECT > -1)
-      #ifdef SDCARDDETECTINVERTED 
-        #define CARDINSERTED (READ(SDCARDDETECT)!=0)
-      #else
-        #define CARDINSERTED (READ(SDCARDDETECT)==0)
-      #endif
-    #endif  //SDCARDTETECTINVERTED
-
-  #else
-
+#else
     //atomatic, do not change
     #define B_LE (1<<BL_LE)
     #define B_UP (1<<BL_UP)
@@ -53,9 +48,18 @@
     
     #define CLICKED ((buttons&B_MI)||(buttons&B_ST))
     #define BLOCK {blocking[BL_MI]=millis()+blocktime;blocking[BL_ST]=millis()+blocktime;}
-    
-  #endif
+#endif
 
+#if (SDCARDDETECT > -1)
+#ifdef SDCARDDETECTINVERTED 
+#define CARDINSERTED (READ(SDCARDDETECT)!=0)
+#else
+#define CARDINSERTED (READ(SDCARDDETECT)==0)
+#endif //SDCARDTETECTINVERTED
+#else
+//If we don't have a card detect line, aways asume the card is inserted
+#define CARDINSERTED true
+#endif
 
     
   // blocking time for recognizing a new keypress of one key, ms
@@ -152,8 +156,8 @@
 
   #define LCD_INIT lcd_init();
   #define LCD_MESSAGE(x) lcd_status(x);
-  #define LCD_MESSAGEPGM(x) lcd_statuspgm(MYPGM(x));
-  #define LCD_ALERTMESSAGEPGM(x) lcd_alertstatuspgm(MYPGM(x));
+  #define LCD_MESSAGEPGM(x) lcd_statuspgm(PSTR(x));
+  #define LCD_ALERTMESSAGEPGM(x) lcd_alertstatuspgm(PSTR(x));
   #define LCD_STATUS lcd_status()
 #else //no lcd
   #define LCD_INIT
@@ -178,4 +182,14 @@ char *itostr31(const int &xx);
 char *itostr3(const int &xx);
 char *itostr4(const int &xx);
 char *ftostr51(const float &x);
+
+//TODO: These do not belong here.
+extern int plaPreheatHotendTemp;
+extern int plaPreheatHPBTemp;
+extern int plaPreheatFanSpeed;
+
+extern int absPreheatHotendTemp;
+extern int absPreheatHPBTemp;
+extern int absPreheatFanSpeed;
+
 #endif //ULTRALCD
diff --git a/Marlin/watchdog.cpp b/Marlin/watchdog.cpp
new file mode 100644
index 0000000000..19e6c54d49
--- /dev/null
+++ b/Marlin/watchdog.cpp
@@ -0,0 +1,54 @@
+#include "Marlin.h"
+
+#ifdef USE_WATCHDOG
+#include "watchdog.h"
+#include "ultralcd.h"
+
+//===========================================================================
+//=============================private variables  ============================
+//===========================================================================
+
+//===========================================================================
+//=============================functinos         ============================
+//===========================================================================
+
+
+/// intialise watch dog with a 1 sec interrupt time
+void watchdog_init()
+{
+#ifdef RESET_MANUAL
+    //We enable the watchdog timer, but only for the interrupt.
+    //Take care, as this requires the correct order of operation, with interrupts disabled. See the datasheet of any AVR chip for details.
+    wdt_reset();
+    _WD_CONTROL_REG = _BV(_WD_CHANGE_BIT) | _BV(WDE);
+    _WD_CONTROL_REG = _BV(WDIE) | WDTO_1S;
+#else
+    wdt_enable(WDTO_1S);
+#endif
+}
+
+/// reset watchdog. MUST be called every 1s after init or avr will reset.
+void watchdog_reset() 
+{
+  wdt_reset();
+}
+
+//===========================================================================
+//=============================ISR               ============================
+//===========================================================================
+
+//Watchdog timer interrupt, called if main program blocks >1sec and manual reset is enabled.
+#ifdef RESET_MANUAL
+ISR(WDT_vect)
+{ 
+    LCD_MESSAGEPGM("ERR:Please Reset");//16 characters so it fits on a 16x2 display
+    LCD_STATUS;
+    SERIAL_ERROR_START;
+    SERIAL_ERRORLNPGM("Something is wrong, please turn off the printer.");
+
+    kill(); //kill blocks
+    while(1); //wait for user or serial reset
+}
+#endif//RESET_MANUAL
+
+#endif//USE_WATCHDOG
diff --git a/Marlin/watchdog.h b/Marlin/watchdog.h
index 937379b696..7f78453530 100644
--- a/Marlin/watchdog.h
+++ b/Marlin/watchdog.h
@@ -1,16 +1,17 @@
 #ifndef WATCHDOG_H
 #define WATCHDOG_H
+
 #include "Marlin.h"
+
 #ifdef USE_WATCHDOG
-
   // intialise watch dog with a 1 sec interrupt time
-  void wd_init();
-  // pad the dog/reset watchdog. MUST be called at least every second after the first wd_init or avr will go into emergency procedures..
-  void wd_reset();
-
+  void watchdog_init();
+  // pad the dog/reset watchdog. MUST be called at least every second after the first watchdog_init or avr will go into emergency procedures..
+  void watchdog_reset();
 #else
-  FORCE_INLINE void wd_init() {};
-  FORCE_INLINE void wd_reset() {};
+  //If we do not have a watchdog, then we can have empty functions which are optimized away.
+  FORCE_INLINE void watchdog_init() {};
+  FORCE_INLINE void watchdog_reset() {};
 #endif
 
 #endif
diff --git a/Marlin/watchdog.pde b/Marlin/watchdog.pde
deleted file mode 100644
index 356e374738..0000000000
--- a/Marlin/watchdog.pde
+++ /dev/null
@@ -1,63 +0,0 @@
-#ifdef USE_WATCHDOG
-#include "Marlin.h"
-#include "watchdog.h"
-
-//===========================================================================
-//=============================private variables  ============================
-//===========================================================================
-
-static volatile uint8_t timeout_seconds=0;
-
-void(* ctrlaltdelete) (void) = 0; //does not work on my atmega2560
-
-//===========================================================================
-//=============================functinos         ============================
-//===========================================================================
-
-
-/// intialise watch dog with a 1 sec interrupt time
-void wd_init() 
-{
-  WDTCSR |= (1<<WDCE )|(1<<WDE ); //allow changes
-  WDTCSR = (1<<WDCE )|(1<<WDE )|(1<<WDP3 )|(1<<WDP0); // Reset after 8 sec.
-//  WDTCSR = (1<<WDIF)|(1<<WDIE)| (1<<WDCE )|(1<<WDE )|  (1<<WDP3) | (1<<WDP0);
-}
-
-/// reset watchdog. MUST be called every 1s after init or avr will reset.
-void wd_reset() 
-{
-  wdt_reset();
-}
-
-//===========================================================================
-//=============================ISR               ============================
-//===========================================================================
-
-//Watchdog timer interrupt, called if main program blocks >1sec
-ISR(WDT_vect) 
-{ 
-  if(timeout_seconds++ >= WATCHDOG_TIMEOUT)
-  {
- 
-    #ifdef RESET_MANUAL
-      LCD_MESSAGEPGM("Please Reset!");
-      LCD_STATUS;
-      SERIAL_ERROR_START;
-      SERIAL_ERRORLNPGM("Something is wrong, please turn off the printer.");
-    #else
-      LCD_MESSAGEPGM("Timeout, resetting!");
-      LCD_STATUS;
-    #endif 
-    //disable watchdog, it will survife reboot.
-    WDTCSR |= (1<<WDCE) | (1<<WDE);
-    WDTCSR = 0;
-    #ifdef RESET_MANUAL
-      kill(); //kill blocks
-      while(1); //wait for user or serial reset
-    #else
-      ctrlaltdelete();
-    #endif
-  }
-}
-
-#endif /* USE_WATCHDOG */