Merge pull request #206 from kliment/experimental

Merge experimental into master

13 files changed, 1134 insertions, 352 deletions

diff --git a/Sprinter/Configuration.h b/Sprinter/Configuration.h
index a9243d2..3d7a399 100644
--- a/Sprinter/Configuration.h
+++ b/Sprinter/Configuration.h
@@ -7,6 +7,7 @@
 // MEGA/RAMPS up to 1.2  = 3,
 // RAMPS 1.3/1.4 = 33
 // Gen6 = 5, 
+// Gen6 deluxe = 51
 // Sanguinololu up to 1.1 = 6
 // Sanguinololu 1.2 and above = 62
 // Gen 7 @ 16MHZ only= 7
@@ -152,6 +153,10 @@ const int NUM_AXIS = 4; // The axis order in all axis related arrays is X, Y, Z,
 
 #define MAX_STEP_FREQUENCY 30000 // Max step frequency
 
+//For the retract (negative Extruder) move this maxiumum Limit of Feedrate is used
+//The next positive Extruder move use also this Limit, 
+//then for the next (second after retract) move the original Maximum (_MAX_FEEDRATE) Limit is used
+#define MAX_RETRACT_FEEDRATE 100    //mm/sec
 
 //-----------------------------------------------------------------------
 //// Not used at the Moment
@@ -166,7 +171,9 @@ const int NUM_AXIS = 4; // The axis order in all axis related arrays is X, Y, Z,
 //#define STEP_DELAY_RATIO 0.25
 
 ///Oscillation reduction.  Forces x,y,or z axis to be stationary for ## ms before allowing axis to switch direcitons.  Alternative method to prevent skipping steps.  Uncomment the line below to activate.
+// At this Version with Planner this Function ist not used
 //#define RAPID_OSCILLATION_REDUCTION
+
 #ifdef RAPID_OSCILLATION_REDUCTION
 const long min_time_before_dir_change = 30; //milliseconds
 #endif
@@ -179,6 +186,7 @@ const long min_time_before_dir_change = 30; //milliseconds
 #define _RETRACT_ACCELERATION 2000 // Extruder Normal acceleration mm/s^2
 #define _MAX_XY_JERK 20.0
 #define _MAX_Z_JERK 0.4
+#define _MAX_E_JERK 5.0    // (mm/sec)
 //#define _MAX_START_SPEED_UNITS_PER_SECOND {25.0,25.0,0.2,10.0}
 #define _MAX_ACCELERATION_UNITS_PER_SQ_SECOND {5000,5000,50,5000}    // X, Y, Z and E max acceleration in mm/s^2 for printing moves or retracts
 
@@ -186,11 +194,13 @@ const long min_time_before_dir_change = 30; //milliseconds
 // Minimum planner junction speed. Sets the default minimum speed the planner plans for at the end
 // of the buffer and all stops. This should not be much greater than zero and should only be changed
 // if unwanted behavior is observed on a user's machine when running at very slow speeds.
-#define MINIMUM_PLANNER_SPEED 2.0 // (mm/sec)
+#define MINIMUM_PLANNER_SPEED 0.05 // (mm/sec)
 
 #define DEFAULT_MINIMUMFEEDRATE       0.0     // minimum feedrate
 #define DEFAULT_MINTRAVELFEEDRATE     0.0
 
+#define _MIN_SEG_TIME 20000
+
 // If defined the movements slow down when the look ahead buffer is only half full
 #define SLOWDOWN
 
@@ -231,6 +241,26 @@ const int dropsegments=5; //everything with less than this number of steps will
 #define N_ARC_CORRECTION 25
 
 //-----------------------------------------------------------------------
+//// FANCONTROL WITH SOFT PWM
+//-----------------------------------------------------------------------
+
+//With this option its possible to drive the fan with SOFT PWM (500hz) and use
+//every Digital output for it, main usage for Sanguinololu
+#define FAN_SOFT_PWM
+
+//-----------------------------------------------------------------------
+//// MINIMUM START SPEED FOR FAN
+//-----------------------------------------------------------------------
+
+//Minimum start speed for FAN when the last speed was zero
+//Set to 0 to deaktivate
+//If value is set the fan will drive with this minimum speed for MINIMUM_FAN_START_TIME
+#define MINIMUM_FAN_START_SPEED  0
+
+//This is the time how long the minimum FAN speed is set
+#define MINIMUM_FAN_START_TIME  6000    //6sec
+
+//-----------------------------------------------------------------------
 //// HEATERCONTROL AND PID PARAMETERS
 //-----------------------------------------------------------------------
 
@@ -261,6 +291,10 @@ const int dropsegments=5; //everything with less than this number of steps will
 //Command M601 / Command M602 Reset the MIN/MAX Value
 //#define DEBUG_HEATER_TEMP
 
+// M303 - PID relay autotune S<temperature> sets the target temperature. 
+// (default target temperature = 150C)
+#define PID_AUTOTUNE
+
 //PID Controler Settings
 #define PID_INTEGRAL_DRIVE_MAX 80 // too big, and heater will lag after changing temperature, too small and it might not compensate enough for long-term errors
 #define PID_PGAIN 2560 //256 is 1.0  // value of X means that error of 1 degree is changing PWM duty by X, probably no need to go over 25
@@ -274,7 +308,7 @@ const int dropsegments=5; //everything with less than this number of steps will
 #define LED_PWM_FOR_BRIGHTNESS(brightness) ((64*brightness-1384)/(300-brightness))
 #endif
 
-// Change this value (range 1-255) to limit the current to the nozzle
+// Change this value (range 30-255) to limit the current to the nozzle
 #define HEATER_CURRENT 255
 
 // How often should the heater check for new temp readings, in milliseconds
@@ -331,6 +365,11 @@ const int dropsegments=5; //everything with less than this number of steps will
 //#define CONTROLLERFAN_PIN 23 //Pin used for the fan to cool controller, comment out to disable this function
 #define CONTROLLERFAN_SEC 60 //How many seconds, after all motors were disabled, the fan should run
 
+//This is for controlling a fan that will keep the extruder cool.
+//#define EXTRUDERFAN_PIN 66 //Pin used to control the fan, comment out to disable this function
+#define EXTRUDERFAN_DEC 50 //Hotend temperature from where the fan will be turned on
+
+//#define CHAIN_OF_COMMAND 1 //Finish buffered moves before executing M42, fan speed, heater target, and so...
 
 //-----------------------------------------------------------------------
 // DEBUGING
diff --git a/Sprinter/Sd2Card.cpp b/Sprinter/Sd2Card.cpp
index 8222cfd..05832e7 100644
--- a/Sprinter/Sd2Card.cpp
+++ b/Sprinter/Sd2Card.cpp
@@ -17,7 +17,11 @@
  * along with the Arduino Sd2Card Library.  If not, see

  * <http://www.gnu.org/licenses/>.

  */

+#if defined(ARDUINO) && ARDUINO >= 100

+#include <Arduino.h>

+#else

 #include <WProgram.h>

+#endif

 #include "Sd2Card.h"

 //------------------------------------------------------------------------------

 #ifndef SOFTWARE_SPI

diff --git a/Sprinter/SdFatUtil.h b/Sprinter/SdFatUtil.h
index de3fee3..361a5a2 100644
--- a/Sprinter/SdFatUtil.h
+++ b/Sprinter/SdFatUtil.h
@@ -23,7 +23,11 @@
  * \file

  * Useful utility functions.

  */

+#if defined(ARDUINO) && ARDUINO >= 100

+#include <Arduino.h>

+#else

 #include <WProgram.h>

+#endif

 #include <avr/pgmspace.h>

 /** Store and print a string in flash memory.*/

 #define PgmPrint(x) SerialPrint_P(PSTR(x))

diff --git a/Sprinter/SdFile.cpp b/Sprinter/SdFile.cpp
index 13f4c6a..c847e01 100644
--- a/Sprinter/SdFile.cpp
+++ b/Sprinter/SdFile.cpp
@@ -19,7 +19,11 @@
  */
 #include "SdFat.h"
 #include <avr/pgmspace.h>
+#if defined(ARDUINO) && ARDUINO >= 100
+#include <Arduino.h>
+#else
 #include <WProgram.h>
+#endif
 //------------------------------------------------------------------------------
 // callback function for date/time
 void (*SdFile::dateTime_)(uint16_t* date, uint16_t* time) = NULL;
diff --git a/Sprinter/Sprinter.h b/Sprinter/Sprinter.h
index 66e6c59..50f3806 100644
--- a/Sprinter/Sprinter.h
+++ b/Sprinter/Sprinter.h
@@ -123,6 +123,10 @@ void check_buffer_while_arc();
 void print_disk_info(void);
 #endif //SDSUPPORT
 
+#if (MINIMUM_FAN_START_SPEED > 0)
+void manage_fan_start_speed(void);
+#endif
+
 #ifdef DEBUG
 void log_message(char*   message);
 void log_bool(char* message, bool value);
diff --git a/Sprinter/Sprinter.pde b/Sprinter/Sprinter.pde
index 0254a68..6383f22 100644
--- a/Sprinter/Sprinter.pde
+++ b/Sprinter/Sprinter.pde
@@ -1,6 +1,6 @@
 /*
  Reprap firmware based on Sprinter
- Optimize for Sanguinololu 1.2 and above, RAMPS 
+ Optimized for Sanguinololu 1.2 and above / RAMPS 
  
  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
@@ -16,7 +16,7 @@
  along with this program.  If not, see <http://www.gnu.org/licenses/>. */
 
 /*
-  This firmware is a mashup between Sprinter,grbl and parts from marlin.
+  This firmware is a mashup between Sprinter, grbl and parts from marlin.
   (https://github.com/kliment/Sprinter)
   
   Changes by Doppler Michael (midopple)
@@ -30,9 +30,9 @@
   Sprinter Changelog
   -  Look forward function --> calculate 16 Steps forward, get from Firmaware Marlin and Grbl
   -  Stepper control with Timer 1 (Interrupt)
-  -  Extruder heating with PID use a Softpwm (Timer 2) with 500 hz to free Timer1 für Steppercontrol
+  -  Extruder heating with PID use a Softpwm (Timer 2) with 500 hz to free Timer1 for Steppercontrol
   -  command M220 Sxxx --> tune Printing speed online (+/- 50 %)
-  -  G2 / G3 command --> circle funktion
+  -  G2 / G3 command --> circle function
   -  Baudrate set to 250 kbaud
   -  Testet on Sanguinololu Board
   -  M30 Command can delete files on SD Card
@@ -42,15 +42,15 @@
  Version 1.3.04T
   - Implement Plannercode from Marlin V1 big thanks to Erik
   - Stepper interrupt with Step loops
-  - Stepperfrequenz 30 Khz
+  - Stepperfrequency 30 Khz
   - New Command
     * M202 - 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 
-    * M205 - advanced settings:  minimum travel speed S=while printing T=travel only,  X= maximum xy jerk, Z=maximum Z jerk
+    * M205 - advanced settings:  minimum travel speed S=while printing T=travel only,  X= maximum xy jerk, Z=maximum Z jerk, E = max E jerk
   - Remove unused Variables
   - Check Uart Puffer while circle processing (CMD: G2 / G3)
   - Fast Xfer Function --> move Text to Flash
-  - Option to deaktivate ARC (G2/G3) function (save flash)
+  - Option to deactivate ARC (G2/G3) function (save flash)
   - Removed modulo (%) operator, which uses an expensive divide
 
  Version 1.3.05T
@@ -77,7 +77,7 @@
    A ';' inside a line ignores just the portion following the ';' character.
    The beginning of the line is still interpreted.
    
- - Same fix for SD Card, testet and work
+ - Same fix for SD Card, tested and work
 
  Version 1.3.09T
  - Move SLOWDOWN Function up
@@ -94,9 +94,54 @@
 - Make fastio & Arduino pin numbering consistent for AT90USB128x. --> Thanks to lincomatic
 - Select Speedtable with F_CPU
 - Use same Values for Speedtables as Marlin 
-- 
 
+ Version 1.3.12T
+- Fixed arc offset.
+
+ Version 1.3.13T
+- Extrudemultiply with code M221 Sxxx (S100 original Extrude value)
+- use Feedratefactor only when Extrude > 0
+- M106 / M107 can drive the FAN with PWM + Port check for not using Timer 1
+- Added M93 command. Sends current steps for all axis.
+- New Option --> FAN_SOFT_PWM, with this option the FAN PWM can use every digital I/O
+
+ Version 1.3.14T
+- When endstop is hit count the virtual steps, so the print lose no position when endstop is hit
+
+ Version 1.3.15T
+- M206 - set additional homing offset 
+- Option for minimum FAN start speed --> #define MINIMUM_FAN_START_SPEED  50  (set it to zero to deactivate)
   
+ Version 1.3.16T
+- Extra Max Feedrate for Retract (MAX_RETRACT_FEEDRATE)
+
+ Version 1.3.17T
+- M303 - PID relay autotune possible
+- G4 Wait until last move is done
+
+ Version 1.3.18T
+- Problem with Thermistor 3 table when sensor is broken and temp is -20 °C
+
+ Version 1.3.19T
+- Set maximum acceleration. If "steps per unit" is Change the acc were not recalculated
+- Extra Parameter for Max Extruder Jerk
+- New Parameter (max_e_jerk) in EEPROM --> Default settings after update !
+
+ Version 1.3.20T
+- fix a few typos and correct english usage
+- reimplement homing routine as an inline function
+- refactor eeprom routines to make it possible to modify the value of a single parameter
+- calculate eeprom parameter addresses based on previous param address plus sizeof(type)
+- add 0 C point in Thermistortable 7
+
+ Version 1.3.21T
+- M301 set PID Parameter, and Store to EEPROM
+- If no PID is used, deaktivate Variables for PID settings
+
+ Version 1.3.22T
+- Error in JERK calculation after G92 command is send, make problems 
+  with Z-Lift function in Slic3r
+- Add homing values can shown with M206 D
 
 */
 
@@ -173,6 +218,7 @@ void __cxa_pure_virtual(){};
 //        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
+// M93  - Send axis_steps_per_unit
 // M115	- Capabilities string
 // M119 - Show Endstopper State 
 // M140 - Set bed target temp
@@ -180,23 +226,30 @@ void __cxa_pure_virtual(){};
 // M201 - Set maximum acceleration in units/s^2 for print moves (M201 X1000 Y1000)
 // M202 - Set maximum feedrate that your machine can sustain (M203 X200 Y200 Z300 E10000) in mm/sec
 // M203 - Set temperture monitor to Sx
-// M204 - Set default acceleration: S normal moves T filament only moves (M204 S3000 T7000) im mm/sec^2 
-// M205 - advanced settings:  minimum travel speed S=while printing T=travel only,  X= maximum xy jerk, Z=maximum Z jerk
+// M204 - Set default acceleration: S normal moves T filament only moves (M204 S3000 T7000) in mm/sec^2
+// M205 - advanced settings:  minimum travel speed S=while printing T=travel only,  X=maximum xy jerk, Z=maximum Z jerk
+// M206 - set additional homing offset
+
+// M220 - set speed factor override percentage S=factor in percent 
+// M221 - set extruder multiply factor S100 --> original Extrude Speed 
+
+// M301 - Set PID parameters P I and D
+// M303 - PID relay autotune S<temperature> sets the target temperature. (default target temperature = 150C)
 
-// M220 - set speed factor override percentage S:factor in percent 
+// M400 - Finish all moves
 
 // M500 - stores paramters in EEPROM
-// M501 - reads parameters from EEPROM (if you need reset them after you changed them temporarily).
+// M501 - reads parameters from EEPROM (if you need to 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 settings
 
 // Debug feature / Testing the PID for Hotend
-// M601 - Show Temp jitter from Extruder (min / max value from Hotend Temperatur while printing)
-// M602 - Reset Temp jitter from Extruder (min / max val) --> Dont use it while Printing
+// M601 - Show Temp jitter from Extruder (min / max value from Hotend Temperature while printing)
+// M602 - Reset Temp jitter from Extruder (min / max val) --> Don't use it while Printing
 // M603 - Show Free Ram
 
 
-#define _VERSION_TEXT "1.3.11T / 19.03.2012"
+#define _VERSION_TEXT "1.3.22T / 20.08.2012"
 
 //Stepper Movement Variables
 char axis_codes[NUM_AXIS] = {'X', 'Y', 'Z', 'E'};
@@ -210,6 +263,11 @@ float move_acceleration = _ACCELERATION;         // Normal acceleration mm/s^2
 float retract_acceleration = _RETRACT_ACCELERATION; // Normal acceleration mm/s^2
 float max_xy_jerk = _MAX_XY_JERK;
 float max_z_jerk = _MAX_Z_JERK;
+float max_e_jerk = _MAX_E_JERK;
+unsigned long min_seg_time = _MIN_SEG_TIME;
+#ifdef PIDTEMP
+ unsigned int PID_Kp = PID_PGAIN, PID_Ki = PID_IGAIN, PID_Kd = PID_DGAIN;
+#endif
 
 long  max_acceleration_units_per_sq_second[4] = _MAX_ACCELERATION_UNITS_PER_SQ_SECOND; // X, Y, Z and E max acceleration in mm/s^2 for printing moves or retracts
 
@@ -228,16 +286,21 @@ unsigned long plateau_steps;
 //unsigned long steps_per_sqr_second;
 
 
-//adjustable feed faktor for online tuning printerspeed
-volatile int feedmultiply=100; //100->original / 200-> Faktor 2 / 50 -> Faktor 0.5
+//adjustable feed factor for online tuning printer speed
+volatile int feedmultiply=100; //100->original / 200 -> Factor 2 / 50 -> Factor 0.5
 int saved_feedmultiply;
 volatile bool feedmultiplychanged=false;
+volatile int extrudemultiply=100; //100->1 200->2
 
 //boolean acceleration_enabled = false, accelerating = false;
 //unsigned long interval;
 float destination[NUM_AXIS] = {0.0, 0.0, 0.0, 0.0};
 float current_position[NUM_AXIS] = {0.0, 0.0, 0.0, 0.0};
+float add_homing[3]={0,0,0};
 
+static unsigned short virtual_steps_x = 0;
+static unsigned short virtual_steps_y = 0;
+static unsigned short virtual_steps_z = 0;
 
 bool home_all_axis = true;
 //unsigned ?? ToDo: Check
@@ -260,7 +323,7 @@ bool is_homing = false;
 
 
 #ifdef USE_ARC_FUNCTION
-//For arc centerpont, send bei Command G2/G3
+//For arc center point coordinates, sent by commands G2/G3
 float offset[3] = {0.0, 0.0, 0.0};
 #endif
 
@@ -275,6 +338,12 @@ float offset[3] = {0.0, 0.0, 0.0};
   float osc_wait_remainder = 0.0;
 #endif
 
+#if (MINIMUM_FAN_START_SPEED > 0)
+  unsigned char fan_last_speed = 0;
+  unsigned char fan_org_start_speed = 0;
+  unsigned long previous_millis_fan_start = 0;
+#endif
+
 // comm variables and Commandbuffer
 // BUFSIZE is reduced from 8 to 6 to free more RAM for the PLANNER
 #define MAX_CMD_SIZE 96
@@ -305,7 +374,7 @@ unsigned long previous_millis_cmd = 0;
 unsigned long max_inactive_time = 0;
 unsigned long stepper_inactive_time = 0;
 
-//Temp Montor for repetier
+//Temp Monitor for repetier
 unsigned char manage_monitor = 255;
 
 
@@ -562,8 +631,8 @@ int FreeRam1(void)
 void analogWrite_check(uint8_t check_pin, int val)
 {
   #if defined(__AVR_ATmega168__) || defined(__AVR_ATmega328P__) 
-  //Atmega168 / 328 can not useed OCR1A and OCR1B
-  //This are PINS PB1 / PB2 or on Ardurino D9 / D10
+  //Atmega168/328 can't use OCR1A and OCR1B
+  //These are pins PB1/PB2 or on Arduino D9/D10
     if((check_pin != 9) && (check_pin != 10))
     {
         analogWrite(check_pin, val);
@@ -571,8 +640,8 @@ void analogWrite_check(uint8_t check_pin, int val)
   #endif
   
   #if defined(__AVR_ATmega644P__) || defined(__AVR_ATmega1284P__) 
-  //Atmega664P / 1284P can not useed OCR1A and OCR1B
-  //This are PINS PD4 / PD5 or on Ardurino D12 / D13
+  //Atmega664P/1284P can't use OCR1A and OCR1B
+  //These are pins PD4/PD5 or on Arduino D12/D13
     if((check_pin != 12) && (check_pin != 13))
     {
         analogWrite(check_pin, val);
@@ -580,8 +649,8 @@ void analogWrite_check(uint8_t check_pin, int val)
   #endif
 
   #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) 
-  //Atmega1280 / 2560 can not useed OCR1A, OCR1B and OCR1C
-  //This are PINS PB5,PB6,PB7 or on Ardurino D11,D12,and D13
+  //Atmega1280/2560 can't use OCR1A, OCR1B and OCR1C
+  //These are pins PB5,PB6,PB7 or on Arduino D11,D12 and D13
     if((check_pin != 11) && (check_pin != 12) && (check_pin != 13))
     {
         analogWrite(check_pin, val);
@@ -657,6 +726,10 @@ void setup()
     SET_OUTPUT(CONTROLLERFAN_PIN); //Set pin used for driver cooling fan
   #endif
   
+  #ifdef EXTRUDERFAN_PIN
+    SET_OUTPUT(EXTRUDERFAN_PIN); //Set pin used for extruder cooling fan
+  #endif
+  
   //endstops and pullups
   #ifdef ENDSTOPPULLUPS
   #if X_MIN_PIN > -1
@@ -744,10 +817,7 @@ void setup()
     SET_OUTPUT(E_STEP_PIN);
   #endif  
 
-  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];
-  }
+  
 
 //  for(int i=0; i < NUM_AXIS; i++){
 //      axis_max_interval[i] = 100000000.0 / (max_start_speed_units_per_second[i] * axis_steps_per_unit[i]);
@@ -782,7 +852,7 @@ void setup()
 
 #endif
 
-  #ifdef PID_SOFT_PWM
+  #if defined(PID_SOFT_PWM) || (defined(FAN_SOFT_PWM) && (FAN_PIN > -1))
   showString(PSTR("Soft PWM Init\r\n"));
   init_Timer2_softpwm();
   #endif
@@ -798,6 +868,10 @@ void setup()
   //second value --> Print settings to UART
   EEPROM_RetrieveSettings(false,false);
   #endif
+  
+  #ifdef PIDTEMP
+  updatePID();
+  #endif
 
   //Free Ram
   showString(PSTR("Free Ram: "));
@@ -808,6 +882,12 @@ void setup()
   Serial.print((int)sizeof(block_t)*BLOCK_BUFFER_SIZE);
   showString(PSTR(" / "));
   Serial.println(BLOCK_BUFFER_SIZE);
+  
+  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];
+  }
+
 }
 
 
@@ -856,6 +936,10 @@ void loop()
   //check heater every n milliseconds
   manage_heater();
   manage_inactivity(1);
+  #if (MINIMUM_FAN_START_SPEED > 0)
+    manage_fan_start_speed();
+  #endif
+  
 }
 
 //------------------------------------------------
@@ -875,10 +959,10 @@ void check_buffer_while_arc()
 //------------------------------------------------
 void get_command() 
 { 
-  while( Serial.available() > 0  && buflen < BUFSIZE)
+  while( Serial.available() > 0 && buflen < BUFSIZE)
   {
     serial_char = Serial.read();
-    if(serial_char == '\n' || serial_char == '\r' || serial_char == ':' || serial_count >= (MAX_CMD_SIZE - 1) ) 
+    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
@@ -984,12 +1068,12 @@ void get_command()
   {
     return;
   }
-  while( filesize > sdpos  && buflen < BUFSIZE)
+  while( filesize > sdpos && buflen < BUFSIZE)
   {
     serial_char = file.read();
     read_char_int = (int)serial_char;
     
-    if(serial_char == '\n' || serial_char == '\r' || serial_char == ':' || serial_count >= (MAX_CMD_SIZE - 1) || read_char_int == -1) 
+    if(serial_char == '\n' || serial_char == '\r' || (serial_char == ':' && comment_mode == false) || serial_count >= (MAX_CMD_SIZE - 1) || read_char_int == -1) 
     {
         sdpos = file.curPosition();
         if(sdpos >= filesize)
@@ -1042,6 +1126,67 @@ FORCE_INLINE bool code_seen(char code)
   return (strchr_pointer != NULL);  //Return True if a character was found
 }
 
+FORCE_INLINE void homing_routine(char axis)
+{
+  int min_pin, max_pin, home_dir, max_length, home_bounce;
+
+  switch(axis){
+    case X_AXIS:
+      min_pin = X_MIN_PIN;
+      max_pin = X_MAX_PIN;
+      home_dir = X_HOME_DIR;
+      max_length = X_MAX_LENGTH;
+      home_bounce = 10;
+      break;
+    case Y_AXIS:
+      min_pin = Y_MIN_PIN;
+      max_pin = Y_MAX_PIN;
+      home_dir = Y_HOME_DIR;
+      max_length = Y_MAX_LENGTH;
+      home_bounce = 10;
+      break;
+    case Z_AXIS:
+      min_pin = Z_MIN_PIN;
+      max_pin = Z_MAX_PIN;
+      home_dir = Z_HOME_DIR;
+      max_length = Z_MAX_LENGTH;
+      home_bounce = 4;
+      break;
+    default:
+      //never reached
+      break;
+  }
+
+  if ((min_pin > -1 && home_dir==-1) || (max_pin > -1 && home_dir==1))
+  {
+    current_position[axis] = -1.5 * max_length * home_dir;
+    plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+    destination[axis] = 0;
+    feedrate = homing_feedrate[axis];
+    prepare_move();
+    st_synchronize();
+
+    current_position[axis] = home_bounce/2 * home_dir;
+    plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+    destination[axis] = 0;
+    prepare_move();
+    st_synchronize();
+
+    current_position[axis] = -home_bounce * home_dir;
+    plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+    destination[axis] = 0;
+    feedrate = homing_feedrate[axis]/2;
+    prepare_move();
+    st_synchronize();
+
+    current_position[axis] = (home_dir == -1) ? 0 : max_length;
+    current_position[axis] += add_homing[axis];
+    plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+    destination[axis] = current_position[axis];
+    feedrate = 0;
+  }
+}
+
 //------------------------------------------------
 // CHECK COMMAND AND CONVERT VALUES
 //------------------------------------------------
@@ -1084,6 +1229,7 @@ FORCE_INLINE void process_commands()
         if(code_seen('P')) codenum = code_value(); // milliseconds to wait
         if(code_seen('S')) codenum = code_value() * 1000; // seconds to wait
         codenum += millis();  // keep track of when we started waiting
+        st_synchronize();  // wait for all movements to finish
         while(millis()  < codenum ){
           manage_heater();
         }
@@ -1107,102 +1253,13 @@ FORCE_INLINE void process_commands()
         home_all_axis = !((code_seen(axis_codes[0])) || (code_seen(axis_codes[1])) || (code_seen(axis_codes[2])));
 
         if((home_all_axis) || (code_seen(axis_codes[X_AXIS]))) 
-        {
-          if ((X_MIN_PIN > -1 && X_HOME_DIR==-1) || (X_MAX_PIN > -1 && X_HOME_DIR==1))
-          {
-            st_synchronize();
-            current_position[X_AXIS] = -1.5 * X_MAX_LENGTH * X_HOME_DIR;
-            plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-            destination[X_AXIS] = 0;
-            feedrate = homing_feedrate[X_AXIS];
-            prepare_move();
-  
-            st_synchronize();        
-            current_position[X_AXIS] = 5 * X_HOME_DIR;
-            plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-            destination[X_AXIS] = 0;
-            prepare_move();
-  
-            st_synchronize();       
-            current_position[X_AXIS] = -10 * X_HOME_DIR;
-            plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);  
-            destination[X_AXIS] = 0;
-            feedrate = homing_feedrate[X_AXIS]/2 ;
-            prepare_move();
-            st_synchronize();
-  
-            current_position[X_AXIS] = (X_HOME_DIR == -1) ? 0 : X_MAX_LENGTH;
-            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];
-            feedrate = 0;
-          }
-        }
-        //showString(PSTR("HOME X AXIS\r\n"));
+          homing_routine(X_AXIS);
 
         if((home_all_axis) || (code_seen(axis_codes[Y_AXIS]))) 
-        {
-          if ((Y_MIN_PIN > -1 && Y_HOME_DIR==-1) || (Y_MAX_PIN > -1 && Y_HOME_DIR==1))
-          {
-            current_position[Y_AXIS] = -1.5 * Y_MAX_LENGTH * Y_HOME_DIR;
-            plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-            destination[Y_AXIS] = 0;
-            feedrate = homing_feedrate[Y_AXIS];
-            prepare_move();
-            st_synchronize();
-  
-            current_position[Y_AXIS] = 5 * Y_HOME_DIR;
-            plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-            destination[Y_AXIS] = 0;
-            prepare_move();
-            st_synchronize();
-  
-            current_position[Y_AXIS] = -10 * Y_HOME_DIR;
-            plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-            destination[Y_AXIS] = 0;
-            feedrate = homing_feedrate[Y_AXIS]/2;
-            prepare_move();
-            st_synchronize();
-  
-            current_position[Y_AXIS] = (Y_HOME_DIR == -1) ? 0 : Y_MAX_LENGTH;
-            plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-            destination[Y_AXIS] = current_position[Y_AXIS];
-            feedrate = 0;
-          }
-        }
-        //showString(PSTR("HOME Y AXIS\r\n"));
+          homing_routine(Y_AXIS);
 
         if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) 
-        {
-          if ((Z_MIN_PIN > -1 && Z_HOME_DIR==-1) || (Z_MAX_PIN > -1 && Z_HOME_DIR==1))
-          {
-            current_position[Z_AXIS] = -1.5 * Z_MAX_LENGTH * Z_HOME_DIR;
-            plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-            destination[Z_AXIS] = 0;
-            feedrate = homing_feedrate[Z_AXIS];
-            prepare_move();
-            st_synchronize();
-  
-            current_position[Z_AXIS] = 2 * Z_HOME_DIR;
-            plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-            destination[Z_AXIS] = 0;
-            prepare_move();
-            st_synchronize();
-  
-            current_position[Z_AXIS] = -3 * Z_HOME_DIR;
-            plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-            destination[Z_AXIS] = 0;
-            feedrate = homing_feedrate[Z_AXIS]/2;
-            prepare_move();
-            st_synchronize();
-  
-            current_position[Z_AXIS] = (Z_HOME_DIR == -1) ? 0 : Z_MAX_LENGTH;
-            plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-            destination[Z_AXIS] = current_position[Z_AXIS];
-            feedrate = 0;         
-          }
-        }    
-   
-        //showString(PSTR("HOME Z AXIS\r\n"));  
+          homing_routine(Z_AXIS);
         
         #ifdef ENDSTOPS_ONLY_FOR_HOMING
             enable_endstops(false);
@@ -1385,6 +1442,9 @@ FORCE_INLINE void process_commands()
       case 42: //M42 -Change pin status via gcode
         if (code_seen('S'))
         {
+#ifdef CHAIN_OF_COMMAND
+          st_synchronize(); // wait for all movements to finish
+#endif
           int pin_status = code_value();
           if (code_seen('P') && pin_status >= 0 && pin_status <= 255)
           {
@@ -1408,6 +1468,9 @@ FORCE_INLINE void process_commands()
         }
         break;
       case 104: // M104
+#ifdef CHAIN_OF_COMMAND
+          st_synchronize(); // wait for all movements to finish
+#endif
         if (code_seen('S')) target_raw = temp2analogh(target_temp = code_value());
         #ifdef WATCHPERIOD
             if(target_raw > current_raw)
@@ -1422,6 +1485,9 @@ FORCE_INLINE void process_commands()
         #endif
         break;
       case 140: // M140 set bed temp
+#ifdef CHAIN_OF_COMMAND
+          st_synchronize(); // wait for all movements to finish
+#endif
         #if TEMP_1_PIN > -1 || defined BED_USES_AD595
             if (code_seen('S')) target_bed_raw = temp2analogBed(code_value());
         #endif
@@ -1464,6 +1530,9 @@ FORCE_INLINE void process_commands()
         return;
         //break;
       case 109: { // M109 - Wait for extruder heater to reach target.
+#ifdef CHAIN_OF_COMMAND
+          st_synchronize(); // wait for all movements to finish
+#endif
         if (code_seen('S')) target_raw = temp2analogh(target_temp = code_value());
         #ifdef WATCHPERIOD
             if(target_raw>current_raw)
@@ -1498,6 +1567,9 @@ FORCE_INLINE void process_commands()
             codenum = millis();
           }
           manage_heater();
+          #if (MINIMUM_FAN_START_SPEED > 0)
+            manage_fan_start_speed();
+          #endif
           #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 */
@@ -1510,7 +1582,10 @@ FORCE_INLINE void process_commands()
 	    }
       }
       break;
-      case 190: // M190 - Wait bed for heater to reach target.
+      case 190: // M190 - Wait for bed heater to reach target temperature.
+#ifdef CHAIN_OF_COMMAND
+          st_synchronize(); // wait for all movements to finish
+#endif
       #if TEMP_1_PIN > -1
         if (code_seen('S')) target_bed_raw = temp2analogBed(code_value());
         codenum = millis(); 
@@ -1526,25 +1601,64 @@ FORCE_INLINE void process_commands()
             codenum = millis(); 
           }
           manage_heater();
+          #if (MINIMUM_FAN_START_SPEED > 0)
+            manage_fan_start_speed();
+          #endif
         }
       #endif
       break;
       #if FAN_PIN > -1
       case 106: //M106 Fan On
+#ifdef CHAIN_OF_COMMAND
+          st_synchronize(); // wait for all movements to finish
+#endif
         if (code_seen('S'))
         {
-            WRITE(FAN_PIN, HIGH);
-            analogWrite_check(FAN_PIN, constrain(code_value(),0,255) );
+            unsigned char l_fan_code_val = constrain(code_value(),0,255);
+            
+            #if (MINIMUM_FAN_START_SPEED > 0)
+              if(l_fan_code_val > 0 && fan_last_speed == 0)
+              {
+                 if(l_fan_code_val < MINIMUM_FAN_START_SPEED)
+                 {
+                   fan_org_start_speed = l_fan_code_val;
+                   l_fan_code_val = MINIMUM_FAN_START_SPEED;
+                   previous_millis_fan_start = millis();
+                 }
+                 fan_last_speed = l_fan_code_val;  
+              }  
+              else
+              {
+                fan_last_speed = l_fan_code_val;
+                fan_org_start_speed = 0;
+              }  
+            #endif
+          
+            #if defined(FAN_SOFT_PWM) && (FAN_PIN > -1)
+              g_fan_pwm_val = l_fan_code_val;
+            #else
+              WRITE(FAN_PIN, HIGH);
+              analogWrite_check(FAN_PIN, l_fan_code_val;
+            #endif
+            
         }
         else 
         {
-            WRITE(FAN_PIN, HIGH);
-            analogWrite_check(FAN_PIN, 255 );
+            #if defined(FAN_SOFT_PWM) && (FAN_PIN > -1)
+              g_fan_pwm_val = 255;
+            #else
+              WRITE(FAN_PIN, HIGH);
+              analogWrite_check(FAN_PIN, 255 );
+            #endif
         }
         break;
       case 107: //M107 Fan Off
-          analogWrite_check(FAN_PIN, 0);
-          WRITE(FAN_PIN, LOW);
+          #if defined(FAN_SOFT_PWM) && (FAN_PIN > -1)
+            g_fan_pwm_val = 0;
+          #else
+            analogWrite_check(FAN_PIN, 0);
+            WRITE(FAN_PIN, LOW);
+          #endif
         break;
       #endif
       #if (PS_ON_PIN > -1)
@@ -1552,6 +1666,9 @@ FORCE_INLINE void process_commands()
         SET_OUTPUT(PS_ON_PIN); //GND
         break;
       case 81: // M81 - ATX Power Off
+#ifdef CHAIN_OF_COMMAND
+          st_synchronize(); // wait for all movements to finish
+#endif
         SET_INPUT(PS_ON_PIN); //Floating
         break;
       #endif
@@ -1567,6 +1684,13 @@ FORCE_INLINE void process_commands()
         {
           stepper_inactive_time = code_value() * 1000; 
         }
+        else if(code_seen('T'))
+        {
+          enable_x(); 
+          enable_y(); 
+          enable_z(); 
+          enable_e(); 
+        }
         else
         { 
           disable_x(); 
@@ -1582,7 +1706,11 @@ FORCE_INLINE void process_commands()
       case 92: // M92
         for(int i=0; i < NUM_AXIS; i++) 
         {
-          if(code_seen(axis_codes[i])) axis_steps_per_unit[i] = code_value();
+          if(code_seen(axis_codes[i])) 
+          {
+            axis_steps_per_unit[i] = code_value();
+            axis_steps_per_sqr_second[i] = max_acceleration_units_per_sq_second[i] * axis_steps_per_unit[i];
+          }
         }
         
           // Update start speed intervals and axis order. TODO: refactor axis_max_interval[] calculation into a function, as it
@@ -1594,6 +1722,17 @@ FORCE_INLINE void process_commands()
 //          all steps_per_unit related variables
 //        }
         break;
+      case 93: // M93 show current axis steps.
+	showString(PSTR("ok "));
+	showString(PSTR("X:"));
+        Serial.print(axis_steps_per_unit[0]);
+	showString(PSTR("Y:"));
+        Serial.print(axis_steps_per_unit[1]);
+	showString(PSTR("Z:"));
+        Serial.print(axis_steps_per_unit[2]);
+	showString(PSTR("E:"));
+        Serial.println(axis_steps_per_unit[3]);
+        break;
       case 115: // M115
         showString(PSTR("FIRMWARE_NAME: Sprinter Experimental PROTOCOL_VERSION:1.0 MACHINE_TYPE:Mendel EXTRUDER_COUNT:1\r\n"));
         //Serial.println(uuid);
@@ -1639,7 +1778,7 @@ FORCE_INLINE void process_commands()
       
         showString(PSTR("\r\n"));
       	break;
-      case 201: // M201
+      case 201: // M201  Set maximum acceleration in units/s^2 for print moves (M201 X1000 Y1000)
 
         for(int8_t i=0; i < NUM_AXIS; i++) 
         {
@@ -1669,28 +1808,74 @@ FORCE_INLINE void process_commands()
           if(code_seen('S')) manage_monitor = code_value();
           if(manage_monitor==100) manage_monitor=1; // Set 100 to heated bed
       break;
-      case 204: // M204 acclereration S normal moves T filmanent only moves
+      case 204: // M204 acceleration S normal moves T filmanent only moves
           if(code_seen('S')) move_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
+      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, E= max E 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 homing offset
+        if(code_seen('D'))
+        {
+          showString(PSTR("Addhome X:")); Serial.print(add_homing[0]);
+          showString(PSTR(" Y:")); Serial.print(add_homing[1]);
+          showString(PSTR(" Z:")); Serial.println(add_homing[2]);
+        }
+
+        for(int8_t cnt_i=0; cnt_i < 3; cnt_i++) 
+        {
+          if(code_seen(axis_codes[cnt_i])) add_homing[cnt_i] = code_value();
+        }
       break;  
       case 220: // M220 S<factor in percent>- set speed factor override percentage
       {
         if(code_seen('S')) 
         {
           feedmultiply = code_value() ;
-          if(feedmultiply < 20) feedmultiply = 20;
-          if(feedmultiply > 200) feedmultiply = 200;
+          feedmultiply = constrain(feedmultiply, 20, 200);
           feedmultiplychanged=true;
         }
       }
       break;
+      case 221: // M221 S<factor in percent>- set extrude factor override percentage
+      {
+        if(code_seen('S')) 
+        {
+          extrudemultiply = code_value() ;
+          extrudemultiply = constrain(extrudemultiply, 40, 200);
+        }
+      }
+      break;
+#ifdef PIDTEMP
+      case 301: // M301
+      {
+        if(code_seen('P')) PID_Kp = code_value();
+        if(code_seen('I')) PID_Ki = code_value();
+        if(code_seen('D')) PID_Kd = code_value();
+        updatePID();
+      }
+      break;
+#endif //PIDTEMP      
+#ifdef PID_AUTOTUNE
+      case 303: // M303 PID autotune
+      {
+        float help_temp = 150.0;
+        if (code_seen('S')) help_temp=code_value();
+        PID_autotune(help_temp);
+      }
+      break;
+#endif
+      case 400: // M400 finish all moves
+      {
+      	st_synchronize();	
+      }
+      break;
 #ifdef USE_EEPROM_SETTINGS
       case 500: // Store settings in EEPROM
       {
@@ -1700,11 +1885,19 @@ FORCE_INLINE void process_commands()
       case 501: // Read settings from EEPROM
       {
         EEPROM_RetrieveSettings(false,true);
+        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];
+        }
       }
       break;
       case 502: // Revert to default settings
       {
         EEPROM_RetrieveSettings(true,true);
+        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];
+        }
       }
       break;
       case 503: // print settings currently in memory
@@ -1796,15 +1989,26 @@ FORCE_INLINE void get_coordinates()
 }
 
 #ifdef USE_ARC_FUNCTION
-FORCE_INLINE void get_arc_coordinates()
+void get_arc_coordinates()
 {
    get_coordinates();
-   if(code_seen('I')) offset[0] = code_value();
-   if(code_seen('J')) offset[1] = code_value();
+   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;
+   }
 }
 #endif
 
 
+
 void prepare_move()
 {
   long help_feedrate = 0;
@@ -1824,8 +2028,16 @@ void prepare_move()
       if (destination[Z_AXIS] > Z_MAX_LENGTH) destination[Z_AXIS] = Z_MAX_LENGTH;
     }
   }
+
+  if(destination[E_AXIS] > current_position[E_AXIS])
+  {
+    help_feedrate = ((long)feedrate*(long)feedmultiply);
+  }
+  else
+  {
+    help_feedrate = ((long)feedrate*(long)100);
+  }
   
-  help_feedrate = ((long)feedrate*(long)feedmultiply);
   plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], help_feedrate/6000.0);
   
   for(int i=0; i < NUM_AXIS; i++)
@@ -1842,8 +2054,15 @@ void prepare_arc_move(char isclockwise)
   float r = hypot(offset[X_AXIS], offset[Y_AXIS]); // Compute arc radius for mc_arc
   long help_feedrate = 0;
 
-  
-  help_feedrate = ((long)feedrate*(long)feedmultiply);
+  if(destination[E_AXIS] > current_position[E_AXIS])
+  {
+    help_feedrate = ((long)feedrate*(long)feedmultiply);
+  }
+  else
+  {
+    help_feedrate = ((long)feedrate*(long)100);
+  }
+
   // Trace the arc
   mc_arc(current_position, destination, offset, X_AXIS, Y_AXIS, Z_AXIS, help_feedrate/6000.0, r, isclockwise);
   
@@ -1892,8 +2111,27 @@ FORCE_INLINE void manage_inactivity(byte debug)
   check_axes_activity();
 }
 
-
-
+#if (MINIMUM_FAN_START_SPEED > 0)
+void manage_fan_start_speed(void)
+{
+  if(fan_org_start_speed > 0)
+  {
+     if((millis() - previous_millis_fan_start) > MINIMUM_FAN_START_TIME )
+     { 
+       #if FAN_PIN > -1
+         #if defined(FAN_SOFT_PWM)
+           g_fan_pwm_val = fan_org_start_speed;
+         #else
+           WRITE(FAN_PIN, HIGH);
+           analogWrite_check(FAN_PIN, fan_org_start_speed;
+         #endif  
+       #endif
+       
+       fan_org_start_speed = 0;
+     }  
+  }
+}
+#endif
 
 // Planner with Interrupt for Stepper
 
@@ -1957,6 +2195,7 @@ static int8_t prev_block_index(int8_t block_index) {
 static long position[4];   
 static float previous_speed[4]; // Speed of previous path line segment
 static float previous_nominal_speed; // Nominal speed of previous path line segment
+static unsigned char G92_reset_previous_speed = 0;
 
 
 // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the 
@@ -2008,7 +2247,7 @@ void calculate_trapezoid_for_block(block_t *block, float entry_factor, float exi
   int32_t plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps;
   
   // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
-  // have to use intersection_distance() to calculate when to abort acceleration and start braking
+  // have to use intersection_distance() to calculate when to abort acceleration and start breaking
   // in order to reach the final_rate exactly at the end of this block.
   if (plateau_steps < 0) {
     accelerate_steps = ceil(
@@ -2083,10 +2322,17 @@ void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *n
 // implements the reverse pass.
 void planner_reverse_pass() {
   uint8_t block_index = block_buffer_head;
-  if(((block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1)) > 3) {
+  
+  //Make a local copy of block_buffer_tail, because the interrupt can alter it
+  CRITICAL_SECTION_START;
+  unsigned char tail = block_buffer_tail;
+  CRITICAL_SECTION_END;
+  
+  if(((block_buffer_head-tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1)) > 3) 
+  {
     block_index = (block_buffer_head - 3) & (BLOCK_BUFFER_SIZE - 1);
     block_t *block[3] = { NULL, NULL, NULL };
-    while(block_index != block_buffer_tail) { 
+    while(block_index != tail) { 
       block_index = prev_block_index(block_index); 
       block[2]= block[1];
       block[1]= block[0];
@@ -2252,6 +2498,9 @@ void check_axes_activity() {
 
 
 float junction_deviation = 0.1;
+float max_E_feedrate_calc = MAX_RETRACT_FEEDRATE;
+bool retract_feedrate_aktiv = false;
+
 // Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in 
 // mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
 // calculation the caller must also provide the physical length of the line in millimeters.
@@ -2265,6 +2514,9 @@ void plan_buffer_line(float x, float y, float z, float e, float feed_rate)
   while(block_buffer_tail == next_buffer_head) { 
     manage_heater(); 
     manage_inactivity(1); 
+    #if (MINIMUM_FAN_START_SPEED > 0)
+      manage_fan_start_speed();
+    #endif 
   }
 
   // The target position of the tool in absolute steps
@@ -2287,6 +2539,8 @@ void plan_buffer_line(float x, float y, float z, float e, float feed_rate)
   block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);
   block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]);
   block->steps_e = labs(target[E_AXIS]-position[E_AXIS]);
+  block->steps_e *= extrudemultiply;
+  block->steps_e /= 100;
   block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e)));
 
   // Bail if this is a zero-length block
@@ -2297,7 +2551,25 @@ void plan_buffer_line(float x, float y, float z, float e, float feed_rate)
   if (target[X_AXIS] < position[X_AXIS]) { block->direction_bits |= (1<<X_AXIS); }
   if (target[Y_AXIS] < position[Y_AXIS]) { block->direction_bits |= (1<<Y_AXIS); }
   if (target[Z_AXIS] < position[Z_AXIS]) { block->direction_bits |= (1<<Z_AXIS); }
-  if (target[E_AXIS] < position[E_AXIS]) { block->direction_bits |= (1<<E_AXIS); }
+  if (target[E_AXIS] < position[E_AXIS]) 
+  { 
+    block->direction_bits |= (1<<E_AXIS); 
+    //High Feedrate for retract
+    max_E_feedrate_calc = MAX_RETRACT_FEEDRATE;
+    retract_feedrate_aktiv = true;
+  }
+  else
+  {
+     if(retract_feedrate_aktiv)
+     {
+       if(block->steps_e > 0)
+         retract_feedrate_aktiv = false;
+     }
+     else
+     {
+       max_E_feedrate_calc = max_feedrate[E_AXIS]; 
+     }
+  }
   
 
  #ifdef DELAY_ENABLE
@@ -2311,12 +2583,12 @@ void plan_buffer_line(float x, float y, float z, float e, float feed_rate)
     enable_y();
     delayMicroseconds(DELAY_ENABLE);
   }
-  if(if(block->steps_z != 0))
+  if(block->steps_z != 0)
   {
     enable_z();
     delayMicroseconds(DELAY_ENABLE);
   }
-  if(if(block->steps_e != 0))
+  if(block->steps_e != 0)
   {
     enable_e();
     delayMicroseconds(DELAY_ENABLE);
@@ -2336,7 +2608,7 @@ void plan_buffer_line(float x, float y, float z, float e, float feed_rate)
     	if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;
   } 
 
-  // slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill
+  // slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
   int moves_queued=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1);
 #ifdef SLOWDOWN  
   if(moves_queued < (BLOCK_BUFFER_SIZE * 0.5) && moves_queued > 1) feed_rate = feed_rate*moves_queued / (BLOCK_BUFFER_SIZE * 0.5); 
@@ -2346,7 +2618,8 @@ void plan_buffer_line(float x, float y, float z, float e, float feed_rate)
   delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
   delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
   delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS];
-  delta_mm[E_AXIS] = (target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS];
+  //delta_mm[E_AXIS] = (target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS];
+  delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS])*extrudemultiply/100.0;
   
   if ( block->steps_x <= dropsegments && block->steps_y <= dropsegments && block->steps_z <= dropsegments ) {
     block->millimeters = fabs(delta_mm[E_AXIS]);
@@ -2381,32 +2654,24 @@ void plan_buffer_line(float x, float y, float z, float e, float feed_rate)
 */
 
 
- // Calculate speed in mm/sec for each axis
+ // Calculate and limit speed in mm/sec for each axis
   float current_speed[4];
-  for(int i=0; i < 4; i++) {
-    current_speed[i] = delta_mm[i] * inverse_second;
-  }
-
-  // Limit speed per axis
   float speed_factor = 1.0; //factor <=1 do decrease speed
-  for(int i=0; i < 4; i++) {
+  for(int i=0; i < 3; i++) 
+  {
+    current_speed[i] = delta_mm[i] * inverse_second;
     if(fabs(current_speed[i]) > max_feedrate[i])
       speed_factor = min(speed_factor, max_feedrate[i] / fabs(current_speed[i]));
   }
+  
+  current_speed[E_AXIS] = delta_mm[E_AXIS] * inverse_second;
+  if(fabs(current_speed[E_AXIS]) > max_E_feedrate_calc)
+    speed_factor = min(speed_factor, max_E_feedrate_calc / fabs(current_speed[E_AXIS]));
+
 
   // Correct the speed  
-  if( speed_factor < 1.0) {
-//    Serial.print("speed factor : "); Serial.println(speed_factor);
-    for(int i=0; i < 4; i++) {
-    if(fabs(current_speed[i]) > max_feedrate[i])
-      speed_factor = min(speed_factor, max_feedrate[i] / fabs(current_speed[i]));
- /*     
-      if(speed_factor < 0.1) {
-        Serial.print("speed factor : "); Serial.println(speed_factor);
-        Serial.print("current_speed"); Serial.print(i); Serial.print(" : "); Serial.println(current_speed[i]);
-      }
- */
-  }
+  if( speed_factor < 1.0) 
+  {
     for(unsigned char i=0; i < 4; i++) {
       current_speed[i] *= speed_factor;
     }
@@ -2475,25 +2740,42 @@ void plan_buffer_line(float x, float y, float z, float e, float feed_rate)
   }
 #endif
   // Start with a safe speed
-  float vmax_junction = max_xy_jerk/2;  
+  float vmax_junction = max_xy_jerk/2; 
+  float vmax_junction_factor = 1.0; 
+
   if(fabs(current_speed[Z_AXIS]) > max_z_jerk/2) 
-    vmax_junction = max_z_jerk/2;
+    vmax_junction = min(vmax_junction, max_z_jerk/2);
+
+  if(fabs(current_speed[E_AXIS]) > max_e_jerk/2) 
+    vmax_junction = min(vmax_junction, max_e_jerk/2);
+
+  if(G92_reset_previous_speed == 1)
+  {
+    vmax_junction = 0.1;
+    G92_reset_previous_speed = 0;  
+  }
+
   vmax_junction = min(vmax_junction, block->nominal_speed);
+  float safe_speed = vmax_junction;
 
-  if ((moves_queued > 1) && (previous_nominal_speed > 0.0)) {
+  if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
     float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2));
-    if((previous_speed[X_AXIS] != 0.0) || (previous_speed[Y_AXIS] != 0.0)) {
-      vmax_junction = block->nominal_speed;
-    }
+    //    if((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
+    vmax_junction = block->nominal_speed;
+    //    }
     if (jerk > max_xy_jerk) {
-      vmax_junction *= (max_xy_jerk/jerk);
+      vmax_junction_factor = (max_xy_jerk/jerk);
     } 
     if(fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk) {
-      vmax_junction *= (max_z_jerk/fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]));
+      vmax_junction_factor= min(vmax_junction_factor, (max_z_jerk/fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS])));
     } 
+    if(fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]) > max_e_jerk) {
+      vmax_junction_factor = min(vmax_junction_factor, (max_e_jerk/fabs(current_speed[E_AXIS] - previous_speed[E_AXIS])));
+    } 
+    vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
   }
   block->max_entry_speed = vmax_junction;
-    
+
   // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
   double v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters);
   block->entry_speed = min(vmax_junction, v_allowable);
@@ -2506,10 +2788,14 @@ void plan_buffer_line(float x, float y, float z, float e, float feed_rate)
   // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
   // the reverse and forward planners, the corresponding block junction speed will always be at the
   // the maximum junction speed and may always be ignored for any speed reduction checks.
-  if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; }
-  else { block->nominal_length_flag = false; }
+  if (block->nominal_speed <= v_allowable) { 
+    block->nominal_length_flag = true; 
+  }
+  else { 
+    block->nominal_length_flag = false; 
+  }
   block->recalculate_flag = true; // Always calculate trapezoid for new block
-  
+
   // Update previous path unit_vector and nominal speed
   memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[]
   previous_nominal_speed = block->nominal_speed;
@@ -2536,10 +2822,8 @@ void plan_buffer_line(float x, float y, float z, float e, float feed_rate)
   #endif // ADVANCE
 
 
-
-
   calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed,
-    MINIMUM_PLANNER_SPEED/block->nominal_speed);
+    safe_speed/block->nominal_speed);
     
   // Move buffer head
   block_buffer_head = next_buffer_head;
@@ -2554,6 +2838,12 @@ void plan_buffer_line(float x, float y, float z, float e, float feed_rate)
   st_wake_up();
 }
 
+int calc_plannerpuffer_fill(void)
+{
+  int moves_queued=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1);
+  return(moves_queued);
+}
+
 void plan_set_position(float x, float y, float z, float e)
 {
   position[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
@@ -2561,11 +2851,17 @@ void plan_set_position(float x, float y, float z, float e)
   position[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);     
   position[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);  
 
+  virtual_steps_x = 0;
+  virtual_steps_y = 0;
+  virtual_steps_z = 0;
+
   previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
   previous_speed[0] = 0.0;
   previous_speed[1] = 0.0;
   previous_speed[2] = 0.0;
   previous_speed[3] = 0.0;
+  
+  G92_reset_previous_speed = 1;
 }
 
 #ifdef AUTOTEMP
@@ -2577,16 +2873,19 @@ void getHighESpeed()
   if((target_temp+2) < autotemp_min)  //probably temperature set to zero.
     return; //do nothing
   
-  float high=0;
+  float high=0.0;
   uint8_t block_index = block_buffer_tail;
   
-  while(block_index != block_buffer_head)
-  {
-    float se=block_buffer[block_index].steps_e/float(block_buffer[block_index].step_event_count)*block_buffer[block_index].nominal_rate;
-    //se; units steps/sec;
-    if(se>high)
-    {
-      high=se;
+  while(block_index != block_buffer_head) {
+    if((block_buffer[block_index].steps_x != 0) ||
+       (block_buffer[block_index].steps_y != 0) ||
+       (block_buffer[block_index].steps_z != 0)) {
+      float se=(float(block_buffer[block_index].steps_e)/float(block_buffer[block_index].step_event_count))*block_buffer[block_index].nominal_speed;
+      //se; units steps/sec;
+      if(se>high)
+      {
+        high=se;
+      }
     }
     block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1);
   }
@@ -2611,7 +2910,6 @@ void getHighESpeed()
 
 
 
-
 // Stepper
 
 // intRes = intIn1 * intIn2 >> 16
@@ -2711,7 +3009,7 @@ static unsigned long step_events_completed; // The number of step events execute
 static short e_steps;
 static unsigned char busy = false; // TRUE when SIG_OUTPUT_COMPARE1A is being serviced. Used to avoid retriggering that handler.
 static long acceleration_time, deceleration_time;
-static unsigned short acc_step_rate; // needed for deccelaration start point
+static unsigned short acc_step_rate; // needed for deceleration start point
 static char step_loops;
 static unsigned short OCR1A_nominal;
 
@@ -2739,7 +3037,7 @@ static bool old_z_max_endstop=false;
 //
 //                           time ----->
 // 
-//  The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates 
+//  The trapezoid is the shape of the speed curve over time. It starts at block->initial_rate, accelerates 
 //  first block->accelerate_until step_events_completed, then keeps going at constant speed until 
 //  step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset.
 //  The slope of acceleration is calculated with the leib ramp alghorithm.
@@ -2840,7 +3138,7 @@ ISR(TIMER1_COMPA_vect)
     // Set directions TO DO This should be done once during init of trapezoid. Endstops -> interrupt
     out_bits = current_block->direction_bits;
 
-    // Set direction en check limit switches
+    // Set direction and check limit switches
     if ((out_bits & (1<<X_AXIS)) != 0) {   // -direction
       WRITE(X_DIR_PIN, INVERT_X_DIR);
       CHECK_ENDSTOPS
@@ -2848,10 +3146,18 @@ ISR(TIMER1_COMPA_vect)
         #if X_MIN_PIN > -1
           bool x_min_endstop=(READ(X_MIN_PIN) != X_ENDSTOP_INVERT);
           if(x_min_endstop && old_x_min_endstop && (current_block->steps_x > 0)) {
-            endstop_x_hit=true;
-            step_events_completed = current_block->step_event_count;
+            if(!is_homing)
+              endstop_x_hit=true;
+            else  
+              step_events_completed = current_block->step_event_count;
+          }
+          else
+          {
+            endstop_x_hit=false;
           }
           old_x_min_endstop = x_min_endstop;
+        #else
+          endstop_x_hit=false;
         #endif
       }
     }
@@ -2862,10 +3168,18 @@ ISR(TIMER1_COMPA_vect)
         #if X_MAX_PIN > -1
           bool x_max_endstop=(READ(X_MAX_PIN) != X_ENDSTOP_INVERT);
           if(x_max_endstop && old_x_max_endstop && (current_block->steps_x > 0)){
-            endstop_x_hit=true;
-            step_events_completed = current_block->step_event_count;
+            if(!is_homing)
+              endstop_x_hit=true;
+            else    
+              step_events_completed = current_block->step_event_count;
+          }
+          else
+          {
+            endstop_x_hit=false;
           }
           old_x_max_endstop = x_max_endstop;
+        #else
+          endstop_x_hit=false;
         #endif
       }
     }
@@ -2877,10 +3191,18 @@ ISR(TIMER1_COMPA_vect)
         #if Y_MIN_PIN > -1
           bool y_min_endstop=(READ(Y_MIN_PIN) != Y_ENDSTOP_INVERT);
           if(y_min_endstop && old_y_min_endstop && (current_block->steps_y > 0)) {
-            endstop_y_hit=true;
-            step_events_completed = current_block->step_event_count;
+            if(!is_homing)
+              endstop_y_hit=true;
+            else
+              step_events_completed = current_block->step_event_count;
+          }
+          else
+          {
+            endstop_y_hit=false;
           }
           old_y_min_endstop = y_min_endstop;
+        #else
+          endstop_y_hit=false;  
         #endif
       }
     }
@@ -2891,10 +3213,18 @@ ISR(TIMER1_COMPA_vect)
         #if Y_MAX_PIN > -1
           bool y_max_endstop=(READ(Y_MAX_PIN) != Y_ENDSTOP_INVERT);
           if(y_max_endstop && old_y_max_endstop && (current_block->steps_y > 0)){
-            endstop_y_hit=true;
-            step_events_completed = current_block->step_event_count;
+            if(!is_homing)
+              endstop_y_hit=true;
+            else  
+              step_events_completed = current_block->step_event_count;
+          }
+          else
+          {
+            endstop_y_hit=false;
           }
           old_y_max_endstop = y_max_endstop;
+        #else
+          endstop_y_hit=false;  
         #endif
       }
     }
@@ -2906,10 +3236,18 @@ ISR(TIMER1_COMPA_vect)
         #if Z_MIN_PIN > -1
           bool z_min_endstop=(READ(Z_MIN_PIN) != Z_ENDSTOP_INVERT);
           if(z_min_endstop && old_z_min_endstop && (current_block->steps_z > 0)) {
-            endstop_z_hit=true;
-            step_events_completed = current_block->step_event_count;
+            if(!is_homing)  
+              endstop_z_hit=true;
+            else  
+              step_events_completed = current_block->step_event_count;
+          }
+          else
+          {
+            endstop_z_hit=false;
           }
           old_z_min_endstop = z_min_endstop;
+        #else
+          endstop_z_hit=false;  
         #endif
       }
     }
@@ -2920,10 +3258,18 @@ ISR(TIMER1_COMPA_vect)
         #if Z_MAX_PIN > -1
           bool z_max_endstop=(READ(Z_MAX_PIN) != Z_ENDSTOP_INVERT);
           if(z_max_endstop && old_z_max_endstop && (current_block->steps_z > 0)) {
-            endstop_z_hit=true;
-            step_events_completed = current_block->step_event_count;
+            if(!is_homing)
+              endstop_z_hit=true;
+            else  
+              step_events_completed = current_block->step_event_count;
+          }
+          else
+          {
+            endstop_z_hit=false;
           }
           old_z_max_endstop = z_max_endstop;
+        #else
+          endstop_z_hit=false;  
         #endif
       }
     }
@@ -2953,24 +3299,52 @@ ISR(TIMER1_COMPA_vect)
         }
       }    
       #endif //ADVANCE
-      
+
+
       counter_x += current_block->steps_x;
       if (counter_x > 0) {
-        WRITE(X_STEP_PIN, HIGH);
+        if(!endstop_x_hit)
+        {
+          if(virtual_steps_x)
+            virtual_steps_x--;
+          else
+            WRITE(X_STEP_PIN, HIGH);
+        }
+        else
+          virtual_steps_x++;
+          
         counter_x -= current_block->step_event_count;
         WRITE(X_STEP_PIN, LOW);
       }
 
       counter_y += current_block->steps_y;
       if (counter_y > 0) {
-        WRITE(Y_STEP_PIN, HIGH);
+        if(!endstop_y_hit)
+        {
+          if(virtual_steps_y)
+            virtual_steps_y--;
+          else
+            WRITE(Y_STEP_PIN, HIGH);
+        }
+        else
+          virtual_steps_y++;
+            
         counter_y -= current_block->step_event_count;
         WRITE(Y_STEP_PIN, LOW);
       }
 
       counter_z += current_block->steps_z;
       if (counter_z > 0) {
-        WRITE(Z_STEP_PIN, HIGH);
+        if(!endstop_z_hit)
+        {
+          if(virtual_steps_z)
+            virtual_steps_z--;
+          else
+            WRITE(Z_STEP_PIN, HIGH);
+        }
+        else
+          virtual_steps_z++;
+          
         counter_z -= current_block->step_event_count;
         WRITE(Z_STEP_PIN, LOW);
       }
@@ -2983,8 +3357,10 @@ ISR(TIMER1_COMPA_vect)
           WRITE(E_STEP_PIN, LOW);
         }
       #endif //!ADVANCE
+
       step_events_completed += 1;  
       if(step_events_completed >= current_block->step_event_count) break;
+      
     }
     // Calculare new timer value
     unsigned short timer;
@@ -3130,6 +3506,9 @@ void st_synchronize()
   while(blocks_queued()) {
     manage_heater();
     manage_inactivity(1);
+    #if (MINIMUM_FAN_START_SPEED > 0)
+      manage_fan_start_speed();
+    #endif
   }   
 }
 
diff --git a/Sprinter/arc_func.cpp b/Sprinter/arc_func.cpp
index 255129f..1446e30 100644
--- a/Sprinter/arc_func.cpp
+++ b/Sprinter/arc_func.cpp
@@ -50,6 +50,8 @@ void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8
   float millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));
   if (millimeters_of_travel < 0.001) { return; }
   uint16_t segments = floor(millimeters_of_travel/MM_PER_ARC_SEGMENT);
+  if(segments == 0) segments = 1;
+
   /*  
     // Multiply inverse feed_rate to compensate for the fact that this movement is approximated
     // by a number of discrete segments. The inverse feed_rate should be correct for the sum of 
diff --git a/Sprinter/heater.cpp b/Sprinter/heater.cpp
index c02924c..027d057 100644
--- a/Sprinter/heater.cpp
+++ b/Sprinter/heater.cpp
@@ -32,6 +32,10 @@
   void controllerFan(void);
 #endif
 
+#ifdef EXTRUDERFAN_PIN
+  void extruderFan(void);
+#endif
+
 // Manage heater variables. For a thermistor or AD595 thermocouple, raw values refer to the 
 // reading from the analog pin. For a MAX6675 thermocouple, the raw value is the temperature in 0.25 
 // degree increments (i.e. 100=25 deg). 
@@ -50,8 +54,8 @@ unsigned long previous_millis_heater, previous_millis_bed_heater, previous_milli
 #ifdef PIDTEMP
   volatile unsigned char g_heater_pwm_val = 0;
  
-  unsigned char PWM_off_time = 0;
-  unsigned char PWM_out_on = 0;
+  //unsigned char PWM_off_time = 0;
+  //unsigned char PWM_out_on = 0;
   
   int temp_iState = 0;
   int temp_dState = 0;
@@ -62,11 +66,15 @@ unsigned long previous_millis_heater, previous_millis_bed_heater, previous_milli
       //int output;
   int error;
   int heater_duty = 0;
-  const int temp_iState_min = 256L * -PID_INTEGRAL_DRIVE_MAX / PID_IGAIN;
-  const int temp_iState_max = 256L * PID_INTEGRAL_DRIVE_MAX / PID_IGAIN;
+  int temp_iState_min = 256L * -PID_INTEGRAL_DRIVE_MAX / PID_IGAIN;
+  int temp_iState_max = 256L * PID_INTEGRAL_DRIVE_MAX / PID_IGAIN;
 #endif
 
 
+#if defined(FAN_SOFT_PWM) && (FAN_PIN > -1)
+  volatile unsigned char g_fan_pwm_val = 0;
+#endif
+
 #ifdef AUTOTEMP
     float autotemp_max=AUTO_TEMP_MAX;
     float autotemp_min=AUTO_TEMP_MIN;
@@ -155,90 +163,348 @@ int read_max6675()
 #endif
 
 
-#ifdef PID_SOFT_PWM
+//------------------------------------------------------------------------
+// Soft PWM for Heater and FAN
+//------------------------------------------------------------------------
 
+#if defined(PID_SOFT_PWM) || (defined(FAN_SOFT_PWM) && (FAN_PIN > -1))
  void init_Timer2_softpwm(void)
  {
   // This is a simple SOFT PWM with 500 Hz for Extruder Heating
 
- 
   TIFR2 = (1 << TOV2);          // clear interrupt flag
   TCCR2B = (1 << CS22) | (1 << CS20);         // start timer (ck/128 prescalar)
-  TCCR2A = (1 << WGM21);        // CTC mode
-  OCR2A = 128;                  // We want to have at least 30Hz or else it gets choppy
-  TIMSK2 = (1 << OCIE2A);       // enable timer2 output compare match interrupt
-
+  TCCR2A = 0;//(1 << WGM21);        // Normal mode
+  
+   TIMSK2 |= (1 << TOIE2);
+  
+  #ifdef PID_SOFT_PWM
+   OCR2A = 128;                  // We want to have at least 500Hz or else it gets choppy
+   TIMSK2 |= (1 << OCIE2A);       // enable timer2 output compare match interrupt
+  #endif
+  
+  #if defined(FAN_SOFT_PWM) && (FAN_PIN > -1)
+   OCR2B = 128;                  // We want to have at least 500Hz or else it gets choppy
+   TIMSK2 |= (1 << OCIE2B);       // enable timer2 output compare match interrupt
+  #endif
+ 
  }
+#endif
 
+#if defined(PID_SOFT_PWM) || (defined(FAN_SOFT_PWM) && (FAN_PIN > -1))
+ISR(TIMER2_OVF_vect)
+{
+  
+  //--------------------------------------
+  // Soft PWM, Heater, start PWM cycle
+  //--------------------------------------
+  #ifdef PID_SOFT_PWM
+    if(g_heater_pwm_val >= 2)
+    {
+      #if LED_PIN > -1
+        WRITE(LED_PIN,HIGH);
+      #endif
+      WRITE(HEATER_0_PIN,HIGH);
 
- ISR(TIMER2_COMPA_vect)
- {
-
-    
-    if(g_heater_pwm_val < 2)
+      if(g_heater_pwm_val <= 253)
+        OCR2A = g_heater_pwm_val; 
+      else
+        OCR2A = 192; 
+    }
+    else
     {
       #if LED_PIN > -1
         WRITE(LED_PIN,LOW);
       #endif
       WRITE(HEATER_0_PIN,LOW);
-      PWM_out_on = 0;
-      OCR2A = 128; 
+      OCR2A = 192; 
     }
-    else if(g_heater_pwm_val > 253)
+  #endif
+  
+  //--------------------------------------
+  // Soft PWM, Fan, start PWM cycle
+  //--------------------------------------
+  #if defined(FAN_SOFT_PWM) && (FAN_PIN > -1)
+    if(g_fan_pwm_val >= 2)
     {
-      #if LED_PIN > -1
-        WRITE(LED_PIN,HIGH);
-      #endif
-      WRITE(HEATER_0_PIN,HIGH);
-      PWM_out_on = 1;
-      OCR2A = 128; 
+      #if (FAN_PIN > -1) 
+        WRITE(FAN_PIN,HIGH);
+      #endif  
+      
+      if(g_fan_pwm_val <= 253)
+        OCR2B = g_fan_pwm_val; 
+      else
+        OCR2B = 128; 
     }
     else
     {
-
-       if(PWM_out_on == 1)
-       {
-
-         #if LED_PIN > -1
-           WRITE(LED_PIN,LOW);
-         #endif
-         WRITE(HEATER_0_PIN,LOW);
-         PWM_out_on = 0;
-         OCR2A = PWM_off_time;
-       }
-       else
-       {
-
-         #if LED_PIN > -1
-           WRITE(LED_PIN,HIGH);
-         #endif
-         WRITE(HEATER_0_PIN,HIGH);
-         PWM_out_on = 1;
-         
-         if(g_heater_pwm_val > 253)
-         {
-           OCR2A = 253;
-           PWM_off_time = 2;
-         }
-         else if(g_heater_pwm_val < 2)
-         {
-           OCR2A = 2;
-           PWM_off_time = 253;
-         }
-         else
-         {
-           OCR2A =  g_heater_pwm_val;
-           PWM_off_time = 255 - g_heater_pwm_val;
-         }
-         
-       }
+      #if (FAN_PIN > -1) 
+        WRITE(FAN_PIN,LOW);
+      #endif  
+      
+      OCR2B = 128; 
     }
-    
+  #endif
   
+
+}
+#endif
+
+
+ #ifdef PID_SOFT_PWM
+ ISR(TIMER2_COMPA_vect)
+ {
+
+
+   if(g_heater_pwm_val > 253)
+   {
+     #if LED_PIN > -1
+       WRITE(LED_PIN,HIGH);
+     #endif
+     WRITE(HEATER_0_PIN,HIGH);
+   }
+   else
+   {
+     #if LED_PIN > -1
+       WRITE(LED_PIN,LOW);
+     #endif
+     WRITE(HEATER_0_PIN,LOW);
+   }
+   
+
  }
  #endif
  
+
+ #if defined(FAN_SOFT_PWM) && (FAN_PIN > -1)
+ ISR(TIMER2_COMPB_vect)
+ {
+
+   
+   if(g_fan_pwm_val > 253)
+   {
+     #if (FAN_PIN > -1) 
+       WRITE(FAN_PIN,HIGH);
+     #endif
+   }
+   else
+   {
+     #if (FAN_PIN > -1) 
+       WRITE(FAN_PIN,LOW);
+     #endif
+   }  
+
+
+ }  
+ #endif
+ //--------------------END SOFT PWM---------------------------
+
+//-------------------- START PID AUTOTUNE ---------------------------
+// Based on PID relay test 
+// Thanks to Erik van der Zalm for this idea to use it for Marlin
+// Some information see:
+// http://brettbeauregard.com/blog/2012/01/arduino-pid-autotune-library/
+//------------------------------------------------------------------
+#ifdef PID_AUTOTUNE
+void PID_autotune(int PIDAT_test_temp)
+{
+  float PIDAT_input = 0;
+  int PIDAT_input_help = 0;
+  unsigned char PIDAT_count_input = 0;
+
+  float PIDAT_max, PIDAT_min;
+ 
+  unsigned char PIDAT_PWM_val = HEATER_CURRENT;
+  
+  unsigned char PIDAT_cycles=0;
+  bool PIDAT_heating = true;
+
+  unsigned long PIDAT_temp_millis = millis();
+  unsigned long PIDAT_t1=PIDAT_temp_millis;
+  unsigned long PIDAT_t2=PIDAT_temp_millis;
+  unsigned long PIDAT_T_check_AI_val = PIDAT_temp_millis;
+
+  unsigned char PIDAT_cycle_cnt = 0;
+  
+  long PIDAT_t_high;
+  long PIDAT_t_low;
+
+  long PIDAT_bias= HEATER_CURRENT/2;
+  long PIDAT_d  =  HEATER_CURRENT/2;
+  
+  float PIDAT_Ku, PIDAT_Tu;
+  float PIDAT_Kp, PIDAT_Ki, PIDAT_Kd;
+  
+  #define PIDAT_TIME_FACTOR ((HEATER_CHECK_INTERVAL*256.0) / 1000.0)
+  
+  showString(PSTR("PID Autotune start\r\n"));
+
+  target_temp = PIDAT_test_temp;
+  
+  #ifdef BED_USES_THERMISTOR
+   WRITE(HEATER_1_PIN,LOW);
+  #endif
+  
+  for(;;) 
+  {
  
+    if((millis() - PIDAT_T_check_AI_val) > 100 )
+    {
+      PIDAT_T_check_AI_val = millis();
+      PIDAT_cycle_cnt++;
+      
+      #ifdef HEATER_USES_THERMISTOR
+        current_raw = analogRead(TEMP_0_PIN); 
+        current_raw = 1023 - current_raw;
+        PIDAT_input_help += analog2temp(current_raw);
+        PIDAT_count_input++;
+      #elif defined HEATER_USES_AD595
+        current_raw = analogRead(TEMP_0_PIN);    
+        PIDAT_input_help += analog2temp(current_raw);
+        PIDAT_count_input++;
+      #elif defined HEATER_USES_MAX6675
+        current_raw = read_max6675();
+        PIDAT_input_help += analog2temp(current_raw);
+        PIDAT_count_input++;
+      #endif
+    }
+    
+    if(PIDAT_cycle_cnt >= 10 )
+    {
+      
+      PIDAT_cycle_cnt = 0;
+      
+      PIDAT_input = (float)PIDAT_input_help / (float)PIDAT_count_input;
+      PIDAT_input_help = 0;
+      PIDAT_count_input = 0;
+      
+      PIDAT_max=max(PIDAT_max,PIDAT_input);
+      PIDAT_min=min(PIDAT_min,PIDAT_input);
+      
+      if(PIDAT_heating == true && PIDAT_input > PIDAT_test_temp) 
+      {
+        if(millis() - PIDAT_t2 > 5000) 
+        { 
+          PIDAT_heating = false;
+          PIDAT_PWM_val = (PIDAT_bias - PIDAT_d);
+          PIDAT_t1 = millis();
+          PIDAT_t_high = PIDAT_t1 - PIDAT_t2;
+          PIDAT_max = PIDAT_test_temp;
+        }
+      }
+      
+      if(PIDAT_heating == false && PIDAT_input < PIDAT_test_temp) 
+      {
+        if(millis() - PIDAT_t1 > 5000) 
+        {
+          PIDAT_heating = true;
+          PIDAT_t2 = millis();
+          PIDAT_t_low = PIDAT_t2 - PIDAT_t1;
+          
+          if(PIDAT_cycles > 0) 
+          {
+            PIDAT_bias += (PIDAT_d*(PIDAT_t_high - PIDAT_t_low))/(PIDAT_t_low + PIDAT_t_high);
+            PIDAT_bias = constrain(PIDAT_bias, 20 ,HEATER_CURRENT - 20);
+            if(PIDAT_bias > (HEATER_CURRENT/2)) PIDAT_d = (HEATER_CURRENT - 1) - PIDAT_bias;
+            else PIDAT_d = PIDAT_bias;
+
+            showString(PSTR(" bias: ")); Serial.print(PIDAT_bias);
+            showString(PSTR(" d: "));    Serial.print(PIDAT_d);
+            showString(PSTR(" min: "));  Serial.print(PIDAT_min);
+            showString(PSTR(" max: "));  Serial.println(PIDAT_max);
+            
+            if(PIDAT_cycles > 2) 
+            {
+              PIDAT_Ku = (4.0*PIDAT_d)/(3.14159*(PIDAT_max-PIDAT_min));
+              PIDAT_Tu = ((float)(PIDAT_t_low + PIDAT_t_high)/1000.0);
+              
+              showString(PSTR(" Ku: ")); Serial.print(PIDAT_Ku);
+              showString(PSTR(" Tu: ")); Serial.println(PIDAT_Tu);
+
+              PIDAT_Kp = 0.60*PIDAT_Ku;
+              PIDAT_Ki = 2*PIDAT_Kp/PIDAT_Tu;
+              PIDAT_Kd = PIDAT_Kp*PIDAT_Tu/8;
+              showString(PSTR(" Clasic PID \r\n"));
+              //showString(PSTR(" Kp: ")); Serial.println(PIDAT_Kp);
+              //showString(PSTR(" Ki: ")); Serial.println(PIDAT_Ki);
+              //showString(PSTR(" Kd: ")); Serial.println(PIDAT_Kd);
+              showString(PSTR(" CFG Kp: ")); Serial.println((unsigned int)(PIDAT_Kp*256));
+              showString(PSTR(" CFG Ki: ")); Serial.println((unsigned int)(PIDAT_Ki*PIDAT_TIME_FACTOR));
+              showString(PSTR(" CFG Kd: ")); Serial.println((unsigned int)(PIDAT_Kd*PIDAT_TIME_FACTOR));
+              
+              PIDAT_Kp = 0.30*PIDAT_Ku;
+              PIDAT_Ki = PIDAT_Kp/PIDAT_Tu;
+              PIDAT_Kd = PIDAT_Kp*PIDAT_Tu/3;
+              showString(PSTR(" Some overshoot \r\n"));
+              showString(PSTR(" CFG Kp: ")); Serial.println((unsigned int)(PIDAT_Kp*256));
+              showString(PSTR(" CFG Ki: ")); Serial.println((unsigned int)(PIDAT_Ki*PIDAT_TIME_FACTOR));
+              showString(PSTR(" CFG Kd: ")); Serial.println((unsigned int)(PIDAT_Kd*PIDAT_TIME_FACTOR));
+              /*
+              PIDAT_Kp = 0.20*PIDAT_Ku;
+              PIDAT_Ki = 2*PIDAT_Kp/PIDAT_Tu;
+              PIDAT_Kd = PIDAT_Kp*PIDAT_Tu/3;
+              showString(PSTR(" No overshoot \r\n"));
+              showString(PSTR(" CFG Kp: ")); Serial.println((unsigned int)(PIDAT_Kp*256));
+              showString(PSTR(" CFG Ki: ")); Serial.println((unsigned int)(PIDAT_Ki*PIDAT_TIME_FACTOR));
+              showString(PSTR(" CFG Kd: ")); Serial.println((unsigned int)(PIDAT_Kd*PIDAT_TIME_FACTOR));
+              */
+            }
+          }
+          PIDAT_PWM_val = (PIDAT_bias + PIDAT_d);
+          PIDAT_cycles++;
+          PIDAT_min = PIDAT_test_temp;
+        }
+      } 
+      
+      #ifdef PID_SOFT_PWM
+        g_heater_pwm_val = PIDAT_PWM_val;
+      #else
+        analogWrite_check(HEATER_0_PIN, PIDAT_PWM_val);
+        #if LED_PIN>-1
+          analogWrite_check(LED_PIN, PIDAT_PWM_val);
+        #endif
+      #endif  
+    }
+    
+    if((PIDAT_input > (PIDAT_test_temp + 55)) || (PIDAT_input > 255))
+    {
+      showString(PSTR("PID Autotune failed! Temperature to high\r\n"));
+      target_temp = 0;
+      return;
+    }
+    
+    if(millis() - PIDAT_temp_millis > 2000) 
+    {
+      PIDAT_temp_millis = millis();
+      showString(PSTR("ok T:"));
+      Serial.print(PIDAT_input);   
+      showString(PSTR(" @:"));
+      Serial.println((unsigned char)PIDAT_PWM_val*1);       
+    }
+    
+    if(((millis() - PIDAT_t1) + (millis() - PIDAT_t2)) > (10L*60L*1000L*2L)) 
+    {
+      showString(PSTR("PID Autotune failed! timeout\r\n"));
+      return;
+    }
+    
+    if(PIDAT_cycles > 5) 
+    {
+      showString(PSTR("PID Autotune finished ! Place the Kp, Ki and Kd constants in the configuration.h\r\n"));
+      return;
+    }
+  }
+}
+#endif  
+//---------------- END AUTOTUNE PID ------------------------------
+
+ void updatePID()
+ {
+   #ifdef PIDTEMP
+    temp_iState_min = (256L * -PID_INTEGRAL_DRIVE_MAX) / PID_Ki;
+    temp_iState_max = (256L * PID_INTEGRAL_DRIVE_MAX) / PID_Ki;
+   #endif
+ }
  
  void manage_heater()
  {
@@ -247,6 +513,8 @@ int read_max6675()
   if((millis() - previous_millis_monitor) > 250 )
   {
     previous_millis_monitor = millis();
+
+
     if(manage_monitor <= 1)
     {
       showString(PSTR("MTEMP:"));
@@ -378,7 +646,7 @@ int read_max6675()
       int delta_temp = current_temp - prev_temp;
       
       prev_temp = current_temp;
-      pTerm = ((long)PID_PGAIN * error) / 256;
+      pTerm = ((long)PID_Kp * error) / 256;
       const int H0 = min(HEATER_DUTY_FOR_SETPOINT(target_temp),HEATER_CURRENT);
       heater_duty = H0 + pTerm;
       
@@ -386,7 +654,7 @@ int read_max6675()
       {
         temp_iState += error;
         temp_iState = constrain(temp_iState, temp_iState_min, temp_iState_max);
-        iTerm = ((long)PID_IGAIN * temp_iState) / 256;
+        iTerm = ((long)PID_Ki * temp_iState) / 256;
         heater_duty += iTerm;
       }
       
@@ -397,7 +665,7 @@ int read_max6675()
       if(prev_error >  9){ prev_error /=  9; log3 += 2; }
       if(prev_error >  3){ prev_error /=  3; log3 ++;   }
       
-      dTerm = ((long)PID_DGAIN * delta_temp) / (256*log3);
+      dTerm = ((long)PID_Kd * delta_temp) / (256*log3);
       heater_duty += dTerm;
       heater_duty = constrain(heater_duty, 0, HEATER_CURRENT);
 
@@ -491,6 +759,10 @@ int read_max6675()
   controllerFan(); //Check if fan should be turned on to cool stepper drivers down
 #endif
 
+#ifdef EXTRUDERFAN_PIN
+  extruderFan(); //Check if fan should be turned on to cool extruder down
+#endif
+
 }
 
 #if defined (HEATER_USES_THERMISTOR) || defined (BED_USES_THERMISTOR)
@@ -601,3 +873,24 @@ void controllerFan()
 }
 #endif
 
+#ifdef EXTRUDERFAN_PIN
+unsigned long lastExtruderCheck = 0;
+
+void extruderFan()
+{
+  if ((millis() - lastExtruderCheck) >= 2500) //Not a time critical function, so we only check every 2500ms
+  {
+    lastExtruderCheck = millis();
+           
+    if (analog2temp(current_raw) < EXTRUDERFAN_DEC)
+    {
+      WRITE(EXTRUDERFAN_PIN, LOW); //... turn the fan off
+    }
+    else
+    {
+      WRITE(EXTRUDERFAN_PIN, HIGH); //... turn the fan on
+    }
+  }
+}
+#endif
+
diff --git a/Sprinter/heater.h b/Sprinter/heater.h
index a1d2917..33252a1 100644
--- a/Sprinter/heater.h
+++ b/Sprinter/heater.h
@@ -88,8 +88,13 @@ extern unsigned char manage_monitor;
   extern int dTerm;
   extern int error;
   extern int heater_duty;
+  
+  extern unsigned int PID_Kp, PID_Ki, PID_Kd;
 #endif
 
+#if defined(FAN_SOFT_PWM) && (FAN_PIN > -1)
+  extern volatile unsigned char g_fan_pwm_val;
+#endif
 
 #ifdef AUTOTEMP
     extern float autotemp_max;
@@ -112,8 +117,16 @@ extern unsigned char manage_monitor;
 
 
 
-#ifdef PID_SOFT_PWM
+#if defined(PID_SOFT_PWM) || (defined(FAN_SOFT_PWM) && (FAN_PIN > -1))
  void init_Timer2_softpwm(void);
 #endif
 
+#ifdef PID_AUTOTUNE
+ void PID_autotune(int PIDAT_test_temp);
+#endif
+
+#ifdef PIDTEMP
+ void updatePID();
+#endif
+
 void manage_heater();
diff --git a/Sprinter/pins.h b/Sprinter/pins.h
index afe99bc..e2d2f89 100644
--- a/Sprinter/pins.h
+++ b/Sprinter/pins.h
@@ -181,7 +181,7 @@
 #if MOTHERBOARD == 2
 #define KNOWN_BOARD 1
 
-#if !defined(__AVR_ATmega644P__) && !defined(__AVR_ATmega1284P__) 
+#if !defined(__AVR_ATmega644P__) && !defined(__AVR_ATmega1284P__) && !defined(__ATmega644P__) && !defined(__ATmega1284P__)  
 #error Oops!  Make sure you have 'Sanguino' selected from the 'Tools -> Boards' menu.
 #endif
 
@@ -598,6 +598,11 @@
 * Gen6 pin assignment
 *
 ****************************************************************************************/
+#if MOTHERBOARD == 51
+  #define MOTHERBOARD 5
+  #define GEN6_DELUXE
+#endif
+
 #if MOTHERBOARD == 5
 #define KNOWN_BOARD 1
 
@@ -632,13 +637,18 @@
     #define E_ENABLE_PIN    3     //Added @ EJE Electronics 20100715
     #define TEMP_0_PIN      5     //changed @ rkoeppl 20110410
     #define HEATER_0_PIN    14    //changed @ rkoeppl 20110410
-    #define HEATER_1_PIN    -1    //changed @ rkoeppl 20110410
     
+    #ifdef GEN6_DELUXE
+      #define HEATER_1_PIN   1    
+      #define TEMP_1_PIN     0    
+    #else
+      #define HEATER_1_PIN   -1   
+      #define TEMP_1_PIN     -1    
+    #endif
     
     #define SDPOWER          -1
     #define SDSS          17
     #define LED_PIN         -1    //changed @ rkoeppl 20110410
-    #define TEMP_1_PIN      -1    //changed @ rkoeppl 20110410
     #define FAN_PIN         -1    //changed @ rkoeppl 20110410
     #define PS_ON_PIN       -1    //changed @ rkoeppl 20110410
     //our pin for debugging.
diff --git a/Sprinter/store_eeprom.cpp b/Sprinter/store_eeprom.cpp
index c2cd353..694e2c5 100644
--- a/Sprinter/store_eeprom.cpp
+++ b/Sprinter/store_eeprom.cpp
@@ -23,27 +23,30 @@
 #include "store_eeprom.h"
 #include "Configuration.h"
 
+#ifdef PIDTEMP
+ extern unsigned int PID_Kp, PID_Ki, PID_Kd;
+#endif
 
 
 #ifdef USE_EEPROM_SETTINGS
 
 //======================================================================================
 //========================= Read / Write EEPROM =======================================
-template <class T> int EEPROM_writeAnything(int &ee, const T& value)
+template <class T> int EEPROM_write_setting(int address, const T& value)
 {
   const byte* p = (const byte*)(const void*)&value;
   int i;
   for (i = 0; i < (int)sizeof(value); i++)
-    eeprom_write_byte((unsigned char *)ee++, *p++);
+    eeprom_write_byte((unsigned char *)address++, *p++);
   return i;
 }
 
-template <class T> int EEPROM_readAnything(int &ee, T& value)
+template <class T> int EEPROM_read_setting(int address, T& value)
 {
   byte* p = (byte*)(void*)&value;
   int i;
   for (i = 0; i < (int)sizeof(value); i++)
-    *p++ = eeprom_read_byte((unsigned char *)ee++);
+    *p++ = eeprom_read_byte((unsigned char *)address++);
   return i;
 }
 //======================================================================================
@@ -51,34 +54,34 @@ template <class T> int EEPROM_readAnything(int &ee, T& value)
 
 void EEPROM_StoreSettings() 
 {
-
-  unsigned long ul_help = 20000;
-  unsigned int ui_help = 0;
   char ver[4]= "000";
-  int i=EEPROM_OFFSET;
-  EEPROM_writeAnything(i,ver); // invalidate data first 
-  EEPROM_writeAnything(i,axis_steps_per_unit);  
-  EEPROM_writeAnything(i,max_feedrate);  
-  EEPROM_writeAnything(i,max_acceleration_units_per_sq_second);
-  EEPROM_writeAnything(i,move_acceleration);
-  EEPROM_writeAnything(i,retract_acceleration);
-  EEPROM_writeAnything(i,minimumfeedrate);
-  EEPROM_writeAnything(i,mintravelfeedrate);
-  EEPROM_writeAnything(i,ul_help);  //Min Segment Time, not used yet
-  EEPROM_writeAnything(i,max_xy_jerk);
-  EEPROM_writeAnything(i,max_z_jerk);
-
-  //PID Settings, not used yet --> placeholder
-  ui_help = 2560;
-  EEPROM_writeAnything(i,ui_help);     //Kp
-  ui_help = 64;
-  EEPROM_writeAnything(i,ui_help);     //Ki
-  ui_help = 4096;
-  EEPROM_writeAnything(i,ui_help);     //Kd
+  EEPROM_write_setting(EEPROM_OFFSET, ver); // invalidate data first
+  EEPROM_write_setting(axis_steps_per_unit_address, axis_steps_per_unit);
+  EEPROM_write_setting(max_feedrate_address, max_feedrate);
+  EEPROM_write_setting(max_acceleration_units_per_sq_second_address, max_acceleration_units_per_sq_second);
+  EEPROM_write_setting(move_acceleration_address, move_acceleration);
+  EEPROM_write_setting(retract_acceleration_address, retract_acceleration);
+  EEPROM_write_setting(minimumfeedrate_address, minimumfeedrate);
+  EEPROM_write_setting(mintravelfeedrate_address, mintravelfeedrate);
+  EEPROM_write_setting(min_seg_time_address, min_seg_time);  //Min Segment Time, not used yet
+  EEPROM_write_setting(max_xy_jerk_address, max_xy_jerk);
+  EEPROM_write_setting(max_z_jerk_address, max_z_jerk);
+  EEPROM_write_setting(max_e_jerk_address, max_e_jerk);
+
+  //PID Settings
+  #ifdef PIDTEMP
+   EEPROM_write_setting(Kp_address, PID_Kp);     //Kp
+   EEPROM_write_setting(Ki_address, PID_Ki);     //Ki
+   EEPROM_write_setting(Kd_address, PID_Kd);     //Kd
+  #else
+   EEPROM_write_setting(Kp_address, 2048);     //Kp
+   EEPROM_write_setting(Ki_address, 32);     //Ki
+   EEPROM_write_setting(Kd_address, 2048);     //Kd
+  #endif
+  
 
   char ver2[4]=EEPROM_VERSION;
-  i=EEPROM_OFFSET;
-  EEPROM_writeAnything(i,ver2); // validate data
+  EEPROM_write_setting(EEPROM_OFFSET, ver2); // validate data
   showString(PSTR("Settings Stored\r\n"));
  
 }
@@ -86,7 +89,6 @@ void EEPROM_StoreSettings()
 
 void EEPROM_printSettings()
 {  
-      // if def=true, the default values will be used
   #ifdef PRINT_EEPROM_SETTING
       showString(PSTR("Steps per unit:\r\n"));
       showString(PSTR(" M92 X"));
@@ -99,7 +101,7 @@ void EEPROM_printSettings()
       Serial.println(axis_steps_per_unit[3]);
       
       showString(PSTR("Maximum feedrates (mm/s):\r\n"));
-      showString(PSTR("  M203 X"));
+      showString(PSTR("  M202 X"));
       Serial.print(max_feedrate[0]);
       showString(PSTR(" Y"));
       Serial.print(max_feedrate[1]); 
@@ -124,29 +126,32 @@ void EEPROM_printSettings()
       showString(PSTR(" T"));
       Serial.println(retract_acceleration);
 
-      showString(PSTR("Advanced variables: S=Min feedrate (mm/s), T=Min travel feedrate (mm/s), X=maximum xY jerk (mm/s),  Z=maximum Z jerk (mm/s)\r\n"));
+      showString(PSTR("Advanced variables (mm/s): S=Min feedrate, T=Min travel feedrate, X=max xY jerk,  Z=max Z jerk, E=max E jerk\r\n"));
 
       showString(PSTR("  M205 S"));
       Serial.print(minimumfeedrate ); 
       showString(PSTR(" T" ));
       Serial.print(mintravelfeedrate ); 
 //      showString(PSTR(" B"));
-//      Serial.print(minsegmenttime ); 
+//      Serial.print(min_seg_time ); 
       showString(PSTR(" X"));
       Serial.print(max_xy_jerk ); 
       showString(PSTR(" Z"));
-      Serial.println(max_z_jerk);
+      Serial.print(max_z_jerk);
+      showString(PSTR(" E"));
+      Serial.println(max_e_jerk);
+
       
     #ifdef PIDTEMP
-    /*
-      showString(PSTR("PID settings:");
-      showString(PSTR("   M301 P"));
-      Serial.print(Kp); 
+    
+      showString(PSTR("PID settings:\r\n"));
+      showString(PSTR("  M301 P"));
+      Serial.print(PID_Kp); 
       showString(PSTR(" I"));
-      Serial.print(Ki); 
-      SshowString(PSTR(" D"));
-      Serial.print(Kd);
-    */
+      Serial.print(PID_Ki); 
+      showString(PSTR(" D"));
+      Serial.println(PID_Kd);
+    
     #endif
   #endif
 
@@ -159,26 +164,27 @@ void EEPROM_RetrieveSettings(bool def, bool printout)
     int i=EEPROM_OFFSET;
     char stored_ver[4];
     char ver[4]=EEPROM_VERSION;
-    unsigned long ul_help = 0;
     
-    EEPROM_readAnything(i,stored_ver); //read stored version
-    if ((!def)&&(strncmp(ver,stored_ver,3)==0)) 
+    EEPROM_read_setting(EEPROM_OFFSET,stored_ver); //read stored version
+    if ((!def)&&(strncmp(ver,stored_ver,3)==0))
     {   // version number match
-      EEPROM_readAnything(i,axis_steps_per_unit);  
-      EEPROM_readAnything(i,max_feedrate);  
-      EEPROM_readAnything(i,max_acceleration_units_per_sq_second);
-      EEPROM_readAnything(i,move_acceleration);
-      EEPROM_readAnything(i,retract_acceleration);
-      EEPROM_readAnything(i,minimumfeedrate);
-      EEPROM_readAnything(i,mintravelfeedrate);
-      EEPROM_readAnything(i,ul_help);  //min Segmenttime --> not used yet
-      EEPROM_readAnything(i,max_xy_jerk);
-      EEPROM_readAnything(i,max_z_jerk);
-
-      unsigned int Kp,Ki,Kd;
-      EEPROM_readAnything(i,Kp);
-      EEPROM_readAnything(i,Ki);
-      EEPROM_readAnything(i,Kd);
+      EEPROM_read_setting(axis_steps_per_unit_address, axis_steps_per_unit);
+      EEPROM_read_setting(max_feedrate_address, max_feedrate);
+      EEPROM_read_setting(max_acceleration_units_per_sq_second_address, max_acceleration_units_per_sq_second);
+      EEPROM_read_setting(move_acceleration_address, move_acceleration);
+      EEPROM_read_setting(retract_acceleration_address, retract_acceleration);
+      EEPROM_read_setting(minimumfeedrate_address, minimumfeedrate);
+      EEPROM_read_setting(mintravelfeedrate_address, mintravelfeedrate);
+      EEPROM_read_setting(min_seg_time_address, min_seg_time);  //min Segmenttime --> not used yet
+      EEPROM_read_setting(max_xy_jerk_address, max_xy_jerk);
+      EEPROM_read_setting(max_z_jerk_address, max_z_jerk);
+      EEPROM_read_setting(max_e_jerk_address, max_e_jerk);
+
+      #ifdef PIDTEMP
+       EEPROM_read_setting(Kp_address, PID_Kp);
+       EEPROM_read_setting(Ki_address, PID_Ki);
+       EEPROM_read_setting(Kd_address, PID_Kd);
+      #endif
 
       showString(PSTR("Stored settings retreived\r\n"));
     }
@@ -200,10 +206,18 @@ void EEPROM_RetrieveSettings(bool def, bool printout)
       mintravelfeedrate=DEFAULT_MINTRAVELFEEDRATE;
       max_xy_jerk=_MAX_XY_JERK;
       max_z_jerk=_MAX_Z_JERK;
+      max_e_jerk=_MAX_E_JERK;
+      min_seg_time=_MIN_SEG_TIME;
+      
+      #ifdef PIDTEMP
+       PID_Kp = PID_PGAIN;
+       PID_Ki = PID_IGAIN;
+       PID_Kd = PID_DGAIN;
+      #endif
 
       showString(PSTR("Using Default settings\r\n"));
     }
-
+    
     if(printout)
     {
       EEPROM_printSettings();
diff --git a/Sprinter/store_eeprom.h b/Sprinter/store_eeprom.h
index cff25d3..a9ddf7d 100644
--- a/Sprinter/store_eeprom.h
+++ b/Sprinter/store_eeprom.h
@@ -27,7 +27,7 @@
 // the default values are used whenever there is a change to the data, to prevent
 // wrong data being written to the variables.
 // ALSO:  always make sure the variables in the Store and retrieve sections are in the same order.
-#define EEPROM_VERSION "S01"  
+#define EEPROM_VERSION "S03"
 
 
 extern float axis_steps_per_unit[4]; 
@@ -39,7 +39,23 @@ extern float mintravelfeedrate;
 extern float minimumfeedrate;
 extern float max_xy_jerk;
 extern float max_z_jerk;
+extern float max_e_jerk;
+extern unsigned long min_seg_time;
 
+#define axis_steps_per_unit_address (EEPROM_OFFSET + 4*sizeof(char))
+#define max_feedrate_address (axis_steps_per_unit_address + 4*sizeof(float))
+#define max_acceleration_units_per_sq_second_address (max_feedrate_address + 4*sizeof(float))
+#define move_acceleration_address (max_acceleration_units_per_sq_second_address + 4*sizeof(long))
+#define retract_acceleration_address (move_acceleration_address + sizeof(float))
+#define mintravelfeedrate_address (retract_acceleration_address + sizeof(float))
+#define minimumfeedrate_address (mintravelfeedrate_address + sizeof(float))
+#define max_xy_jerk_address (minimumfeedrate_address + sizeof(float))
+#define max_z_jerk_address (max_xy_jerk_address + sizeof(float))
+#define max_e_jerk_address (max_z_jerk_address + sizeof(float))
+#define min_seg_time_address (max_e_jerk_address + sizeof(float))
+#define Kp_address (min_seg_time_address + sizeof(unsigned long))
+#define Ki_address (Kp_address + sizeof(unsigned int))
+#define Kd_address (Ki_address + sizeof(unsigned int))
 
 extern void EEPROM_RetrieveSettings(bool def, bool printout );
 extern void EEPROM_printSettings();
diff --git a/Sprinter/thermistortables.h b/Sprinter/thermistortables.h
index 222c013..270cfaf 100644
--- a/Sprinter/thermistortables.h
+++ b/Sprinter/thermistortables.h
@@ -423,4 +423,4 @@ const short temptable_7[NUMTEMPS_7][2] = {
 #error No bed thermistor table specified
 #endif
 
-#endif //THERMISTORTABLES_H_
\ No newline at end of file
+#endif //THERMISTORTABLES_H_