chiark / gitweb /
Merge remote-tracking branch 'origin/Marlin_v1' into Marlin_v1
authorErik van der Zalm <erik@vdzalm.eu>
Sun, 13 Nov 2011 19:46:44 +0000 (20:46 +0100)
committerErik van der Zalm <erik@vdzalm.eu>
Sun, 13 Nov 2011 19:46:44 +0000 (20:46 +0100)
1  2 
Marlin/Configuration.h
Marlin/Marlin.pde
Marlin/planner.cpp
Marlin/planner.h
Marlin/stepper.cpp
Marlin/temperature.cpp

index 2f1e8ad8373de5f293d55df31525c5e9e79e4fd8,8661ae438e78afd285434d87752b0b730f3fb0a5..6c8b3a4ecf4b80441e90f5033282abbf901cd6c2
@@@ -1,20 -1,14 +1,23 @@@
  #ifndef __CONFIGURATION_H
  #define __CONFIGURATION_H
  
- //#define DEBUG_STEPS
  
- #define MM_PER_ARC_SEGMENT 1
- #define N_ARC_CORRECTION 25
+ // This determines the communication speed of the printer
+ //#define BAUDRATE 250000
+ #define BAUDRATE 115200
+ //#define BAUDRATE 230400
  
 +// Frequency limit
 +// See nophead's blog for more info
 +#define XY_FREQUENCY_LIMIT  15
 +
 +// 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)
 +
  // BASIC SETTINGS: select your board type, thermistor type, axis scaling, and endstop configuration
  
  //// The following define selects which electronics board you have. Please choose the one that matches your setup
@@@ -152,92 -196,37 +211,37 @@@ const bool ENDSTOPS_INVERTING = true; /
  
  // minimum time in microseconds that a movement needs to take if the buffer is emptied.   Increase this number if you see blobs while printing high speed & high detail.  It will slowdown on the detailed stuff.
  #define DEFAULT_MINSEGMENTTIME        20000
 -#define DEFAULT_XYJERK                30.0*60    
 -#define DEFAULT_ZJERK                 10.0*60
 +#define DEFAULT_XYJERK                30.0    // (mm/sec)
 +#define DEFAULT_ZJERK                 0.4     // (mm/sec)
  
  
- // The watchdog waits for the watchperiod in milliseconds whenever an M104 or M109 increases the target temperature
- //this enables the watchdog interrupt.
- #define USE_WATCHDOG
- //you cannot reboot on a mega2560 due to a bug in he bootloader. Hence, you have to reset manually, and this is done hereby:
- #define RESET_MANUAL
- #define WATCHDOG_TIMEOUT 4
  
- //// Experimental watchdog and minimal temp
- // The watchdog waits for the watchperiod in milliseconds whenever an M104 or M109 increases the target temperature
- // If the temperature has not increased at the end of that period, the target temperature is set to zero. It can be reset with another M104/M109
- /// CURRENTLY NOT IMPLEMENTED AND UNUSEABLE
- //#define WATCHPERIOD 5000 //5 seconds
- // Actual temperature must be close to target for this long before M109 returns success
- //#define TEMP_RESIDENCY_TIME 20  // (seconds)
- //#define TEMP_HYSTERESIS 5       // (C°) range of +/- temperatures considered "close" to the target one
  
- //// The minimal temperature defines the temperature below which the heater will not be enabled
- //#define HEATER_0_MINTEMP 5
- //#define HEATER_1_MINTEMP 5
- //#define BED_MINTEMP 5
+ //===========================================================================
+ //=============================Additional Features===========================
+ //===========================================================================
  
+ // EEPROM
+ // the microcontroller can store settings in the EEPROM, e.g. max velocity...
+ // M500 - stores paramters in EEPROM
+ // M501 - reads parameters from EEPROM (if you need reset them after you changed them temporarily).  
+ // M502 - reverts to the default "factory settings".  You still need to store them in EEPROM afterwards if you want to.
+ //define this to enable eeprom support
+ #define EEPROM_SETTINGS
+ //to disable EEPROM Serial responses and decrease program space by ~1700 byte: comment this out:
+ // please keep turned on if you can.
+ #define EEPROM_CHITCHAT
  
- // When temperature exceeds max temp, your heater will be switched off.
- // This feature exists to protect your hotend from overheating accidentally, but *NOT* from thermistor short/failure!
- // You should use MINTEMP for thermistor short/failure protection.
- //#define HEATER_0_MAXTEMP 275
- //#define_HEATER_1_MAXTEMP 275
- //#define BED_MAXTEMP 150
  
- /// PID settings:
- // Uncomment the following line to enable PID support.
-   
- #define PIDTEMP
- #ifdef PIDTEMP
-   //#define PID_DEBUG // Sends debug data to the serial port. 
-   //#define PID_OPENLOOP 1 // Puts PID in open loop. M104 sets the output power in %
-   
-   #define PID_MAX 255 // limits current to nozzle; 255=full current
-   #define PID_INTEGRAL_DRIVE_MAX 255  //limit for the integral term
-   #define K1 0.95 //smoothing factor withing the PID
-   #define PID_dT 0.1 //sampling period of the PID
+ // The watchdog waits for the watchperiod in milliseconds whenever an M104 or M109 increases the target temperature
+ // this enables the watchdog interrupt.
+ #define USE_WATCHDOG
+ // you cannot reboot on a mega2560 due to a bug in he bootloader. Hence, you have to reset manually, and this is done hereby:
+ #define RESET_MANUAL
+ #define WATCHDOG_TIMEOUT 4  //seconds
  
-   //To develop some PID settings for your machine, you can initiall follow 
-   // the Ziegler-Nichols method.
-   // set Ki and Kd to zero. 
-   // heat with a defined Kp and see if the temperature stabilizes
-   // ideally you do this graphically with repg.
-   // the PID_CRITIAL_GAIN should be the Kp at which temperature oscillatins are not dampned out/decreas in amplitutde
-   // PID_SWING_AT_CRITIAL is the time for a full period of the oscillations at the critical Gain
-   // usually further manual tunine is necessary.
  
-   #define PID_CRITIAL_GAIN 3000
-   #define PID_SWING_AT_CRITIAL 45 //seconds
-   
-   #define PID_PI    //no differentail term
-   //#define PID_PID //normal PID
  
-   #ifdef PID_PID
-     //PID according to Ziegler-Nichols method
-     #define  DEFAULT_Kp  (0.6*PID_CRITIAL_GAIN)
-     #define  DEFAULT_Ki (2*Kp/PID_SWING_AT_CRITIAL*PID_dT)  
-     #define  DEFAULT_Kd (PID_SWING_AT_CRITIAL/8./PID_dT)  
-   #endif
-  
-   #ifdef PID_PI
-     //PI according to Ziegler-Nichols method
-     #define  DEFAULT_Kp (PID_CRITIAL_GAIN/2.2) 
-     #define  DEFAULT_Ki (1.2*Kp/PID_SWING_AT_CRITIAL*PID_dT)
-     #define  DEFAULT_Kd (0)
-   #endif
-   
-   // this adds an experimental additional term to the heatingpower, proportional to the extrusion speed.
-   // if Kc is choosen well, the additional required power due to increased melting should be compensated.
-   #define PID_ADD_EXTRUSION_RATE  
-   #ifdef PID_ADD_EXTRUSION_RATE
-     #define  DEFAULT_Kc (5) //heatingpower=Kc*(e_speed)
-   #endif
- #endif // PIDTEMP
  
  // extruder advance constant (s2/mm3)
  //
@@@ -261,11 -296,12 +311,12 @@@ const int dropsegments=0; //everything 
  // The number of linear motions that can be in the plan at any give time.  
  // THE BLOCK_BUFFER_SIZE NEEDS TO BE A POWER OF 2, i.g. 8,16,32 because shifts and ors are used to do the ringbuffering.
  #if defined SDSUPPORT
 -  #define BLOCK_BUFFER_SIZE 16   // SD,LCD,Buttons take more memory, block buffer needs to be smaller
 +  #define BLOCK_BUFFER_SIZE 8   // SD,LCD,Buttons take more memory, block buffer needs to be smaller
  #else
 -  #define BLOCK_BUFFER_SIZE 16 // maximize block buffer
 +  #define BLOCK_BUFFER_SIZE 8 // maximize block buffer
  #endif
  
  //The ASCII buffer for recieving from the serial:
  #define MAX_CMD_SIZE 96
  #define BUFSIZE 4
Simple merge
index fe19055c4b8ecb2669d87aadf787ade55287416c,863b116a5bc5fbf5e4d742663486dc5791702260..998a609101a076e26e3a6beb553d507464608414
 -/*\r
 -  planner.c - buffers movement commands and manages the acceleration profile plan\r
 -  Part of Grbl\r
 -\r
 -  Copyright (c) 2009-2011 Simen Svale Skogsrud\r
 -\r
 -  Grbl is free software: you can redistribute it and/or modify\r
 -  it under the terms of the GNU General Public License as published by\r
 -  the Free Software Foundation, either version 3 of the License, or\r
 -  (at your option) any later version.\r
 -\r
 -  Grbl is distributed in the hope that it will be useful,\r
 -  but WITHOUT ANY WARRANTY; without even the implied warranty of\r
 -  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
 -  GNU General Public License for more details.\r
 -\r
 -  You should have received a copy of the GNU General Public License\r
 -  along with Grbl.  If not, see <http://www.gnu.org/licenses/>.\r
 -*/\r
 -\r
 -/* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */\r
 -\r
 -/*  \r
 -  Reasoning behind the mathematics in this module (in the key of 'Mathematica'):\r
 -  \r
 -  s == speed, a == acceleration, t == time, d == distance\r
 -\r
 -  Basic definitions:\r
 -\r
 -    Speed[s_, a_, t_] := s + (a*t) \r
 -    Travel[s_, a_, t_] := Integrate[Speed[s, a, t], t]\r
 -\r
 -  Distance to reach a specific speed with a constant acceleration:\r
 -\r
 -    Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, d, t]\r
 -      d -> (m^2 - s^2)/(2 a) --> estimate_acceleration_distance()\r
 -\r
 -  Speed after a given distance of travel with constant acceleration:\r
 -\r
 -    Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, m, t]\r
 -      m -> Sqrt[2 a d + s^2]    \r
 -\r
 -    DestinationSpeed[s_, a_, d_] := Sqrt[2 a d + s^2]\r
 -\r
 -  When to start braking (di) to reach a specified destionation speed (s2) after accelerating\r
 -  from initial speed s1 without ever stopping at a plateau:\r
 -\r
 -    Solve[{DestinationSpeed[s1, a, di] == DestinationSpeed[s2, a, d - di]}, di]\r
 -      di -> (2 a d - s1^2 + s2^2)/(4 a) --> intersection_distance()\r
 -\r
 -    IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a)\r
 -*/\r
 -                                                                                                            \r
 -\r
 -//#include <inttypes.h>\r
 -//#include <math.h>       \r
 -//#include <stdlib.h>\r
 -\r
 -#include "Marlin.h"\r
 -#include "Configuration.h"\r
 -#include "pins.h"\r
 -#include "fastio.h"\r
 -#include "planner.h"\r
 -#include "stepper.h"\r
 -#include "temperature.h"\r
 -#include "ultralcd.h"\r
 -\r
 -//===========================================================================\r
 -//=============================public variables ============================\r
 -//===========================================================================\r
 -\r
 -unsigned long minsegmenttime;\r
 -float max_feedrate[4]; // set the max speeds\r
 -float axis_steps_per_unit[4];\r
 -long max_acceleration_units_per_sq_second[4]; // Use M201 to override by software\r
 -float minimumfeedrate;\r
 -float acceleration;         // Normal acceleration mm/s^2  THIS IS THE DEFAULT ACCELERATION for all moves. M204 SXXXX\r
 -float retract_acceleration; //  mm/s^2   filament pull-pack and push-forward  while standing still in the other axis M204 TXXXX\r
 -float max_xy_jerk; //speed than can be stopped at once, if i understand correctly.\r
 -float max_z_jerk;\r
 -float mintravelfeedrate;\r
 -unsigned long axis_steps_per_sqr_second[NUM_AXIS];\r
 -\r
 -// The current position of the tool in absolute steps\r
 -long position[4];   //rescaled from extern when axis_steps_per_unit are changed by gcode\r
 -\r
 -#ifdef AUTOTEMP\r
 -float high_e_speed=0;\r
 -#endif\r
 -\r
 -\r
 -//===========================================================================\r
 -//=============================private variables ============================\r
 -//===========================================================================\r
 -static block_t block_buffer[BLOCK_BUFFER_SIZE];            // A ring buffer for motion instfructions\r
 -static volatile unsigned char block_buffer_head;           // Index of the next block to be pushed\r
 -static volatile unsigned char block_buffer_tail;           // Index of the block to process now\r
 -\r
 -\r
 -\r
 -//===========================================================================\r
 -//=============================functions         ============================\r
 -//===========================================================================\r
 -#define ONE_MINUTE_OF_MICROSECONDS 60000000.0\r
 -\r
 -// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the \r
 -// given acceleration:\r
 -inline float estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration) {\r
 -  if (acceleration!=0) {\r
 -  return((target_rate*target_rate-initial_rate*initial_rate)/\r
 -         (2.0*acceleration));\r
 -  }\r
 -  else {\r
 -    return 0.0;  // acceleration was 0, set acceleration distance to 0\r
 -  }\r
 -}\r
 -\r
 -// This function gives you the point at which you must start braking (at the rate of -acceleration) if \r
 -// you started at speed initial_rate and accelerated until this point and want to end at the final_rate after\r
 -// a total travel of distance. This can be used to compute the intersection point between acceleration and\r
 -// deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)\r
 -\r
 -inline float intersection_distance(float initial_rate, float final_rate, float acceleration, float distance) {\r
 - if (acceleration!=0) {\r
 -  return((2.0*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/\r
 -         (4.0*acceleration) );\r
 -  }\r
 -  else {\r
 -    return 0.0;  // acceleration was 0, set intersection distance to 0\r
 -  }\r
 -}\r
 -\r
 -// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.\r
 -\r
 -void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit_speed) {\r
 -  if(block->busy == true) return; // If block is busy then bail out.\r
 -  float entry_factor = entry_speed / block->nominal_speed;\r
 -  float exit_factor = exit_speed / block->nominal_speed;\r
 -  long initial_rate = ceil(block->nominal_rate*entry_factor);\r
 -  long final_rate = ceil(block->nominal_rate*exit_factor);\r
 -  \r
 -  #ifdef ADVANCE\r
 -    long initial_advance = block->advance*entry_factor*entry_factor;\r
 -    long final_advance = block->advance*exit_factor*exit_factor;\r
 -  #endif // ADVANCE\r
 -\r
 -  // Limit minimal step rate (Otherwise the timer will overflow.)\r
 -  if(initial_rate <120) initial_rate=120;\r
 -  if(final_rate < 120) final_rate=120;\r
 -  \r
 -  // Calculate the acceleration steps\r
 -  long acceleration = block->acceleration_st;\r
 -  long accelerate_steps = estimate_acceleration_distance(initial_rate, block->nominal_rate, acceleration);\r
 -  long decelerate_steps = estimate_acceleration_distance(final_rate, block->nominal_rate, acceleration);\r
 -  // Calculate the size of Plateau of Nominal Rate. \r
 -  long plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps;\r
 -\r
 -  // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will\r
 -  // have to use intersection_distance() to calculate when to abort acceleration and start braking \r
 -  // in order to reach the final_rate exactly at the end of this block.\r
 -  if (plateau_steps < 0) {  \r
 -    accelerate_steps = intersection_distance(initial_rate, final_rate, acceleration, block->step_event_count);\r
 -    plateau_steps = 0;\r
 -  }  \r
 -\r
 -  long decelerate_after = accelerate_steps+plateau_steps;\r
 -\r
 -  CRITICAL_SECTION_START;  // Fill variables used by the stepper in a critical section\r
 -  if(block->busy == false) { // Don't update variables if block is busy.\r
 -    block->accelerate_until = accelerate_steps;\r
 -    block->decelerate_after = decelerate_after;\r
 -    block->initial_rate = initial_rate;\r
 -    block->final_rate = final_rate;\r
 -  #ifdef ADVANCE\r
 -      block->initial_advance = initial_advance;\r
 -      block->final_advance = final_advance;\r
 -  #endif //ADVANCE\r
 -  }\r
 -  CRITICAL_SECTION_END;\r
 -}                    \r
 -\r
 -// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the \r
 -// acceleration within the allotted distance.\r
 -inline float max_allowable_speed(float acceleration, float target_velocity, float distance) {\r
 -  return  sqrt(target_velocity*target_velocity-2*acceleration*60*60*distance);\r
 -}\r
 -\r
 -// "Junction jerk" in this context is the immediate change in speed at the junction of two blocks.\r
 -// This method will calculate the junction jerk as the euclidean distance between the nominal \r
 -// velocities of the respective blocks.\r
 -inline float junction_jerk(block_t *before, block_t *after) {\r
 -  return sqrt(\r
 -    pow((before->speed_x-after->speed_x), 2)+pow((before->speed_y-after->speed_y), 2));\r
 -}\r
 -\r
 -// Return the safe speed which is max_jerk/2, e.g. the \r
 -// speed under which you cannot exceed max_jerk no matter what you do.\r
 -float safe_speed(block_t *block) {\r
 -  float safe_speed;\r
 -  safe_speed = max_xy_jerk/2;  \r
 -  if(abs(block->speed_z) > max_z_jerk/2) \r
 -    safe_speed = max_z_jerk/2;\r
 -  if (safe_speed > block->nominal_speed) \r
 -    safe_speed = block->nominal_speed;\r
 -  return safe_speed;  \r
 -}\r
 -\r
 -// The kernel called by planner_recalculate() when scanning the plan from last to first entry.\r
 -void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) {\r
 -  if(!current) { \r
 -    return; \r
 -  }\r
 -\r
 -  float entry_speed = current->nominal_speed;\r
 -  float exit_factor;\r
 -  float exit_speed;\r
 -  if (next) {\r
 -    exit_speed = next->entry_speed;\r
 -  } \r
 -  else {\r
 -    exit_speed = safe_speed(current);\r
 -  }\r
 -\r
 -  // Calculate the entry_factor for the current block. \r
 -  if (previous) {\r
 -    // Reduce speed so that junction_jerk is within the maximum allowed\r
 -    float jerk = junction_jerk(previous, current);\r
 -    if((previous->steps_x == 0) && (previous->steps_y == 0)) {\r
 -      entry_speed = safe_speed(current);\r
 -    }\r
 -    else if (jerk > max_xy_jerk) {\r
 -      entry_speed = (max_xy_jerk/jerk) * entry_speed;\r
 -    } \r
 -    if(abs(previous->speed_z - current->speed_z) > max_z_jerk) {\r
 -      entry_speed = (max_z_jerk/abs(previous->speed_z - current->speed_z)) * entry_speed;\r
 -    } \r
 -    // If the required deceleration across the block is too rapid, reduce the entry_factor accordingly.\r
 -    if (entry_speed > exit_speed) {\r
 -      float max_entry_speed = max_allowable_speed(-current->acceleration,exit_speed, current->millimeters);\r
 -      if (max_entry_speed < entry_speed) {\r
 -        entry_speed = max_entry_speed;\r
 -      }\r
 -    }    \r
 -  } \r
 -  else {\r
 -    entry_speed = safe_speed(current);\r
 -  }\r
 -  // Store result\r
 -  current->entry_speed = entry_speed;\r
 -}\r
 -\r
 -// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This \r
 -// implements the reverse pass.\r
 -void planner_reverse_pass() {\r
 -  char block_index = block_buffer_head;\r
 -  if(((block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1)) > 3) {\r
 -    block_index = (block_buffer_head - 3) & (BLOCK_BUFFER_SIZE - 1);\r
 -    block_t *block[5] = {\r
 -      NULL, NULL, NULL, NULL, NULL  };\r
 -    while(block_index != block_buffer_tail) { \r
 -      block_index = (block_index-1) & (BLOCK_BUFFER_SIZE -1); \r
 -      block[2]= block[1];\r
 -      block[1]= block[0];\r
 -      block[0] = &block_buffer[block_index];\r
 -      planner_reverse_pass_kernel(block[0], block[1], block[2]);\r
 -    }\r
 -    planner_reverse_pass_kernel(NULL, block[0], block[1]);\r
 -  }\r
 -}\r
 -\r
 -// The kernel called by planner_recalculate() when scanning the plan from first to last entry.\r
 -void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) {\r
 -  if(!current) { \r
 -    return; \r
 -  }\r
 -  if(previous) {\r
 -    // If the previous block is an acceleration block, but it is not long enough to \r
 -    // complete the full speed change within the block, we need to adjust out entry\r
 -    // speed accordingly. Remember current->entry_factor equals the exit factor of \r
 -    // the previous block.\r
 -    if(previous->entry_speed < current->entry_speed) {\r
 -      float max_entry_speed = max_allowable_speed(-previous->acceleration, previous->entry_speed, previous->millimeters);\r
 -      if (max_entry_speed < current->entry_speed) {\r
 -        current->entry_speed = max_entry_speed;\r
 -      }\r
 -    }\r
 -  }\r
 -}\r
 -\r
 -// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This \r
 -// implements the forward pass.\r
 -void planner_forward_pass() {\r
 -  char block_index = block_buffer_tail;\r
 -  block_t *block[3] = {\r
 -    NULL, NULL, NULL  };\r
 -\r
 -  while(block_index != block_buffer_head) {\r
 -    block[0] = block[1];\r
 -    block[1] = block[2];\r
 -    block[2] = &block_buffer[block_index];\r
 -    planner_forward_pass_kernel(block[0],block[1],block[2]);\r
 -    block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1);\r
 -  }\r
 -  planner_forward_pass_kernel(block[1], block[2], NULL);\r
 -}\r
 -\r
 -// Recalculates the trapezoid speed profiles for all blocks in the plan according to the \r
 -// entry_factor for each junction. Must be called by planner_recalculate() after \r
 -// updating the blocks.\r
 -void planner_recalculate_trapezoids() {\r
 -  char block_index = block_buffer_tail;\r
 -  block_t *current;\r
 -  block_t *next = NULL;\r
 -  while(block_index != block_buffer_head) {\r
 -    current = next;\r
 -    next = &block_buffer[block_index];\r
 -    if (current) {\r
 -      calculate_trapezoid_for_block(current, current->entry_speed, next->entry_speed);      \r
 -    }\r
 -    block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1);\r
 -  }\r
 -  calculate_trapezoid_for_block(next, next->entry_speed, safe_speed(next));\r
 -}\r
 -\r
 -// Recalculates the motion plan according to the following algorithm:\r
 -//\r
 -//   1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_factor) \r
 -//      so that:\r
 -//     a. The junction jerk is within the set limit\r
 -//     b. No speed reduction within one block requires faster deceleration than the one, true constant \r
 -//        acceleration.\r
 -//   2. Go over every block in chronological order and dial down junction speed reduction values if \r
 -//     a. The speed increase within one block would require faster accelleration than the one, true \r
 -//        constant acceleration.\r
 -//\r
 -// When these stages are complete all blocks have an entry_factor that will allow all speed changes to \r
 -// be performed using only the one, true constant acceleration, and where no junction jerk is jerkier than \r
 -// the set limit. Finally it will:\r
 -//\r
 -//   3. Recalculate trapezoids for all blocks.\r
 -\r
 -void planner_recalculate() {   \r
 -  planner_reverse_pass();\r
 -  planner_forward_pass();\r
 -  planner_recalculate_trapezoids();\r
 -}\r
 -\r
 -void plan_init() {\r
 -  block_buffer_head = 0;\r
 -  block_buffer_tail = 0;\r
 -  memset(position, 0, sizeof(position)); // clear position\r
 -}\r
 -\r
 -\r
 -void plan_discard_current_block() {\r
 -  if (block_buffer_head != block_buffer_tail) {\r
 -    block_buffer_tail = (block_buffer_tail + 1) & (BLOCK_BUFFER_SIZE - 1);  \r
 -  }\r
 -}\r
 -\r
 -block_t *plan_get_current_block() {\r
 -  if (block_buffer_head == block_buffer_tail) { \r
 -    return(NULL); \r
 -  }\r
 -  block_t *block = &block_buffer[block_buffer_tail];\r
 -  block->busy = true;\r
 -  return(block);\r
 -}\r
 -\r
 -#ifdef AUTOTEMP\r
 -void getHighESpeed()\r
 -{\r
 -  if(degTargetHotend0()+2<AUTOTEMP_MIN)  //probably temperature set to zero.\r
 -    return; //do nothing\r
 -  float high=0;\r
 -  char block_index = block_buffer_tail;\r
 -  \r
 -  while(block_index != block_buffer_head) {\r
 -    float se=block_buffer[block_index].speed_e;\r
 -    if(se>high)\r
 -    {\r
 -      high=se;\r
 -    }\r
 -    block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1);\r
 -  }\r
 -  high_e_speed=high*axis_steps_per_unit[E_AXIS]/(1000000.0);  //so it is independent of the esteps/mm. before \r
 -   \r
 -  float g=AUTOTEMP_MIN+high_e_speed*AUTOTEMP_FACTOR;\r
 -  float t=constrain(AUTOTEMP_MIN,g,AUTOTEMP_MAX);\r
 -  setTargetHotend0(t);\r
 -  SERIAL_ECHO_START;\r
 -  SERIAL_ECHOPAIR("highe",high_e_speed);\r
 -  SERIAL_ECHOPAIR(" t",t);\r
 -  SERIAL_ECHOLN("");\r
 -}\r
 -#endif\r
 -\r
 -void check_axes_activity() {\r
 -  unsigned char x_active = 0;\r
 -  unsigned char y_active = 0;  \r
 -  unsigned char z_active = 0;\r
 -  unsigned char e_active = 0;\r
 -  block_t *block;\r
 -\r
 -  if(block_buffer_tail != block_buffer_head) {\r
 -    char block_index = block_buffer_tail;\r
 -    while(block_index != block_buffer_head) {\r
 -      block = &block_buffer[block_index];\r
 -      if(block->steps_x != 0) x_active++;\r
 -      if(block->steps_y != 0) y_active++;\r
 -      if(block->steps_z != 0) z_active++;\r
 -      if(block->steps_e != 0) e_active++;\r
 -      block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1);\r
 -    }\r
 -  }\r
 -  if((DISABLE_X) && (x_active == 0)) disable_x();\r
 -  if((DISABLE_Y) && (y_active == 0)) disable_y();\r
 -  if((DISABLE_Z) && (z_active == 0)) disable_z();\r
 -  if((DISABLE_E) && (e_active == 0)) disable_e();\r
 -}\r
 -\r
 -// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in \r
 -// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration\r
 -// calculation the caller must also provide the physical length of the line in millimeters.\r
 -void plan_buffer_line(const float &x, const float &y, const float &z, const float &e,  float feed_rate)\r
 -{\r
 -  // Calculate the buffer head after we push this byte\r
 -  int next_buffer_head = (block_buffer_head + 1) & (BLOCK_BUFFER_SIZE - 1);\r
 -\r
 -  // If the buffer is full: good! That means we are well ahead of the robot. \r
 -  // Rest here until there is room in the buffer.\r
 -  while(block_buffer_tail == next_buffer_head) { \r
 -    manage_heater(); \r
 -    manage_inactivity(1); \r
 -    LCD_STATUS;\r
 -  }\r
 -\r
 -  // The target position of the tool in absolute steps\r
 -  // Calculate target position in absolute steps\r
 -  //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow\r
 -  long target[4];\r
 -  target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);\r
 -  target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);\r
 -  target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);     \r
 -  target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]); \r
 -  \r
 -  // Prepare to set up new block\r
 -  block_t *block = &block_buffer[block_buffer_head];\r
 -  \r
 -  // Mark block as not busy (Not executed by the stepper interrupt)\r
 -  block->busy = false;\r
 -\r
 -  // Number of steps for each axis\r
 -  block->steps_x = labs(target[X_AXIS]-position[X_AXIS]);\r
 -  block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);\r
 -  block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]);\r
 -  block->steps_e = labs(target[E_AXIS]-position[E_AXIS]);\r
 -  block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e)));\r
 -\r
 -  // Bail if this is a zero-length block\r
 -  if (block->step_event_count <=dropsegments) { \r
 -    return; \r
 -  };\r
 -\r
 -  //enable active axes\r
 -  if(block->steps_x != 0) enable_x();\r
 -  if(block->steps_y != 0) enable_y();\r
 -  if(block->steps_z != 0) enable_z();\r
 -  if(block->steps_e != 0) enable_e();\r
 -\r
 -  float delta_x_mm = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];\r
 -  float delta_y_mm = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];\r
 -  float delta_z_mm = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS];\r
 -  float delta_e_mm = (target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS];\r
 -  block->millimeters = sqrt(square(delta_x_mm) + square(delta_y_mm) + square(delta_z_mm) + square(delta_e_mm));\r
 -\r
 -  unsigned long microseconds;\r
 -\r
 -  if (block->steps_e == 0) {\r
 -        if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;\r
 -  }\r
 -  else {\r
 -      if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;\r
 -  } \r
 -\r
 -  microseconds = lround((block->millimeters/feed_rate)*1000000);\r
 -\r
 -  // slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill\r
 -  // reduces/removes corner blobs as the machine won't come to a full stop.\r
 -  int blockcount=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1);\r
 -  \r
 -  if ((blockcount>0) && (blockcount < (BLOCK_BUFFER_SIZE - 4))) {\r
 -    if (microseconds<minsegmenttime)  { // buffer is draining, add extra time.  The amount of time added increases if the buffer is still emptied more.\r
 -        microseconds=microseconds+lround(2*(minsegmenttime-microseconds)/blockcount);\r
 -    }\r
 -  }\r
 -  else {\r
 -    if (microseconds<minsegmenttime) microseconds=minsegmenttime;\r
 -  }\r
 -  //  END OF SLOW DOWN SECTION  \r
 -  \r
 -  \r
 -  // Calculate speed in mm/minute for each axis\r
 -  float multiplier = 60.0*1000000.0/microseconds;\r
 -  block->speed_z = delta_z_mm * multiplier; \r
 -  block->speed_x = delta_x_mm * multiplier;\r
 -  block->speed_y = delta_y_mm * multiplier;\r
 -  block->speed_e = delta_e_mm * multiplier; \r
 -\r
 -\r
 -  // Limit speed per axis\r
 -  float speed_factor = 1; //factor <=1 do decrease speed\r
 -  if(abs(block->speed_x) > max_feedrate[X_AXIS]) {\r
 -    speed_factor = max_feedrate[X_AXIS] / abs(block->speed_x);\r
 -    //if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor; /is not need here because auf the init above\r
 -  }\r
 -  if(abs(block->speed_y) > max_feedrate[Y_AXIS]){\r
 -    float tmp_speed_factor = max_feedrate[Y_AXIS] / abs(block->speed_y);\r
 -    if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor;\r
 -  }\r
 -  if(abs(block->speed_z) > max_feedrate[Z_AXIS]){\r
 -    float tmp_speed_factor = max_feedrate[Z_AXIS] / abs(block->speed_z);\r
 -    if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor;\r
 -  }\r
 -  if(abs(block->speed_e) > max_feedrate[E_AXIS]){\r
 -    float tmp_speed_factor = max_feedrate[E_AXIS] / abs(block->speed_e);\r
 -    if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor;\r
 -  }\r
 -  multiplier = multiplier * speed_factor;\r
 -  block->speed_z = delta_z_mm * multiplier; \r
 -  block->speed_x = delta_x_mm * multiplier;\r
 -  block->speed_y = delta_y_mm * multiplier;\r
 -  block->speed_e = delta_e_mm * multiplier; \r
 -  block->nominal_speed = block->millimeters * multiplier;\r
 -  block->nominal_rate = ceil(block->step_event_count * multiplier / 60);  \r
 -\r
 -  if(block->nominal_rate < 120) \r
 -    block->nominal_rate = 120;\r
 -  block->entry_speed = safe_speed(block);\r
 -\r
 -  // Compute the acceleration rate for the trapezoid generator. \r
 -  float travel_per_step = block->millimeters/block->step_event_count;\r
 -  if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0) {\r
 -    block->acceleration_st = ceil( (retract_acceleration)/travel_per_step); // convert to: acceleration steps/sec^2\r
 -  }\r
 -  else {\r
 -    block->acceleration_st = ceil( (acceleration)/travel_per_step);      // convert to: acceleration steps/sec^2\r
 -    float tmp_acceleration = (float)block->acceleration_st / (float)block->step_event_count;\r
 -    // Limit acceleration per axis\r
 -    if((tmp_acceleration * block->steps_x) > axis_steps_per_sqr_second[X_AXIS]) {\r
 -      block->acceleration_st = axis_steps_per_sqr_second[X_AXIS];\r
 -      tmp_acceleration = (float)block->acceleration_st / (float)block->step_event_count;\r
 -    }\r
 -    if((tmp_acceleration * block->steps_y) > axis_steps_per_sqr_second[Y_AXIS]) {\r
 -      block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS];\r
 -      tmp_acceleration = (float)block->acceleration_st / (float)block->step_event_count;\r
 -    }\r
 -    if((tmp_acceleration * block->steps_e) > axis_steps_per_sqr_second[E_AXIS]) {\r
 -      block->acceleration_st = axis_steps_per_sqr_second[E_AXIS];\r
 -      tmp_acceleration = (float)block->acceleration_st / (float)block->step_event_count;\r
 -    }\r
 -    if((tmp_acceleration * block->steps_z) > axis_steps_per_sqr_second[Z_AXIS]) {\r
 -      block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS];\r
 -      tmp_acceleration = (float)block->acceleration_st / (float)block->step_event_count;\r
 -    }\r
 -  }\r
 -  block->acceleration = block->acceleration_st * travel_per_step;\r
 -  block->acceleration_rate = (long)((float)block->acceleration_st * 8.388608);\r
 -\r
 -  #ifdef ADVANCE\r
 -    // Calculate advance rate\r
 -    if((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) {\r
 -      block->advance_rate = 0;\r
 -      block->advance = 0;\r
 -    }\r
 -    else {\r
 -      long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);\r
 -      float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) * \r
 -        (block->speed_e * block->speed_e * EXTRUTION_AREA * EXTRUTION_AREA / 3600.0)*65536;\r
 -      block->advance = advance;\r
 -      if(acc_dist == 0) {\r
 -        block->advance_rate = 0;\r
 -      } \r
 -      else {\r
 -        block->advance_rate = advance / (float)acc_dist;\r
 -      }\r
 -    }\r
 -  #endif // ADVANCE\r
 -\r
 -  // compute a preliminary conservative acceleration trapezoid\r
 -  float safespeed = safe_speed(block);\r
 -  calculate_trapezoid_for_block(block, safespeed, safespeed); \r
 -\r
 -  // Compute direction bits for this block\r
 -  block->direction_bits = 0;\r
 -  if (target[X_AXIS] < position[X_AXIS]) { \r
 -    block->direction_bits |= (1<<X_AXIS); \r
 -  }\r
 -  if (target[Y_AXIS] < position[Y_AXIS]) { \r
 -    block->direction_bits |= (1<<Y_AXIS); \r
 -  }\r
 -  if (target[Z_AXIS] < position[Z_AXIS]) { \r
 -    block->direction_bits |= (1<<Z_AXIS); \r
 -  }\r
 -  if (target[E_AXIS] < position[E_AXIS]) { \r
 -    block->direction_bits |= (1<<E_AXIS); \r
 -  }\r
 -\r
 -  // Move buffer head\r
 -  block_buffer_head = next_buffer_head;     \r
 -\r
 -  // Update position \r
 -  memcpy(position, target, sizeof(target)); // position[] = target[]\r
 -\r
 -  planner_recalculate();\r
 -  #ifdef AUTOTEMP\r
 -    getHighESpeed();\r
 -  #endif\r
 -  st_wake_up();\r
 -}\r
 -\r
 -void plan_set_position(const float &x, const float &y, const float &z, const float &e)\r
 -{\r
 -  position[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);\r
 -  position[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);\r
 -  position[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);     \r
 -  position[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);     \r
 -}\r
 -\r
 +/*
 +  planner.c - buffers movement commands and manages the acceleration profile plan
 +  Part of Grbl
 +
 +  Copyright (c) 2009-2011 Simen Svale Skogsrud
 +
 +  Grbl is free software: you can redistribute it and/or modify
 +  it under the terms of the GNU General Public License as published by
 +  the Free Software Foundation, either version 3 of the License, or
 +  (at your option) any later version.
 +
 +  Grbl is distributed in the hope that it will be useful,
 +  but WITHOUT ANY WARRANTY; without even the implied warranty of
 +  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 +  GNU General Public License for more details.
 +
 +  You should have received a copy of the GNU General Public License
 +  along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
 +*/
 +
 +/* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */
 +
 +/*  
 +  Reasoning behind the mathematics in this module (in the key of 'Mathematica'):
 +  
 +  s == speed, a == acceleration, t == time, d == distance
 +
 +  Basic definitions:
 +
 +    Speed[s_, a_, t_] := s + (a*t) 
 +    Travel[s_, a_, t_] := Integrate[Speed[s, a, t], t]
 +
 +  Distance to reach a specific speed with a constant acceleration:
 +
 +    Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, d, t]
 +      d -> (m^2 - s^2)/(2 a) --> estimate_acceleration_distance()
 +
 +  Speed after a given distance of travel with constant acceleration:
 +
 +    Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, m, t]
 +      m -> Sqrt[2 a d + s^2]    
 +
 +    DestinationSpeed[s_, a_, d_] := Sqrt[2 a d + s^2]
 +
 +  When to start braking (di) to reach a specified destionation speed (s2) after accelerating
 +  from initial speed s1 without ever stopping at a plateau:
 +
 +    Solve[{DestinationSpeed[s1, a, di] == DestinationSpeed[s2, a, d - di]}, di]
 +      di -> (2 a d - s1^2 + s2^2)/(4 a) --> intersection_distance()
 +
 +    IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a)
 +*/
 +                                                                                                            
 +
 +//#include <inttypes.h>
 +//#include <math.h>       
 +//#include <stdlib.h>
 +
 +#include "Marlin.h"
 +#include "Configuration.h"
 +#include "pins.h"
 +#include "fastio.h"
 +#include "planner.h"
 +#include "stepper.h"
 +#include "temperature.h"
 +#include "ultralcd.h"
 +
 +//===========================================================================
 +//=============================public variables ============================
 +//===========================================================================
 +
 +unsigned long minsegmenttime;
 +float max_feedrate[4]; // set the max speeds
 +float axis_steps_per_unit[4];
 +long max_acceleration_units_per_sq_second[4]; // Use M201 to override by software
 +float minimumfeedrate;
 +float acceleration;         // Normal acceleration mm/s^2  THIS IS THE DEFAULT ACCELERATION for all moves. M204 SXXXX
 +float retract_acceleration; //  mm/s^2   filament pull-pack and push-forward  while standing still in the other axis M204 TXXXX
 +float max_xy_jerk; //speed than can be stopped at once, if i understand correctly.
 +float max_z_jerk;
 +float mintravelfeedrate;
 +unsigned long axis_steps_per_sqr_second[NUM_AXIS];
 +
 +// The current position of the tool in absolute steps
 +long position[4];   //rescaled from extern when axis_steps_per_unit are changed by gcode
 +static float previous_speed[4]; // Speed of previous path line segment
 +static float previous_nominal_speed; // Nominal speed of previous path line segment
 +
++#ifdef AUTOTEMP
++float high_e_speed=0;
++#endif
++
 +
 +//===========================================================================
 +//=============================private variables ============================
 +//===========================================================================
 +static block_t block_buffer[BLOCK_BUFFER_SIZE];            // A ring buffer for motion instfructions
 +static volatile unsigned char block_buffer_head;           // Index of the next block to be pushed
 +static volatile unsigned char block_buffer_tail;           // Index of the block to process now
 +
 +// Used for the frequency limit
 +static unsigned char old_direction_bits = 0;               // Old direction bits. Used for speed calculations
 +static long x_segment_time[3]={0,0,0};                     // Segment times (in us). Used for speed calculations
 +static long y_segment_time[3]={0,0,0};
 +
 +// Returns the index of the next block in the ring buffer
 +// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication.
 +static int8_t next_block_index(int8_t block_index) {
 +  block_index++;
 +  if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; }
 +  return(block_index);
 +}
 +
 +
 +// Returns the index of the previous block in the ring buffer
 +static int8_t prev_block_index(int8_t block_index) {
 +  if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; }
 +  block_index--;
 +  return(block_index);
 +}
 +
 +//===========================================================================
 +//=============================functions         ============================
 +//===========================================================================
 +
 +// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the 
 +// given acceleration:
 +inline float estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration) {
 +  if (acceleration!=0) {
 +  return((target_rate*target_rate-initial_rate*initial_rate)/
 +         (2.0*acceleration));
 +  }
 +  else {
 +    return 0.0;  // acceleration was 0, set acceleration distance to 0
 +  }
 +}
 +
 +// This function gives you the point at which you must start braking (at the rate of -acceleration) if 
 +// you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
 +// a total travel of distance. This can be used to compute the intersection point between acceleration and
 +// deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
 +
 +inline float intersection_distance(float initial_rate, float final_rate, float acceleration, float distance) {
 + if (acceleration!=0) {
 +  return((2.0*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/
 +         (4.0*acceleration) );
 +  }
 +  else {
 +    return 0.0;  // acceleration was 0, set intersection distance to 0
 +  }
 +}
 +
 +// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
 +
 +void calculate_trapezoid_for_block(block_t *block, float entry_factor, float exit_factor) {
 +  long initial_rate = ceil(block->nominal_rate*entry_factor); // (step/min)
 +  long final_rate = ceil(block->nominal_rate*exit_factor); // (step/min)
 +
 +  // Limit minimal step rate (Otherwise the timer will overflow.)
 +  if(initial_rate <120) {initial_rate=120; }
 +  if(final_rate < 120) {final_rate=120;  }
 +  
 +  long acceleration = block->acceleration_st;
 +  int32_t accelerate_steps =
 +    ceil(estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration));
 +  int32_t decelerate_steps =
 +    floor(estimate_acceleration_distance(block->nominal_rate, block->final_rate, -acceleration));
 +    
 +  // Calculate the size of Plateau of Nominal Rate.
 +  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
 +  // in order to reach the final_rate exactly at the end of this block.
 +  if (plateau_steps < 0) {
 +    accelerate_steps = ceil(
 +      intersection_distance(block->initial_rate, block->final_rate, acceleration, block->step_event_count));
 +    accelerate_steps = max(accelerate_steps,0); // Check limits due to numerical round-off
 +    accelerate_steps = min(accelerate_steps,block->step_event_count);
 +    plateau_steps = 0;
 +  }
 +
 +  #ifdef ADVANCE
 +    long initial_advance = block->advance*entry_factor*entry_factor;
 +    long final_advance = block->advance*exit_factor*exit_factor;
 +  #endif // ADVANCE
 +  
 + // block->accelerate_until = accelerate_steps;
 + // block->decelerate_after = accelerate_steps+plateau_steps;
 +  
 +  CRITICAL_SECTION_START;  // Fill variables used by the stepper in a critical section
 +  if(block->busy == false) { // Don't update variables if block is busy.
 +    block->accelerate_until = accelerate_steps;
 +    block->decelerate_after = accelerate_steps+plateau_steps;
 +    block->initial_rate = initial_rate;
 +    block->final_rate = final_rate;
 +  #ifdef ADVANCE
 +      block->initial_advance = initial_advance;
 +      block->final_advance = final_advance;
 +  #endif //ADVANCE
 +  }
 +  CRITICAL_SECTION_END;
 +}                    
 +
 +// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the 
 +// acceleration within the allotted distance.
 +inline float max_allowable_speed(float acceleration, float target_velocity, float distance) {
 +  return  sqrt(target_velocity*target_velocity-2*acceleration*distance);
 +}
 +
 +// "Junction jerk" in this context is the immediate change in speed at the junction of two blocks.
 +// This method will calculate the junction jerk as the euclidean distance between the nominal 
 +// velocities of the respective blocks.
 +//inline float junction_jerk(block_t *before, block_t *after) {
 +//  return sqrt(
 +//    pow((before->speed_x-after->speed_x), 2)+pow((before->speed_y-after->speed_y), 2));
 +//}
 +
 +
 +// The kernel called by planner_recalculate() when scanning the plan from last to first entry.
 +void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) {
 +  if(!current) { return; }
 +  
 +    if (next) {
 +    // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
 +    // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
 +    // check for maximum allowable speed reductions to ensure maximum possible planned speed.
 +    if (current->entry_speed != current->max_entry_speed) {
 +    
 +      // If nominal length true, max junction speed is guaranteed to be reached. Only compute
 +      // for max allowable speed if block is decelerating and nominal length is false.
 +      if ((!current->nominal_length_flag) && (current->max_entry_speed > next->entry_speed)) {
 +        current->entry_speed = min( current->max_entry_speed,
 +          max_allowable_speed(-current->acceleration,next->entry_speed,current->millimeters));
 +      } else {
 +        current->entry_speed = current->max_entry_speed;
 +      }
 +      current->recalculate_flag = true;
 +    
 +    }
 +  } // Skip last block. Already initialized and set for recalculation.
 +}
 +
 +// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This 
 +// implements the reverse pass.
 +void planner_reverse_pass() {
 +  char block_index = block_buffer_head;
 +  if(((block_buffer_head-block_buffer_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) { 
 +      block_index = prev_block_index(block_index); 
 +      block[2]= block[1];
 +      block[1]= block[0];
 +      block[0] = &block_buffer[block_index];
 +      planner_reverse_pass_kernel(block[0], block[1], block[2]);
 +    }
 +  }
 +}
 +
 +// The kernel called by planner_recalculate() when scanning the plan from first to last entry.
 +void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) {
 +  if(!previous) { return; }
 +  
 +  // If the previous block is an acceleration block, but it is not long enough to complete the
 +  // full speed change within the block, we need to adjust the entry speed accordingly. Entry
 +  // speeds have already been reset, maximized, and reverse planned by reverse planner.
 +  // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
 +  if (!previous->nominal_length_flag) {
 +    if (previous->entry_speed < current->entry_speed) {
 +      double entry_speed = min( current->entry_speed,
 +        max_allowable_speed(-previous->acceleration,previous->entry_speed,previous->millimeters) );
 +
 +      // Check for junction speed change
 +      if (current->entry_speed != entry_speed) {
 +        current->entry_speed = entry_speed;
 +        current->recalculate_flag = true;
 +      }
 +    }
 +  }
 +}
 +
 +// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This 
 +// implements the forward pass.
 +void planner_forward_pass() {
 +  char block_index = block_buffer_tail;
 +  block_t *block[3] = { NULL, NULL, NULL };
 +
 +  while(block_index != block_buffer_head) {
 +    block[0] = block[1];
 +    block[1] = block[2];
 +    block[2] = &block_buffer[block_index];
 +    planner_forward_pass_kernel(block[0],block[1],block[2]);
 +    block_index = next_block_index(block_index);
 +  }
 +  planner_forward_pass_kernel(block[1], block[2], NULL);
 +}
 +
 +// Recalculates the trapezoid speed profiles for all blocks in the plan according to the 
 +// entry_factor for each junction. Must be called by planner_recalculate() after 
 +// updating the blocks.
 +void planner_recalculate_trapezoids() {
 +  int8_t block_index = block_buffer_tail;
 +  block_t *current;
 +  block_t *next = NULL;
 +  
 +  while(block_index != block_buffer_head) {
 +    current = next;
 +    next = &block_buffer[block_index];
 +    if (current) {
 +      // Recalculate if current block entry or exit junction speed has changed.
 +      if (current->recalculate_flag || next->recalculate_flag) {
 +        // NOTE: Entry and exit factors always > 0 by all previous logic operations.
 +        calculate_trapezoid_for_block(current, current->entry_speed/current->nominal_speed,
 +          next->entry_speed/current->nominal_speed);
 +        current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
 +      }
 +    }
 +    block_index = next_block_index( block_index );
 +  }
 +  // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
 +  if(next != NULL) {
 +    calculate_trapezoid_for_block(next, next->entry_speed/next->nominal_speed,
 +      MINIMUM_PLANNER_SPEED/next->nominal_speed);
 +    next->recalculate_flag = false;
 +  }
 +}
 +
 +// Recalculates the motion plan according to the following algorithm:
 +//
 +//   1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_factor) 
 +//      so that:
 +//     a. The junction jerk is within the set limit
 +//     b. No speed reduction within one block requires faster deceleration than the one, true constant 
 +//        acceleration.
 +//   2. Go over every block in chronological order and dial down junction speed reduction values if 
 +//     a. The speed increase within one block would require faster accelleration than the one, true 
 +//        constant acceleration.
 +//
 +// When these stages are complete all blocks have an entry_factor that will allow all speed changes to 
 +// be performed using only the one, true constant acceleration, and where no junction jerk is jerkier than 
 +// the set limit. Finally it will:
 +//
 +//   3. Recalculate trapezoids for all blocks.
 +
 +void planner_recalculate() {   
 +  planner_reverse_pass();
 +  planner_forward_pass();
 +  planner_recalculate_trapezoids();
 +}
 +
 +void plan_init() {
 +  block_buffer_head = 0;
 +  block_buffer_tail = 0;
 +  memset(position, 0, sizeof(position)); // clear position
 +  previous_speed[0] = 0.0;
 +  previous_speed[1] = 0.0;
 +  previous_speed[2] = 0.0;
 +  previous_speed[3] = 0.0;
 +  previous_nominal_speed = 0.0;
 +}
 +
 +
 +void plan_discard_current_block() {
 +  if (block_buffer_head != block_buffer_tail) {
 +    block_buffer_tail = (block_buffer_tail + 1) & (BLOCK_BUFFER_SIZE - 1);  
 +  }
 +}
 +
 +block_t *plan_get_current_block() {
 +  if (block_buffer_head == block_buffer_tail) { 
 +    return(NULL); 
 +  }
 +  block_t *block = &block_buffer[block_buffer_tail];
 +  block->busy = true;
 +  return(block);
 +}
 +
++#ifdef AUTOTEMP
++void getHighESpeed()
++{
++  if(degTargetHotend0()+2<AUTOTEMP_MIN)  //probably temperature set to zero.
++    return; //do nothing
++  float high=0;
++  char block_index = block_buffer_tail;
++  
++  while(block_index != block_buffer_head) {
++    float se=block_buffer[block_index].speed_e;
++    if(se>high)
++    {
++      high=se;
++    }
++    block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1);
++  }
++  high_e_speed=high*axis_steps_per_unit[E_AXIS]/(1000000.0);  //so it is independent of the esteps/mm. before 
++   
++  float g=AUTOTEMP_MIN+high_e_speed*AUTOTEMP_FACTOR;
++  float t=constrain(AUTOTEMP_MIN,g,AUTOTEMP_MAX);
++  setTargetHotend0(t);
++  SERIAL_ECHO_START;
++  SERIAL_ECHOPAIR("highe",high_e_speed);
++  SERIAL_ECHOPAIR(" t",t);
++  SERIAL_ECHOLN("");
++}
++#endif
++
 +void check_axes_activity() {
 +  unsigned char x_active = 0;
 +  unsigned char y_active = 0;  
 +  unsigned char z_active = 0;
 +  unsigned char e_active = 0;
 +  block_t *block;
 +
 +  if(block_buffer_tail != block_buffer_head) {
 +    char block_index = block_buffer_tail;
 +    while(block_index != block_buffer_head) {
 +      block = &block_buffer[block_index];
 +      if(block->steps_x != 0) x_active++;
 +      if(block->steps_y != 0) y_active++;
 +      if(block->steps_z != 0) z_active++;
 +      if(block->steps_e != 0) e_active++;
 +      block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1);
 +    }
 +  }
 +  if((DISABLE_X) && (x_active == 0)) disable_x();
 +  if((DISABLE_Y) && (y_active == 0)) disable_y();
 +  if((DISABLE_Z) && (z_active == 0)) disable_z();
 +  if((DISABLE_E) && (e_active == 0)) disable_e();
 +}
 +
 +
 +float junction_deviation = 0.1;
 +// 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.
 +void plan_buffer_line(const float &x, const float &y, const float &z, const float &e,  float feed_rate)
 +{
 +  // Calculate the buffer head after we push this byte
 +  int next_buffer_head = next_block_index(block_buffer_head);
 +
 +  // If the buffer is full: good! That means we are well ahead of the robot. 
 +  // Rest here until there is room in the buffer.
 +  while(block_buffer_tail == next_buffer_head) { 
 +    manage_heater(); 
 +    manage_inactivity(1); 
 +    LCD_STATUS;
 +  }
 +
 +  // The target position of the tool in absolute steps
 +  // Calculate target position in absolute steps
 +  //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
 +  long target[4];
 +  target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
 +  target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
 +  target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);     
 +  target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]); 
 +  
 +  // Prepare to set up new block
 +  block_t *block = &block_buffer[block_buffer_head];
 +  
 +  // Mark block as not busy (Not executed by the stepper interrupt)
 +  block->busy = false;
 +
 +  // Number of steps for each axis
 +  block->steps_x = labs(target[X_AXIS]-position[X_AXIS]);
 +  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->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
 +  if (block->step_event_count <=dropsegments) { return; };
 +
 +  // Compute direction bits for this block
 +  block->direction_bits = 0;
 +  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); }
 +  
 +  //enable active axes
 +  if(block->steps_x != 0) enable_x();
 +  if(block->steps_y != 0) enable_y();
 +  if(block->steps_z != 0) enable_z();
 +  if(block->steps_e != 0) enable_e();
 +
 +  float delta_mm[4];
 +  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];
 +  block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) +
 +                            square(delta_mm[Z_AXIS]));
 +  float inverse_millimeters = 1.0/block->millimeters;  // Inverse millimeters to remove multiple divides 
 +  
 +  // Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
 +  float inverse_second = feed_rate * inverse_millimeters;
 +  
 +  block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
 +  block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0
 +
 +//  unsigned long microseconds;
 +#if 0
 +  if (block->steps_e == 0) {
 +        if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;
 +  }
 +  else {
 +      if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;
 +  } 
 +
 +  microseconds = lround((block->millimeters/feed_rate)*1000000);
 +
 +  // slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill
 +  // reduces/removes corner blobs as the machine won't come to a full stop.
 +  int blockcount=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1);
 +  
 +  if ((blockcount>0) && (blockcount < (BLOCK_BUFFER_SIZE - 4))) {
 +    if (microseconds<minsegmenttime)  { // buffer is draining, add extra time.  The amount of time added increases if the buffer is still emptied more.
 +        microseconds=microseconds+lround(2*(minsegmenttime-microseconds)/blockcount);
 +    }
 +  }
 +  else {
 +    if (microseconds<minsegmenttime) microseconds=minsegmenttime;
 +  }
 +  //  END OF SLOW DOWN SECTION  
 +#endif  
 +
 + // Calculate 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++) {
 +    if(abs(current_speed[i]) > max_feedrate[i])
 +      speed_factor = min(speed_factor, max_feedrate[i] / abs(current_speed[i]));
 +  }
 +
 +// Max segement time in us.
 +
 +#ifdef XY_FREQUENCY_LIMIT
 +#define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT)
 +
 +  // Check and limit the xy direction change frequency
 +  unsigned char direction_change = block->direction_bits ^ old_direction_bits;
 +  old_direction_bits = block->direction_bits;
 +  long segment_time = lround(1000000.0/inverse_second);
 +  if((direction_change & (1<<X_AXIS)) == 0) {
 +     x_segment_time[0] += segment_time;
 +  }
 +  else {
 +    x_segment_time[2] = x_segment_time[1];
 +    x_segment_time[1] = x_segment_time[0];
 +    x_segment_time[0] = segment_time;
 +  }
 +  if((direction_change & (1<<Y_AXIS)) == 0) {
 +     y_segment_time[0] += segment_time;
 +  }
 +  else {
 +    y_segment_time[2] = y_segment_time[1];
 +    y_segment_time[1] = y_segment_time[0];
 +    y_segment_time[0] = segment_time;
 +  }
 +  long max_x_segment_time = max(x_segment_time[0], max(x_segment_time[1], x_segment_time[2]));
 +  long max_y_segment_time = max(y_segment_time[0], max(y_segment_time[1], y_segment_time[2]));
 +  long min_xy_segment_time =min(max_x_segment_time, max_y_segment_time);
 +  if(min_xy_segment_time < MAX_FREQ_TIME) speed_factor = min(speed_factor, (float)min_xy_segment_time / (float)MAX_FREQ_TIME);
 +#endif
 +
 +
 +  // Correct the speed  
 +  if( speed_factor < 1.0) {
 +//    Serial.print("speed factor : "); Serial.println(speed_factor);
 +    for(int i=0; i < 4; i++) {
 +    if(abs(current_speed[i]) > max_feedrate[i])
 +      speed_factor = min(speed_factor, max_feedrate[i] / abs(current_speed[i]));
 +//      Serial.print("current_speed"); Serial.print(i); Serial.print(" : "); Serial.println(current_speed[i]);
 +  }
 +    for(unsigned char i=0; i < 4; i++) {
 +      current_speed[i] *= speed_factor;
 +    }
 +    block->nominal_speed *= speed_factor;
 +    block->nominal_rate *= speed_factor;
 +  }
 +
 +  // Compute and limit the acceleration rate for the trapezoid generator.  
 +  float steps_per_mm = block->step_event_count/block->millimeters;
 +  if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0) {
 +    block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
 +  }
 +  else {
 +    block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
 +    // Limit acceleration per axis
 +    if(((float)block->acceleration_st * (float)block->steps_x / (float)block->step_event_count) > axis_steps_per_sqr_second[X_AXIS])
 +      block->acceleration_st = axis_steps_per_sqr_second[X_AXIS];
 +    if(((float)block->acceleration_st * (float)block->steps_y / (float)block->step_event_count) > axis_steps_per_sqr_second[Y_AXIS])
 +      block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS];
 +    if(((float)block->acceleration_st * (float)block->steps_e / (float)block->step_event_count) > axis_steps_per_sqr_second[E_AXIS])
 +      block->acceleration_st = axis_steps_per_sqr_second[E_AXIS];
 +    if(((float)block->acceleration_st * (float)block->steps_z / (float)block->step_event_count ) > axis_steps_per_sqr_second[Z_AXIS])
 +      block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS];
 +  }
 +  block->acceleration = block->acceleration_st / steps_per_mm;
 +  block->acceleration_rate = (long)((float)block->acceleration_st * 8.388608);
 +  
 +#if 0  // Use old jerk for now
 +  // Compute path unit vector
 +  double unit_vec[3];
 +
 +  unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
 +  unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
 +  unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;
 +  
 +  // Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
 +  // Let a circle be tangent to both previous and current path line segments, where the junction
 +  // deviation is defined as the distance from the junction to the closest edge of the circle,
 +  // colinear with the circle center. The circular segment joining the two paths represents the
 +  // path of centripetal acceleration. Solve for max velocity based on max acceleration about the
 +  // radius of the circle, defined indirectly by junction deviation. This may be also viewed as
 +  // path width or max_jerk in the previous grbl version. This approach does not actually deviate
 +  // from path, but used as a robust way to compute cornering speeds, as it takes into account the
 +  // nonlinearities of both the junction angle and junction velocity.
 +  double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
 +
 +  // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
 +  if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
 +    // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
 +    // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
 +    double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
 +                       - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
 +                       - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
 +                           
 +    // Skip and use default max junction speed for 0 degree acute junction.
 +    if (cos_theta < 0.95) {
 +      vmax_junction = min(previous_nominal_speed,block->nominal_speed);
 +      // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
 +      if (cos_theta > -0.95) {
 +        // Compute maximum junction velocity based on maximum acceleration and junction deviation
 +        double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
 +        vmax_junction = min(vmax_junction,
 +          sqrt(block->acceleration * junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
 +      }
 +    }
 +  }
 +#endif
 +  // Start with a safe speed
 +  float vmax_junction = max_xy_jerk/2;  
 +  if(abs(current_speed[Z_AXIS]) > max_z_jerk/2) 
 +    vmax_junction = max_z_jerk/2;
 +  vmax_junction = min(vmax_junction, block->nominal_speed);
 +
 +  if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
 +    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 (jerk > max_xy_jerk) {
 +      vmax_junction *= (max_xy_jerk/jerk);
 +    } 
 +    if(abs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk) {
 +      vmax_junction *= (max_z_jerk/abs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]));
 +    } 
 +  }
 +  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);
 +
 +  // Initialize planner efficiency flags
 +  // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
 +  // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
 +  // the current block and next block junction speeds are guaranteed to always be at their maximum
 +  // junction speeds in deceleration and acceleration, respectively. This is due to how the current
 +  // 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; }
 +  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;
 +  
 +  #ifdef ADVANCE
 +    // Calculate advance rate
 +    if((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) {
 +      block->advance_rate = 0;
 +      block->advance = 0;
 +    }
 +    else {
 +      long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);
 +      float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) * 
 +        (block->speed_e * block->speed_e * EXTRUTION_AREA * EXTRUTION_AREA / 3600.0)*65536;
 +      block->advance = advance;
 +      if(acc_dist == 0) {
 +        block->advance_rate = 0;
 +      } 
 +      else {
 +        block->advance_rate = advance / (float)acc_dist;
 +      }
 +    }
 +  #endif // ADVANCE
 +
 +
 +
 +
 +  calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed,
 +    MINIMUM_PLANNER_SPEED/block->nominal_speed);
 +    
 +  // Move buffer head
 +  block_buffer_head = next_buffer_head;
 +  
 +  // Update position
 +  memcpy(position, target, sizeof(target)); // position[] = target[]
 +
 +  planner_recalculate();
-   
++  #ifdef AUTOTEMP
++    getHighESpeed();
++  #endif
 +  st_wake_up();
 +}
 +
 +void plan_set_position(const float &x, const float &y, const float &z, const float &e)
 +{
 +  position[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
 +  position[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
 +  position[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);     
 +  position[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);  
 +  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;
 +}
\r
++
index 40f214ebfbb5ce16079abde61bf1068258c3b46d,c5bc5b8aaf456d8f7bc90ad6d2e01bc33037a19c..f5c01ea2668ce0056c8f8707e91cded88e62a4af
@@@ -1,96 -1,95 +1,98 @@@
 -/*\r
 -  planner.h - buffers movement commands and manages the acceleration profile plan\r
 -  Part of Grbl\r
 -\r
 -  Copyright (c) 2009-2011 Simen Svale Skogsrud\r
 -\r
 -  Grbl is free software: you can redistribute it and/or modify\r
 -  it under the terms of the GNU General Public License as published by\r
 -  the Free Software Foundation, either version 3 of the License, or\r
 -  (at your option) any later version.\r
 -\r
 -  Grbl is distributed in the hope that it will be useful,\r
 -  but WITHOUT ANY WARRANTY; without even the implied warranty of\r
 -  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
 -  GNU General Public License for more details.\r
 -\r
 -  You should have received a copy of the GNU General Public License\r
 -  along with Grbl.  If not, see <http://www.gnu.org/licenses/>.\r
 -*/\r
 -\r
 -// This module is to be considered a sub-module of stepper.c. Please don't include \r
 -// this file from any other module.\r
 -\r
 -#ifndef planner_h\r
 -#define planner_h\r
 -\r
 -#include "Configuration.h"\r
 -\r
 -// This struct is used when buffering the setup for each linear movement "nominal" values are as specified in \r
 -// the source g-code and may never actually be reached if acceleration management is active.\r
 -typedef struct {\r
 -  // Fields used by the bresenham algorithm for tracing the line\r
 -  long steps_x, steps_y, steps_z, steps_e;  // Step count along each axis\r
 -  long step_event_count;                    // The number of step events required to complete this block\r
 -  volatile long accelerate_until;           // The index of the step event on which to stop acceleration\r
 -  volatile long decelerate_after;           // The index of the step event on which to start decelerating\r
 -  volatile long acceleration_rate;          // The acceleration rate used for acceleration calculation\r
 -  unsigned char direction_bits;             // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)\r
 -  #ifdef ADVANCE\r
 -    long advance_rate;\r
 -    volatile long initial_advance;\r
 -    volatile long final_advance;\r
 -    float advance;\r
 -  #endif\r
 -\r
 -  // Fields used by the motion planner to manage acceleration\r
 -  float speed_x, speed_y, speed_z, speed_e;          // Nominal mm/minute for each axis\r
 -  float nominal_speed;                               // The nominal speed for this block in mm/min  \r
 -  float millimeters;                                 // The total travel of this block in mm\r
 -  float entry_speed;\r
 -  float acceleration;                                // acceleration mm/sec^2\r
 -\r
 -  // Settings for the trapezoid generator\r
 -  long nominal_rate;                                 // The nominal step rate for this block in step_events/sec \r
 -  volatile long initial_rate;                        // The jerk-adjusted step rate at start of block  \r
 -  volatile long final_rate;                          // The minimal rate at exit\r
 -  long acceleration_st;                              // acceleration steps/sec^2\r
 -  volatile char busy;\r
 -} block_t;\r
 -\r
 -// Initialize the motion plan subsystem      \r
 -void plan_init();\r
 -\r
 -// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in \r
 -// millimaters. Feed rate specifies the speed of the motion.\r
 -void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate);\r
 -\r
 -// Set position. Used for G92 instructions.\r
 -void plan_set_position(const float &x, const float &y, const float &z, const float &e);\r
 -\r
 -\r
 -// Called when the current block is no longer needed. Discards the block and makes the memory\r
 -// availible for new blocks.\r
 -void plan_discard_current_block();\r
 -\r
 -// Gets the current block. Returns NULL if buffer empty\r
 -block_t *plan_get_current_block();\r
 -\r
 -void check_axes_activity();\r
 -\r
 -extern unsigned long minsegmenttime;\r
 -extern float max_feedrate[4]; // set the max speeds\r
 -extern float axis_steps_per_unit[4];\r
 -extern long max_acceleration_units_per_sq_second[4]; // Use M201 to override by software\r
 -extern float minimumfeedrate;\r
 -extern float acceleration;         // Normal acceleration mm/s^2  THIS IS THE DEFAULT ACCELERATION for all moves. M204 SXXXX\r
 -extern float retract_acceleration; //  mm/s^2   filament pull-pack and push-forward  while standing still in the other axis M204 TXXXX\r
 -extern float max_xy_jerk; //speed than can be stopped at once, if i understand correctly.\r
 -extern float max_z_jerk;\r
 -extern float mintravelfeedrate;\r
 -extern unsigned long axis_steps_per_sqr_second[NUM_AXIS];\r
 -#ifdef AUTOTEMP\r
 -extern float high_e_speed;\r
 -#endif\r
 -#endif\r
 +/*
 +  planner.h - buffers movement commands and manages the acceleration profile plan
 +  Part of Grbl
 +
 +  Copyright (c) 2009-2011 Simen Svale Skogsrud
 +
 +  Grbl is free software: you can redistribute it and/or modify
 +  it under the terms of the GNU General Public License as published by
 +  the Free Software Foundation, either version 3 of the License, or
 +  (at your option) any later version.
 +
 +  Grbl is distributed in the hope that it will be useful,
 +  but WITHOUT ANY WARRANTY; without even the implied warranty of
 +  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 +  GNU General Public License for more details.
 +
 +  You should have received a copy of the GNU General Public License
 +  along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
 +*/
 +
 +// This module is to be considered a sub-module of stepper.c. Please don't include 
 +// this file from any other module.
 +
 +#ifndef planner_h
 +#define planner_h
 +
 +#include "Configuration.h"
 +
 +// This struct is used when buffering the setup for each linear movement "nominal" values are as specified in 
 +// the source g-code and may never actually be reached if acceleration management is active.
 +typedef struct {
 +  // Fields used by the bresenham algorithm for tracing the line
 +  long steps_x, steps_y, steps_z, steps_e;  // Step count along each axis
 +  long step_event_count;                    // The number of step events required to complete this block
 +  volatile long accelerate_until;           // The index of the step event on which to stop acceleration
 +  volatile long decelerate_after;           // The index of the step event on which to start decelerating
 +  volatile long acceleration_rate;          // The acceleration rate used for acceleration calculation
 +  unsigned char direction_bits;             // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
 +  #ifdef ADVANCE
 +//    long advance_rate;
 +//    volatile long initial_advance;
 +//    volatile long final_advance;
 +//    float advance;
 +  #endif
 +
 +  // Fields used by the motion planner to manage acceleration
 +//  float speed_x, speed_y, speed_z, speed_e;        // Nominal mm/minute for each axis
 +  float nominal_speed;                               // The nominal speed for this block in mm/min  
 +  float entry_speed;                                 // Entry speed at previous-current junction in mm/min
 +  float max_entry_speed;                             // Maximum allowable junction entry speed in mm/min
 +  float millimeters;                                 // The total travel of this block in mm
 +  float acceleration;                                // acceleration mm/sec^2
 +  unsigned char recalculate_flag;                          // Planner flag to recalculate trapezoids on entry junction
 +  unsigned char nominal_length_flag;                       // Planner flag for nominal speed always reached
 +
 +  // Settings for the trapezoid generator
 +  long nominal_rate;                                 // The nominal step rate for this block in step_events/sec 
 +  volatile long initial_rate;                        // The jerk-adjusted step rate at start of block  
 +  volatile long final_rate;                          // The minimal rate at exit
 +  long acceleration_st;                              // acceleration steps/sec^2
 +  volatile char busy;
 +} block_t;
 +
 +// Initialize the motion plan subsystem      
 +void plan_init();
 +
 +// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in 
 +// millimaters. Feed rate specifies the speed of the motion.
 +void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate);
 +
 +// Set position. Used for G92 instructions.
 +void plan_set_position(const float &x, const float &y, const float &z, const float &e);
 +
 +
 +// Called when the current block is no longer needed. Discards the block and makes the memory
 +// availible for new blocks.
 +void plan_discard_current_block();
 +
 +// Gets the current block. Returns NULL if buffer empty
 +block_t *plan_get_current_block();
 +
 +void check_axes_activity();
 +
 +extern unsigned long minsegmenttime;
 +extern float max_feedrate[4]; // set the max speeds
 +extern float axis_steps_per_unit[4];
 +extern long max_acceleration_units_per_sq_second[4]; // Use M201 to override by software
 +extern float minimumfeedrate;
 +extern float acceleration;         // Normal acceleration mm/s^2  THIS IS THE DEFAULT ACCELERATION for all moves. M204 SXXXX
 +extern float retract_acceleration; //  mm/s^2   filament pull-pack and push-forward  while standing still in the other axis M204 TXXXX
 +extern float max_xy_jerk; //speed than can be stopped at once, if i understand correctly.
 +extern float max_z_jerk;
 +extern float mintravelfeedrate;
 +extern unsigned long axis_steps_per_sqr_second[NUM_AXIS];
- #endif\r
++#ifdef AUTOTEMP
++extern float high_e_speed;
++#endif
++#endif
index e50818ca8c6642166942b097eb766c8f3928dcd9,23066ef320c0546952e2ea4993686b356ec29e0b..2f5f3a80d2a69bd9274eec75548c5468b7573cf6
 -/*\r
 -  stepper.c - stepper motor driver: executes motion plans using stepper motors\r
 -  Part of Grbl\r
 -\r
 -  Copyright (c) 2009-2011 Simen Svale Skogsrud\r
 -\r
 -  Grbl is free software: you can redistribute it and/or modify\r
 -  it under the terms of the GNU General Public License as published by\r
 -  the Free Software Foundation, either version 3 of the License, or\r
 -  (at your option) any later version.\r
 -\r
 -  Grbl is distributed in the hope that it will be useful,\r
 -  but WITHOUT ANY WARRANTY; without even the implied warranty of\r
 -  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
 -  GNU General Public License for more details.\r
 -\r
 -  You should have received a copy of the GNU General Public License\r
 -  along with Grbl.  If not, see <http://www.gnu.org/licenses/>.\r
 -*/\r
 -\r
 -/* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith\r
 -   and Philipp Tiefenbacher. */\r
 -\r
 -#include "stepper.h"\r
 -#include "Configuration.h"\r
 -#include "Marlin.h"\r
 -#include "planner.h"\r
 -#include "pins.h"\r
 -#include "fastio.h"\r
 -#include "temperature.h"\r
 -#include "ultralcd.h"\r
 -\r
 -#include "speed_lookuptable.h"\r
 -\r
 -\r
 -\r
 -//===========================================================================\r
 -//=============================public variables  ============================\r
 -//===========================================================================\r
 -block_t *current_block;  // A pointer to the block currently being traced\r
 -\r
 -\r
 -\r
 -//===========================================================================\r
 -//=============================private variables ============================\r
 -//===========================================================================\r
 -//static makes it inpossible to be called from outside of this file by extern.!\r
 -\r
 -// Variables used by The Stepper Driver Interrupt\r
 -static unsigned char out_bits;        // The next stepping-bits to be output\r
 -static long counter_x,       // Counter variables for the bresenham line tracer\r
 -            counter_y, \r
 -            counter_z,       \r
 -            counter_e;\r
 -static unsigned long step_events_completed; // The number of step events executed in the current block\r
 -#ifdef ADVANCE\r
 -  static long advance_rate, advance, final_advance = 0;\r
 -  static short old_advance = 0;\r
 -  static short e_steps;\r
 -#endif\r
 -static unsigned char busy = false; // TRUE when SIG_OUTPUT_COMPARE1A is being serviced. Used to avoid retriggering that handler.\r
 -static long acceleration_time, deceleration_time;\r
 -//static unsigned long accelerate_until, decelerate_after, acceleration_rate, initial_rate, final_rate, nominal_rate;\r
 -static unsigned short acc_step_rate; // needed for deccelaration start point\r
 -static char step_loops;\r
 -\r
 -volatile long endstops_trigsteps[3]={0,0,0};\r
 -volatile long endstops_stepsTotal,endstops_stepsDone;\r
 -static volatile bool endstops_hit=false;\r
 -\r
 -// if DEBUG_STEPS is enabled, M114 can be used to compare two methods of determining the X,Y,Z position of the printer.\r
 -// for debugging purposes only, should be disabled by default\r
 -#ifdef DEBUG_STEPS\r
 -  volatile long count_position[NUM_AXIS] = { 0, 0, 0, 0};\r
 -  volatile int count_direction[NUM_AXIS] = { 1, 1, 1, 1};\r
 -#endif\r
 -\r
 -//===========================================================================\r
 -//=============================functions         ============================\r
 -//===========================================================================\r
 -  \r
 -\r
 -// intRes = intIn1 * intIn2 >> 16\r
 -// uses:\r
 -// r26 to store 0\r
 -// r27 to store the byte 1 of the 24 bit result\r
 -#define MultiU16X8toH16(intRes, charIn1, intIn2) \\r
 -asm volatile ( \\r
 -"clr r26 \n\t" \\r
 -"mul %A1, %B2 \n\t" \\r
 -"movw %A0, r0 \n\t" \\r
 -"mul %A1, %A2 \n\t" \\r
 -"add %A0, r1 \n\t" \\r
 -"adc %B0, r26 \n\t" \\r
 -"lsr r0 \n\t" \\r
 -"adc %A0, r26 \n\t" \\r
 -"adc %B0, r26 \n\t" \\r
 -"clr r1 \n\t" \\r
 -: \\r
 -"=&r" (intRes) \\r
 -: \\r
 -"d" (charIn1), \\r
 -"d" (intIn2) \\r
 -: \\r
 -"r26" \\r
 -)\r
 -\r
 -// intRes = longIn1 * longIn2 >> 24\r
 -// uses:\r
 -// r26 to store 0\r
 -// r27 to store the byte 1 of the 48bit result\r
 -#define MultiU24X24toH16(intRes, longIn1, longIn2) \\r
 -asm volatile ( \\r
 -"clr r26 \n\t" \\r
 -"mul %A1, %B2 \n\t" \\r
 -"mov r27, r1 \n\t" \\r
 -"mul %B1, %C2 \n\t" \\r
 -"movw %A0, r0 \n\t" \\r
 -"mul %C1, %C2 \n\t" \\r
 -"add %B0, r0 \n\t" \\r
 -"mul %C1, %B2 \n\t" \\r
 -"add %A0, r0 \n\t" \\r
 -"adc %B0, r1 \n\t" \\r
 -"mul %A1, %C2 \n\t" \\r
 -"add r27, r0 \n\t" \\r
 -"adc %A0, r1 \n\t" \\r
 -"adc %B0, r26 \n\t" \\r
 -"mul %B1, %B2 \n\t" \\r
 -"add r27, r0 \n\t" \\r
 -"adc %A0, r1 \n\t" \\r
 -"adc %B0, r26 \n\t" \\r
 -"mul %C1, %A2 \n\t" \\r
 -"add r27, r0 \n\t" \\r
 -"adc %A0, r1 \n\t" \\r
 -"adc %B0, r26 \n\t" \\r
 -"mul %B1, %A2 \n\t" \\r
 -"add r27, r1 \n\t" \\r
 -"adc %A0, r26 \n\t" \\r
 -"adc %B0, r26 \n\t" \\r
 -"lsr r27 \n\t" \\r
 -"adc %A0, r26 \n\t" \\r
 -"adc %B0, r26 \n\t" \\r
 -"clr r1 \n\t" \\r
 -: \\r
 -"=&r" (intRes) \\r
 -: \\r
 -"d" (longIn1), \\r
 -"d" (longIn2) \\r
 -: \\r
 -"r26" , "r27" \\r
 -)\r
 -\r
 -// Some useful constants\r
 -\r
 -#define ENABLE_STEPPER_DRIVER_INTERRUPT()  TIMSK1 |= (1<<OCIE1A)\r
 -#define DISABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 &= ~(1<<OCIE1A)\r
 -\r
 -\r
 -void endstops_triggered(const unsigned long &stepstaken)  \r
 -{\r
 -  //this will only work if there is no bufferig\r
 -  //however, if you perform a move at which the endstops should be triggered, and wait for it to complete, i.e. by blocking command, it should work\r
 -  //yes, it uses floats, but: if endstops are triggered, thats hopefully not critical anymore anyways.\r
 -  //endstops_triggerpos;\r
 -  \r
 -  if(endstops_hit) //hitting a second time while the first hit is not reported\r
 -    return;\r
 -  if(current_block == NULL)\r
 -    return;\r
 -  endstops_stepsTotal=current_block->step_event_count;\r
 -  endstops_stepsDone=stepstaken;\r
 -  endstops_trigsteps[0]=current_block->steps_x;\r
 -  endstops_trigsteps[1]=current_block->steps_y;\r
 -  endstops_trigsteps[2]=current_block->steps_z;\r
 -\r
 -  endstops_hit=true;\r
 -}\r
 -\r
 -void checkHitEndstops()\r
 -{\r
 -  if( !endstops_hit)\r
 -   return;\r
 -  float endstops_triggerpos[3]={0,0,0};\r
 -  float ratiodone=endstops_stepsDone/float(endstops_stepsTotal);  //ratio of current_block thas was performed\r
 -  \r
 -  endstops_triggerpos[0]=current_position[0]-(endstops_trigsteps[0]*ratiodone)/float(axis_steps_per_unit[0]);\r
 -  endstops_triggerpos[1]=current_position[1]-(endstops_trigsteps[1]*ratiodone)/float(axis_steps_per_unit[1]);\r
 -  endstops_triggerpos[2]=current_position[2]-(endstops_trigsteps[2]*ratiodone)/float(axis_steps_per_unit[2]);\r
 - SERIAL_ECHO_START;\r
 - SERIAL_ECHOPGM("endstops hit: ");\r
 - SERIAL_ECHOPAIR(" X:",endstops_triggerpos[0]);\r
 - SERIAL_ECHOPAIR(" Y:",endstops_triggerpos[1]);\r
 - SERIAL_ECHOPAIR(" Z:",endstops_triggerpos[2]);\r
 - SERIAL_ECHOLN("");\r
 - endstops_hit=false;\r
 -}\r
 -\r
 -void endstops_hit_on_purpose()\r
 -{\r
 -  endstops_hit=false;\r
 -}\r
 -\r
 -//         __________________________\r
 -//        /|                        |\     _________________         ^\r
 -//       / |                        | \   /|               |\        |\r
 -//      /  |                        |  \ / |               | \       s\r
 -//     /   |                        |   |  |               |  \      p\r
 -//    /    |                        |   |  |               |   \     e\r
 -//   +-----+------------------------+---+--+---------------+----+    e\r
 -//   |               BLOCK 1            |      BLOCK 2          |    d\r
 -//\r
 -//                           time ----->\r
 -// \r
 -//  The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates \r
 -//  first block->accelerate_until step_events_completed, then keeps going at constant speed until \r
 -//  step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset.\r
 -//  The slope of acceleration is calculated with the leib ramp alghorithm.\r
 -\r
 -void st_wake_up() {\r
 -  //  TCNT1 = 0;\r
 -  ENABLE_STEPPER_DRIVER_INTERRUPT();  \r
 -}\r
 -\r
 -inline unsigned short calc_timer(unsigned short step_rate) {\r
 -  unsigned short timer;\r
 -  if(step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY;\r
 -  \r
 -  if(step_rate > 20000) { // If steprate > 20kHz >> step 4 times\r
 -    step_rate = step_rate >> 2;\r
 -    step_loops = 4;\r
 -  }\r
 -  else if(step_rate > 10000) { // If steprate > 10kHz >> step 2 times\r
 -    step_rate = step_rate >> 1;\r
 -    step_loops = 2;\r
 -  }\r
 -  else {\r
 -    step_loops = 1;\r
 -  } \r
 -  \r
 -  if(step_rate < 32) step_rate = 32;\r
 -  step_rate -= 32; // Correct for minimal speed\r
 -  if(step_rate >= (8*256)){ // higher step rate \r
 -    unsigned short table_address = (unsigned short)&speed_lookuptable_fast[(unsigned char)(step_rate>>8)][0];\r
 -    unsigned char tmp_step_rate = (step_rate & 0x00ff);\r
 -    unsigned short gain = (unsigned short)pgm_read_word_near(table_address+2);\r
 -    MultiU16X8toH16(timer, tmp_step_rate, gain);\r
 -    timer = (unsigned short)pgm_read_word_near(table_address) - timer;\r
 -  }\r
 -  else { // lower step rates\r
 -    unsigned short table_address = (unsigned short)&speed_lookuptable_slow[0][0];\r
 -    table_address += ((step_rate)>>1) & 0xfffc;\r
 -    timer = (unsigned short)pgm_read_word_near(table_address);\r
 -    timer -= (((unsigned short)pgm_read_word_near(table_address+2) * (unsigned char)(step_rate & 0x0007))>>3);\r
 -  }\r
 -  if(timer < 100) timer = 100;\r
 -  return timer;\r
 -}\r
 -\r
 -// Initializes the trapezoid generator from the current block. Called whenever a new \r
 -// block begins.\r
 -inline void trapezoid_generator_reset() {\r
 -  #ifdef ADVANCE\r
 -    advance = current_block->initial_advance;\r
 -    final_advance = current_block->final_advance;\r
 -  #endif\r
 -  deceleration_time = 0;\r
 -  // advance_rate = current_block->advance_rate;\r
 -  // step_rate to timer interval\r
 -  acc_step_rate = current_block->initial_rate;\r
 -  acceleration_time = calc_timer(acc_step_rate);\r
 -  OCR1A = acceleration_time;\r
 -}\r
 -\r
 -// "The Stepper Driver Interrupt" - This timer interrupt is the workhorse.  \r
 -// It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately. \r
 -ISR(TIMER1_COMPA_vect)\r
 -{        \r
 -  if(busy){ \r
 -    SERIAL_ERROR_START\r
 -    SERIAL_ERROR(*(unsigned short *)OCR1A);\r
 -    SERIAL_ERRORLNPGM(" ISR overtaking itself.");\r
 -    return; \r
 -  } // The busy-flag is used to avoid reentering this interrupt\r
 -\r
 -  busy = true;\r
 -  sei(); // Re enable interrupts (normally disabled while inside an interrupt handler)\r
 -\r
 -  // If there is no current block, attempt to pop one from the buffer\r
 -  if (current_block == NULL) {\r
 -    // Anything in the buffer?\r
 -    current_block = plan_get_current_block();\r
 -    if (current_block != NULL) {\r
 -      trapezoid_generator_reset();\r
 -      counter_x = -(current_block->step_event_count >> 1);\r
 -      counter_y = counter_x;\r
 -      counter_z = counter_x;\r
 -      counter_e = counter_x;\r
 -      step_events_completed = 0;\r
 -      #ifdef ADVANCE\r
 -      e_steps = 0;\r
 -      #endif\r
 -    } \r
 -    else {\r
 -//      DISABLE_STEPPER_DRIVER_INTERRUPT();\r
 -    }    \r
 -  } \r
 -\r
 -  if (current_block != NULL) {\r
 -    // Set directions TO DO This should be done once during init of trapezoid. Endstops -> interrupt\r
 -    out_bits = current_block->direction_bits;\r
 -\r
 -    #ifdef ADVANCE\r
 -        // Calculate E early.\r
 -        counter_e += current_block->steps_e;\r
 -        if (counter_e > 0) {\r
 -          counter_e -= current_block->step_event_count;\r
 -          if ((out_bits & (1<<E_AXIS)) != 0) { // - direction\r
 -            CRITICAL_SECTION_START;\r
 -            e_steps--;\r
 -            CRITICAL_SECTION_END;\r
 -          }\r
 -          else {\r
 -            CRITICAL_SECTION_START;\r
 -            e_steps++;\r
 -            CRITICAL_SECTION_END;\r
 -          }\r
 -        }    \r
 -        // Do E steps + advance steps\r
 -        CRITICAL_SECTION_START;\r
 -        e_steps += ((advance >> 16) - old_advance);\r
 -        CRITICAL_SECTION_END;\r
 -        old_advance = advance >> 16;  \r
 -    #endif //ADVANCE\r
 -\r
 -    // Set direction en check limit switches\r
 -    if ((out_bits & (1<<X_AXIS)) != 0) {   // -direction\r
 -      WRITE(X_DIR_PIN, INVERT_X_DIR);\r
 -      #ifdef DEBUG_STEPS\r
 -        count_direction[X_AXIS]=-1;\r
 -      #endif\r
 -      #if X_MIN_PIN > -1\r
 -            if(READ(X_MIN_PIN) != ENDSTOPS_INVERTING) {\r
 -              endstops_triggered(step_events_completed);\r
 -              step_events_completed = current_block->step_event_count;\r
 -            }\r
 -      #endif\r
 -    }\r
 -    else { // +direction \r
 -      WRITE(X_DIR_PIN,!INVERT_X_DIR);\r
 -      #ifdef DEBUG_STEPS\r
 -        count_direction[X_AXIS]=1;\r
 -      #endif\r
 -      #if X_MAX_PIN > -1\r
 -        if((READ(X_MAX_PIN) != ENDSTOPS_INVERTING)  && (current_block->steps_x >0)){\r
 -          endstops_triggered(step_events_completed);\r
 -          step_events_completed = current_block->step_event_count;\r
 -        }\r
 -        #endif\r
 -    }\r
 -\r
 -    if ((out_bits & (1<<Y_AXIS)) != 0) {   // -direction\r
 -      WRITE(Y_DIR_PIN,INVERT_Y_DIR);\r
 -      #ifdef DEBUG_STEPS\r
 -        count_direction[Y_AXIS]=-1;\r
 -      #endif\r
 -      #if Y_MIN_PIN > -1\r
 -        if(READ(Y_MIN_PIN) != ENDSTOPS_INVERTING) {\r
 -          endstops_triggered(step_events_completed);\r
 -          step_events_completed = current_block->step_event_count;\r
 -        }\r
 -      #endif\r
 -    }\r
 -    else { // +direction\r
 -    WRITE(Y_DIR_PIN,!INVERT_Y_DIR);\r
 -      #ifdef DEBUG_STEPS\r
 -        count_direction[Y_AXIS]=1;\r
 -      #endif\r
 -      #if Y_MAX_PIN > -1\r
 -      if((READ(Y_MAX_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_y >0)){\r
 -          endstops_triggered(step_events_completed);\r
 -          step_events_completed = current_block->step_event_count;\r
 -        }\r
 -      #endif\r
 -    }\r
 -\r
 -    if ((out_bits & (1<<Z_AXIS)) != 0) {   // -direction\r
 -      WRITE(Z_DIR_PIN,INVERT_Z_DIR);\r
 -      #ifdef DEBUG_STEPS\r
 -      count_direction[Z_AXIS]=-1;\r
 -      #endif\r
 -      #if Z_MIN_PIN > -1\r
 -        if(READ(Z_MIN_PIN) != ENDSTOPS_INVERTING) {\r
 -          endstops_triggered(step_events_completed);\r
 -          step_events_completed = current_block->step_event_count;\r
 -        }\r
 -      #endif\r
 -    }\r
 -    else { // +direction\r
 -      WRITE(Z_DIR_PIN,!INVERT_Z_DIR);\r
 -      #ifdef DEBUG_STEPS\r
 -        count_direction[Z_AXIS]=1;\r
 -      #endif\r
 -      #if Z_MAX_PIN > -1\r
 -        if((READ(Z_MAX_PIN) != ENDSTOPS_INVERTING)  && (current_block->steps_z >0)){\r
 -          endstops_triggered(step_events_completed);\r
 -          step_events_completed = current_block->step_event_count;\r
 -        }\r
 -      #endif\r
 -    }\r
 -\r
 -    #ifndef ADVANCE\r
 -      if ((out_bits & (1<<E_AXIS)) != 0)   // -direction\r
 -        WRITE(E_DIR_PIN,INVERT_E_DIR);\r
 -      else // +direction\r
 -        WRITE(E_DIR_PIN,!INVERT_E_DIR);\r
 -    #endif //!ADVANCE\r
 -\r
 -    for(int8_t i=0; i < step_loops; i++) { // Take multiple steps per interrupt (For high speed moves) \r
 -      counter_x += current_block->steps_x;\r
 -      if (counter_x > 0) {\r
 -        WRITE(X_STEP_PIN, HIGH);\r
 -        counter_x -= current_block->step_event_count;\r
 -        WRITE(X_STEP_PIN, LOW);\r
 -        #ifdef DEBUG_STEPS\r
 -          count_position[X_AXIS]+=count_direction[X_AXIS];   \r
 -        #endif\r
 -      }\r
 -\r
 -      counter_y += current_block->steps_y;\r
 -      if (counter_y > 0) {\r
 -        WRITE(Y_STEP_PIN, HIGH);\r
 -        counter_y -= current_block->step_event_count;\r
 -        WRITE(Y_STEP_PIN, LOW);\r
 -        #ifdef DEBUG_STEPS\r
 -          count_position[Y_AXIS]+=count_direction[Y_AXIS];\r
 -        #endif\r
 -      }\r
 -\r
 -      counter_z += current_block->steps_z;\r
 -      if (counter_z > 0) {\r
 -        WRITE(Z_STEP_PIN, HIGH);\r
 -        counter_z -= current_block->step_event_count;\r
 -        WRITE(Z_STEP_PIN, LOW);\r
 -        #ifdef DEBUG_STEPS\r
 -          count_position[Z_AXIS]+=count_direction[Z_AXIS];\r
 -        #endif\r
 -      }\r
 -\r
 -      #ifndef ADVANCE\r
 -        counter_e += current_block->steps_e;\r
 -        if (counter_e > 0) {\r
 -          WRITE(E_STEP_PIN, HIGH);\r
 -          counter_e -= current_block->step_event_count;\r
 -          WRITE(E_STEP_PIN, LOW);\r
 -        }\r
 -      #endif //!ADVANCE\r
 -      step_events_completed += 1;  \r
 -      if(step_events_completed >= current_block->step_event_count) break;\r
 -    }\r
 -    // Calculare new timer value\r
 -    unsigned short timer;\r
 -    unsigned short step_rate;\r
 -    if (step_events_completed <= current_block->accelerate_until) {\r
 -      MultiU24X24toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);\r
 -      acc_step_rate += current_block->initial_rate;\r
 -      \r
 -      // upper limit\r
 -      if(acc_step_rate > current_block->nominal_rate)\r
 -        acc_step_rate = current_block->nominal_rate;\r
 -\r
 -      // step_rate to timer interval\r
 -      timer = calc_timer(acc_step_rate);\r
 -      #ifdef ADVANCE\r
 -        advance += advance_rate;\r
 -      #endif\r
 -      acceleration_time += timer;\r
 -      OCR1A = timer;\r
 -    } \r
 -    else if (step_events_completed > current_block->decelerate_after) {   \r
 -      MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate);\r
 -      \r
 -      if(step_rate > acc_step_rate) { // Check step_rate stays positive\r
 -        step_rate = current_block->final_rate;\r
 -      }\r
 -      else {\r
 -        step_rate = acc_step_rate - step_rate; // Decelerate from aceleration end point.\r
 -      }\r
 -\r
 -      // lower limit\r
 -      if(step_rate < current_block->final_rate)\r
 -        step_rate = current_block->final_rate;\r
 -\r
 -      // step_rate to timer interval\r
 -      timer = calc_timer(step_rate);\r
 -      #ifdef ADVANCE\r
 -        advance -= advance_rate;\r
 -        if(advance < final_advance)\r
 -          advance = final_advance;\r
 -      #endif //ADVANCE\r
 -      deceleration_time += timer;\r
 -      OCR1A = timer;\r
 -    }       \r
 -    // If current block is finished, reset pointer \r
 -    if (step_events_completed >= current_block->step_event_count) {\r
 -      current_block = NULL;\r
 -      plan_discard_current_block();\r
 -    }   \r
 -  } \r
 -  cli(); // disable interrupts\r
 -  busy=false;\r
 -}\r
 -\r
 -#ifdef ADVANCE\r
 -  unsigned char old_OCR0A;\r
 -  // Timer interrupt for E. e_steps is set in the main routine;\r
 -  // Timer 0 is shared with millies\r
 -  ISR(TIMER0_COMPA_vect)\r
 -  {\r
 -    // Critical section needed because Timer 1 interrupt has higher priority. \r
 -    // The pin set functions are placed on trategic position to comply with the stepper driver timing.\r
 -    WRITE(E_STEP_PIN, LOW);\r
 -    // Set E direction (Depends on E direction + advance)\r
 -    if (e_steps < 0) {\r
 -      WRITE(E_DIR_PIN,INVERT_E_DIR);    \r
 -      e_steps++;\r
 -      WRITE(E_STEP_PIN, HIGH);\r
 -    } \r
 -    if (e_steps > 0) {\r
 -      WRITE(E_DIR_PIN,!INVERT_E_DIR);\r
 -      e_steps--;\r
 -      WRITE(E_STEP_PIN, HIGH);\r
 -    }\r
 -    old_OCR0A += 25; // 10kHz interrupt\r
 -    OCR0A = old_OCR0A;\r
 -  }\r
 -#endif // ADVANCE\r
 -\r
 -void st_init()\r
 -{\r
 -    //Initialize Dir Pins\r
 -  #if X_DIR_PIN > -1\r
 -    SET_OUTPUT(X_DIR_PIN);\r
 -  #endif\r
 -  #if Y_DIR_PIN > -1 \r
 -    SET_OUTPUT(Y_DIR_PIN);\r
 -  #endif\r
 -  #if Z_DIR_PIN > -1 \r
 -    SET_OUTPUT(Z_DIR_PIN);\r
 -  #endif\r
 -  #if E_DIR_PIN > -1 \r
 -    SET_OUTPUT(E_DIR_PIN);\r
 -  #endif\r
 -\r
 -  //Initialize Enable Pins - steppers default to disabled.\r
 -\r
 -  #if (X_ENABLE_PIN > -1)\r
 -    SET_OUTPUT(X_ENABLE_PIN);\r
 -    if(!X_ENABLE_ON) WRITE(X_ENABLE_PIN,HIGH);\r
 -  #endif\r
 -  #if (Y_ENABLE_PIN > -1)\r
 -    SET_OUTPUT(Y_ENABLE_PIN);\r
 -    if(!Y_ENABLE_ON) WRITE(Y_ENABLE_PIN,HIGH);\r
 -  #endif\r
 -  #if (Z_ENABLE_PIN > -1)\r
 -    SET_OUTPUT(Z_ENABLE_PIN);\r
 -    if(!Z_ENABLE_ON) WRITE(Z_ENABLE_PIN,HIGH);\r
 -  #endif\r
 -  #if (E_ENABLE_PIN > -1)\r
 -    SET_OUTPUT(E_ENABLE_PIN);\r
 -    if(!E_ENABLE_ON) WRITE(E_ENABLE_PIN,HIGH);\r
 -  #endif\r
 -\r
 -  //endstops and pullups\r
 -  #ifdef ENDSTOPPULLUPS\r
 -    #if X_MIN_PIN > -1\r
 -      SET_INPUT(X_MIN_PIN); \r
 -      WRITE(X_MIN_PIN,HIGH);\r
 -    #endif\r
 -    #if X_MAX_PIN > -1\r
 -      SET_INPUT(X_MAX_PIN); \r
 -      WRITE(X_MAX_PIN,HIGH);\r
 -    #endif\r
 -    #if Y_MIN_PIN > -1\r
 -      SET_INPUT(Y_MIN_PIN); \r
 -      WRITE(Y_MIN_PIN,HIGH);\r
 -    #endif\r
 -    #if Y_MAX_PIN > -1\r
 -      SET_INPUT(Y_MAX_PIN); \r
 -      WRITE(Y_MAX_PIN,HIGH);\r
 -    #endif\r
 -    #if Z_MIN_PIN > -1\r
 -      SET_INPUT(Z_MIN_PIN); \r
 -      WRITE(Z_MIN_PIN,HIGH);\r
 -    #endif\r
 -    #if Z_MAX_PIN > -1\r
 -      SET_INPUT(Z_MAX_PIN); \r
 -      WRITE(Z_MAX_PIN,HIGH);\r
 -    #endif\r
 -  #else //ENDSTOPPULLUPS\r
 -    #if X_MIN_PIN > -1\r
 -      SET_INPUT(X_MIN_PIN); \r
 -    #endif\r
 -    #if X_MAX_PIN > -1\r
 -      SET_INPUT(X_MAX_PIN); \r
 -    #endif\r
 -    #if Y_MIN_PIN > -1\r
 -      SET_INPUT(Y_MIN_PIN); \r
 -    #endif\r
 -    #if Y_MAX_PIN > -1\r
 -      SET_INPUT(Y_MAX_PIN); \r
 -    #endif\r
 -    #if Z_MIN_PIN > -1\r
 -      SET_INPUT(Z_MIN_PIN); \r
 -    #endif\r
 -    #if Z_MAX_PIN > -1\r
 -      SET_INPUT(Z_MAX_PIN); \r
 -    #endif\r
 -  #endif //ENDSTOPPULLUPS\r
 - \r
 -\r
 -  //Initialize Step Pins\r
 -  #if (X_STEP_PIN > -1) \r
 -    SET_OUTPUT(X_STEP_PIN);\r
 -  #endif  \r
 -  #if (Y_STEP_PIN > -1) \r
 -    SET_OUTPUT(Y_STEP_PIN);\r
 -  #endif  \r
 -  #if (Z_STEP_PIN > -1) \r
 -    SET_OUTPUT(Z_STEP_PIN);\r
 -  #endif  \r
 -  #if (E_STEP_PIN > -1) \r
 -    SET_OUTPUT(E_STEP_PIN);\r
 -  #endif  \r
 -\r
 -  // waveform generation = 0100 = CTC\r
 -  TCCR1B &= ~(1<<WGM13);\r
 -  TCCR1B |=  (1<<WGM12);\r
 -  TCCR1A &= ~(1<<WGM11); \r
 -  TCCR1A &= ~(1<<WGM10);\r
 -\r
 -  // output mode = 00 (disconnected)\r
 -  TCCR1A &= ~(3<<COM1A0); \r
 -  TCCR1A &= ~(3<<COM1B0); \r
 -  TCCR1B = (TCCR1B & ~(0x07<<CS10)) | (2<<CS10); // 2MHz timer\r
 -\r
 -  OCR1A = 0x4000;\r
 -  DISABLE_STEPPER_DRIVER_INTERRUPT();  \r
 -\r
 -  #ifdef ADVANCE\r
 -    e_steps = 0;\r
 -    TIMSK0 |= (1<<OCIE0A);\r
 -  #endif //ADVANCE\r
 -  sei();\r
 -}\r
 -\r
 -// Block until all buffered steps are executed\r
 -void st_synchronize()\r
 -{\r
 -  while(plan_get_current_block()) {\r
 -    manage_heater();\r
 -    manage_inactivity(1);\r
 -    LCD_STATUS;\r
 -  }   \r
 -}\r
 +/*
 +  stepper.c - stepper motor driver: executes motion plans using stepper motors
 +  Part of Grbl
 +
 +  Copyright (c) 2009-2011 Simen Svale Skogsrud
 +
 +  Grbl is free software: you can redistribute it and/or modify
 +  it under the terms of the GNU General Public License as published by
 +  the Free Software Foundation, either version 3 of the License, or
 +  (at your option) any later version.
 +
 +  Grbl is distributed in the hope that it will be useful,
 +  but WITHOUT ANY WARRANTY; without even the implied warranty of
 +  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 +  GNU General Public License for more details.
 +
 +  You should have received a copy of the GNU General Public License
 +  along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
 +*/
 +
 +/* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith
 +   and Philipp Tiefenbacher. */
 +
 +#include "stepper.h"
 +#include "Configuration.h"
 +#include "Marlin.h"
 +#include "planner.h"
 +#include "pins.h"
 +#include "fastio.h"
 +#include "temperature.h"
 +#include "ultralcd.h"
 +
 +#include "speed_lookuptable.h"
 +
 +
++
 +//===========================================================================
 +//=============================public variables  ============================
 +//===========================================================================
 +block_t *current_block;  // A pointer to the block currently being traced
 +
 +
++
 +//===========================================================================
 +//=============================private variables ============================
 +//===========================================================================
 +//static makes it inpossible to be called from outside of this file by extern.!
 +
 +// Variables used by The Stepper Driver Interrupt
 +static unsigned char out_bits;        // The next stepping-bits to be output
 +static long counter_x,       // Counter variables for the bresenham line tracer
 +            counter_y, 
 +            counter_z,       
 +            counter_e;
 +static unsigned long step_events_completed; // The number of step events executed in the current block
 +#ifdef ADVANCE
 +  static long advance_rate, advance, final_advance = 0;
 +  static short old_advance = 0;
 +  static short e_steps;
 +#endif
 +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 long accelerate_until, decelerate_after, acceleration_rate, initial_rate, final_rate, nominal_rate;
 +static unsigned short acc_step_rate; // needed for deccelaration start point
 +static char step_loops;
 +
++volatile long endstops_trigsteps[3]={0,0,0};
++volatile long endstops_stepsTotal,endstops_stepsDone;
++static volatile bool endstops_hit=false;
 +
 +// if DEBUG_STEPS is enabled, M114 can be used to compare two methods of determining the X,Y,Z position of the printer.
 +// for debugging purposes only, should be disabled by default
 +#ifdef DEBUG_STEPS
 +  volatile long count_position[NUM_AXIS] = { 0, 0, 0, 0};
 +  volatile int count_direction[NUM_AXIS] = { 1, 1, 1, 1};
 +#endif
 +
 +//===========================================================================
 +//=============================functions         ============================
 +//===========================================================================
 +  
 +
 +// intRes = intIn1 * intIn2 >> 16
 +// uses:
 +// r26 to store 0
 +// r27 to store the byte 1 of the 24 bit result
 +#define MultiU16X8toH16(intRes, charIn1, intIn2) \
 +asm volatile ( \
 +"clr r26 \n\t" \
 +"mul %A1, %B2 \n\t" \
 +"movw %A0, r0 \n\t" \
 +"mul %A1, %A2 \n\t" \
 +"add %A0, r1 \n\t" \
 +"adc %B0, r26 \n\t" \
 +"lsr r0 \n\t" \
 +"adc %A0, r26 \n\t" \
 +"adc %B0, r26 \n\t" \
 +"clr r1 \n\t" \
 +: \
 +"=&r" (intRes) \
 +: \
 +"d" (charIn1), \
 +"d" (intIn2) \
 +: \
 +"r26" \
 +)
 +
 +// intRes = longIn1 * longIn2 >> 24
 +// uses:
 +// r26 to store 0
 +// r27 to store the byte 1 of the 48bit result
 +#define MultiU24X24toH16(intRes, longIn1, longIn2) \
 +asm volatile ( \
 +"clr r26 \n\t" \
 +"mul %A1, %B2 \n\t" \
 +"mov r27, r1 \n\t" \
 +"mul %B1, %C2 \n\t" \
 +"movw %A0, r0 \n\t" \
 +"mul %C1, %C2 \n\t" \
 +"add %B0, r0 \n\t" \
 +"mul %C1, %B2 \n\t" \
 +"add %A0, r0 \n\t" \
 +"adc %B0, r1 \n\t" \
 +"mul %A1, %C2 \n\t" \
 +"add r27, r0 \n\t" \
 +"adc %A0, r1 \n\t" \
 +"adc %B0, r26 \n\t" \
 +"mul %B1, %B2 \n\t" \
 +"add r27, r0 \n\t" \
 +"adc %A0, r1 \n\t" \
 +"adc %B0, r26 \n\t" \
 +"mul %C1, %A2 \n\t" \
 +"add r27, r0 \n\t" \
 +"adc %A0, r1 \n\t" \
 +"adc %B0, r26 \n\t" \
 +"mul %B1, %A2 \n\t" \
 +"add r27, r1 \n\t" \
 +"adc %A0, r26 \n\t" \
 +"adc %B0, r26 \n\t" \
 +"lsr r27 \n\t" \
 +"adc %A0, r26 \n\t" \
 +"adc %B0, r26 \n\t" \
 +"clr r1 \n\t" \
 +: \
 +"=&r" (intRes) \
 +: \
 +"d" (longIn1), \
 +"d" (longIn2) \
 +: \
 +"r26" , "r27" \
 +)
 +
 +// Some useful constants
 +
 +#define ENABLE_STEPPER_DRIVER_INTERRUPT()  TIMSK1 |= (1<<OCIE1A)
 +#define DISABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 &= ~(1<<OCIE1A)
 +
 +
++void endstops_triggered(const unsigned long &stepstaken)  
++{
++  //this will only work if there is no bufferig
++  //however, if you perform a move at which the endstops should be triggered, and wait for it to complete, i.e. by blocking command, it should work
++  //yes, it uses floats, but: if endstops are triggered, thats hopefully not critical anymore anyways.
++  //endstops_triggerpos;
++  
++  if(endstops_hit) //hitting a second time while the first hit is not reported
++    return;
++  if(current_block == NULL)
++    return;
++  endstops_stepsTotal=current_block->step_event_count;
++  endstops_stepsDone=stepstaken;
++  endstops_trigsteps[0]=current_block->steps_x;
++  endstops_trigsteps[1]=current_block->steps_y;
++  endstops_trigsteps[2]=current_block->steps_z;
++
++  endstops_hit=true;
++}
 +
++void checkHitEndstops()
++{
++  if( !endstops_hit)
++   return;
++  float endstops_triggerpos[3]={0,0,0};
++  float ratiodone=endstops_stepsDone/float(endstops_stepsTotal);  //ratio of current_block thas was performed
++  
++  endstops_triggerpos[0]=current_position[0]-(endstops_trigsteps[0]*ratiodone)/float(axis_steps_per_unit[0]);
++  endstops_triggerpos[1]=current_position[1]-(endstops_trigsteps[1]*ratiodone)/float(axis_steps_per_unit[1]);
++  endstops_triggerpos[2]=current_position[2]-(endstops_trigsteps[2]*ratiodone)/float(axis_steps_per_unit[2]);
++ SERIAL_ECHO_START;
++ SERIAL_ECHOPGM("endstops hit: ");
++ SERIAL_ECHOPAIR(" X:",endstops_triggerpos[0]);
++ SERIAL_ECHOPAIR(" Y:",endstops_triggerpos[1]);
++ SERIAL_ECHOPAIR(" Z:",endstops_triggerpos[2]);
++ SERIAL_ECHOLN("");
++ endstops_hit=false;
++}
 +
++void endstops_hit_on_purpose()
++{
++  endstops_hit=false;
++}
 +
 +//         __________________________
 +//        /|                        |\     _________________         ^
 +//       / |                        | \   /|               |\        |
 +//      /  |                        |  \ / |               | \       s
 +//     /   |                        |   |  |               |  \      p
 +//    /    |                        |   |  |               |   \     e
 +//   +-----+------------------------+---+--+---------------+----+    e
 +//   |               BLOCK 1            |      BLOCK 2          |    d
 +//
 +//                           time ----->
 +// 
 +//  The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates 
 +//  first block->accelerate_until step_events_completed, then keeps going at constant speed until 
 +//  step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset.
 +//  The slope of acceleration is calculated with the leib ramp alghorithm.
 +
 +void st_wake_up() {
 +  //  TCNT1 = 0;
 +  if(busy == false) 
 +  ENABLE_STEPPER_DRIVER_INTERRUPT();  
 +}
 +
 +inline unsigned short calc_timer(unsigned short step_rate) {
 +  unsigned short timer;
 +  if(step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY;
 +  
 +  if(step_rate > 20000) { // If steprate > 20kHz >> step 4 times
 +    step_rate = step_rate >> 2;
 +    step_loops = 4;
 +  }
 +  else if(step_rate > 10000) { // If steprate > 10kHz >> step 2 times
 +    step_rate = step_rate >> 1;
 +    step_loops = 2;
 +  }
 +  else {
 +    step_loops = 1;
 +  } 
 +  
 +  if(step_rate < 32) step_rate = 32;
 +  step_rate -= 32; // Correct for minimal speed
 +  if(step_rate >= (8*256)){ // higher step rate 
 +    unsigned short table_address = (unsigned short)&speed_lookuptable_fast[(unsigned char)(step_rate>>8)][0];
 +    unsigned char tmp_step_rate = (step_rate & 0x00ff);
 +    unsigned short gain = (unsigned short)pgm_read_word_near(table_address+2);
 +    MultiU16X8toH16(timer, tmp_step_rate, gain);
 +    timer = (unsigned short)pgm_read_word_near(table_address) - timer;
 +  }
 +  else { // lower step rates
 +    unsigned short table_address = (unsigned short)&speed_lookuptable_slow[0][0];
 +    table_address += ((step_rate)>>1) & 0xfffc;
 +    timer = (unsigned short)pgm_read_word_near(table_address);
 +    timer -= (((unsigned short)pgm_read_word_near(table_address+2) * (unsigned char)(step_rate & 0x0007))>>3);
 +  }
 +  //if(timer < 100) timer = 100;
 +  return timer;
 +}
 +
 +// Initializes the trapezoid generator from the current block. Called whenever a new 
 +// block begins.
 +inline void trapezoid_generator_reset() {
 +  #ifdef ADVANCE
 +    advance = current_block->initial_advance;
 +    final_advance = current_block->final_advance;
 +  #endif
 +  deceleration_time = 0;
 +  // step_rate to timer interval
 +  acc_step_rate = current_block->initial_rate;
 +  acceleration_time = calc_timer(acc_step_rate);
 +  OCR1A = acceleration_time;
 +}
 +
 +// "The Stepper Driver Interrupt" - This timer interrupt is the workhorse.  
 +// It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately. 
 +ISR(TIMER1_COMPA_vect)
 +{        
 +  if(busy){ 
- /*    SERIAL_ERRORLN(*(unsigned short *)OCR1A<< " ISR overtaking itself.");*/
++    SERIAL_ERROR_START
++    SERIAL_ERROR(*(unsigned short *)OCR1A);
++    SERIAL_ERRORLNPGM(" ISR overtaking itself.");
 +    return; 
 +  } // The busy-flag is used to avoid reentering this interrupt
 +
 +  busy = true;
 +  sei(); // Re enable interrupts (normally disabled while inside an interrupt handler)
 +
 +  // If there is no current block, attempt to pop one from the buffer
 +  if (current_block == NULL) {
 +    // Anything in the buffer?
 +    current_block = plan_get_current_block();
 +    if (current_block != NULL) {
 +      trapezoid_generator_reset();
 +      counter_x = -(current_block->step_event_count >> 1);
 +      counter_y = counter_x;
 +      counter_z = counter_x;
 +      counter_e = counter_x;
 +      step_events_completed = 0;
 +      #ifdef ADVANCE
 +      e_steps = 0;
 +      #endif
 +    } 
 +    else {
 +//      DISABLE_STEPPER_DRIVER_INTERRUPT();
 +    }    
 +  } 
 +
 +  if (current_block != NULL) {
 +    // Set directions TO DO This should be done once during init of trapezoid. Endstops -> interrupt
 +    out_bits = current_block->direction_bits;
 +
 +    #ifdef ADVANCE
 +        // Calculate E early.
 +        counter_e += current_block->steps_e;
 +        if (counter_e > 0) {
 +          counter_e -= current_block->step_event_count;
 +          if ((out_bits & (1<<E_AXIS)) != 0) { // - direction
 +            CRITICAL_SECTION_START;
 +            e_steps--;
 +            CRITICAL_SECTION_END;
 +          }
 +          else {
 +            CRITICAL_SECTION_START;
 +            e_steps++;
 +            CRITICAL_SECTION_END;
 +          }
 +        }    
 +        // Do E steps + advance steps
 +        CRITICAL_SECTION_START;
 +        e_steps += ((advance >> 16) - old_advance);
 +        CRITICAL_SECTION_END;
 +        old_advance = advance >> 16;  
 +    #endif //ADVANCE
 +
 +    // Set direction en check limit switches
 +    if ((out_bits & (1<<X_AXIS)) != 0) {   // -direction
 +      WRITE(X_DIR_PIN, INVERT_X_DIR);
 +      #ifdef DEBUG_STEPS
 +        count_direction[X_AXIS]=-1;
 +      #endif
 +      #if X_MIN_PIN > -1
 +            if(READ(X_MIN_PIN) != ENDSTOPS_INVERTING) {
++              endstops_triggered(step_events_completed);
 +              step_events_completed = current_block->step_event_count;
 +            }
 +      #endif
 +    }
 +    else { // +direction 
 +      WRITE(X_DIR_PIN,!INVERT_X_DIR);
 +      #ifdef DEBUG_STEPS
 +        count_direction[X_AXIS]=1;
 +      #endif
 +      #if X_MAX_PIN > -1
 +        if((READ(X_MAX_PIN) != ENDSTOPS_INVERTING)  && (current_block->steps_x >0)){
++          endstops_triggered(step_events_completed);
 +          step_events_completed = current_block->step_event_count;
 +        }
 +        #endif
 +    }
 +
 +    if ((out_bits & (1<<Y_AXIS)) != 0) {   // -direction
 +      WRITE(Y_DIR_PIN,INVERT_Y_DIR);
 +      #ifdef DEBUG_STEPS
 +        count_direction[Y_AXIS]=-1;
 +      #endif
 +      #if Y_MIN_PIN > -1
 +        if(READ(Y_MIN_PIN) != ENDSTOPS_INVERTING) {
++          endstops_triggered(step_events_completed);
 +          step_events_completed = current_block->step_event_count;
 +        }
 +      #endif
 +    }
 +    else { // +direction
 +    WRITE(Y_DIR_PIN,!INVERT_Y_DIR);
 +      #ifdef DEBUG_STEPS
 +        count_direction[Y_AXIS]=1;
 +      #endif
 +      #if Y_MAX_PIN > -1
 +      if((READ(Y_MAX_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_y >0)){
++          endstops_triggered(step_events_completed);
 +          step_events_completed = current_block->step_event_count;
 +        }
 +      #endif
 +    }
 +
 +    if ((out_bits & (1<<Z_AXIS)) != 0) {   // -direction
 +      WRITE(Z_DIR_PIN,INVERT_Z_DIR);
 +      #ifdef DEBUG_STEPS
 +      count_direction[Z_AXIS]=-1;
 +      #endif
 +      #if Z_MIN_PIN > -1
 +        if(READ(Z_MIN_PIN) != ENDSTOPS_INVERTING) {
++          endstops_triggered(step_events_completed);
 +          step_events_completed = current_block->step_event_count;
 +        }
 +      #endif
 +    }
 +    else { // +direction
 +      WRITE(Z_DIR_PIN,!INVERT_Z_DIR);
 +      #ifdef DEBUG_STEPS
 +        count_direction[Z_AXIS]=1;
 +      #endif
 +      #if Z_MAX_PIN > -1
 +        if((READ(Z_MAX_PIN) != ENDSTOPS_INVERTING)  && (current_block->steps_z >0)){
++          endstops_triggered(step_events_completed);
 +          step_events_completed = current_block->step_event_count;
 +        }
 +      #endif
 +    }
 +
 +    #ifndef ADVANCE
 +      if ((out_bits & (1<<E_AXIS)) != 0)   // -direction
 +        WRITE(E_DIR_PIN,INVERT_E_DIR);
 +      else // +direction
 +        WRITE(E_DIR_PIN,!INVERT_E_DIR);
 +    #endif //!ADVANCE
 +
 +    for(int8_t i=0; i < step_loops; i++) { // Take multiple steps per interrupt (For high speed moves) 
 +      counter_x += current_block->steps_x;
 +      if (counter_x > 0) {
 +        WRITE(X_STEP_PIN, HIGH);
 +        counter_x -= current_block->step_event_count;
 +        WRITE(X_STEP_PIN, LOW);
 +        #ifdef DEBUG_STEPS
 +          count_position[X_AXIS]+=count_direction[X_AXIS];   
 +        #endif
 +      }
 +
 +      counter_y += current_block->steps_y;
 +      if (counter_y > 0) {
 +        WRITE(Y_STEP_PIN, HIGH);
 +        counter_y -= current_block->step_event_count;
 +        WRITE(Y_STEP_PIN, LOW);
 +        #ifdef DEBUG_STEPS
 +          count_position[Y_AXIS]+=count_direction[Y_AXIS];
 +        #endif
 +      }
 +
 +      counter_z += current_block->steps_z;
 +      if (counter_z > 0) {
 +        WRITE(Z_STEP_PIN, HIGH);
 +        counter_z -= current_block->step_event_count;
 +        WRITE(Z_STEP_PIN, LOW);
 +        #ifdef DEBUG_STEPS
 +          count_position[Z_AXIS]+=count_direction[Z_AXIS];
 +        #endif
 +      }
 +
 +      #ifndef ADVANCE
 +        counter_e += current_block->steps_e;
 +        if (counter_e > 0) {
 +          WRITE(E_STEP_PIN, HIGH);
 +          counter_e -= current_block->step_event_count;
 +          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;
 +    unsigned short step_rate;
 +    if (step_events_completed <= current_block->accelerate_until) {
 +      MultiU24X24toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
 +      acc_step_rate += current_block->initial_rate;
 +      
 +      // upper limit
 +      if(acc_step_rate > current_block->nominal_rate)
 +        acc_step_rate = current_block->nominal_rate;
 +
 +      // step_rate to timer interval
 +      timer = calc_timer(acc_step_rate);
 +      #ifdef ADVANCE
 +        advance += advance_rate;
 +      #endif
 +      acceleration_time += timer;
 +      OCR1A = timer;
 +    } 
 +    else if (step_events_completed > current_block->decelerate_after) {   
 +      MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate);
 +      
 +      if(step_rate > acc_step_rate) { // Check step_rate stays positive
 +        step_rate = current_block->final_rate;
 +      }
 +      else {
 +        step_rate = acc_step_rate - step_rate; // Decelerate from aceleration end point.
 +      }
 +
 +      // lower limit
 +      if(step_rate < current_block->final_rate)
 +        step_rate = current_block->final_rate;
 +
 +      // step_rate to timer interval
 +      timer = calc_timer(step_rate);
 +      #ifdef ADVANCE
 +        advance -= advance_rate;
 +        if(advance < final_advance)
 +          advance = final_advance;
 +      #endif //ADVANCE
 +      deceleration_time += timer;
 +      OCR1A = timer;
 +    }
 +    else {
 +      timer = calc_timer(current_block->nominal_rate);
 +      OCR1A = timer;
 +    }
 +    
 +    // If current block is finished, reset pointer 
 +    if (step_events_completed >= current_block->step_event_count) {
 +      current_block = NULL;
 +      plan_discard_current_block();
 +    }   
 +  } 
 +  cli(); // disable interrupts
 +  busy=false;
 +}
 +
 +#ifdef ADVANCE
 +  unsigned char old_OCR0A;
 +  // Timer interrupt for E. e_steps is set in the main routine;
 +  // Timer 0 is shared with millies
 +  ISR(TIMER0_COMPA_vect)
 +  {
 +    // Critical section needed because Timer 1 interrupt has higher priority. 
 +    // The pin set functions are placed on trategic position to comply with the stepper driver timing.
 +    WRITE(E_STEP_PIN, LOW);
 +    // Set E direction (Depends on E direction + advance)
 +    if (e_steps < 0) {
 +      WRITE(E_DIR_PIN,INVERT_E_DIR);    
 +      e_steps++;
 +      WRITE(E_STEP_PIN, HIGH);
 +    } 
 +    if (e_steps > 0) {
 +      WRITE(E_DIR_PIN,!INVERT_E_DIR);
 +      e_steps--;
 +      WRITE(E_STEP_PIN, HIGH);
 +    }
 +    old_OCR0A += 25; // 10kHz interrupt
 +    OCR0A = old_OCR0A;
 +  }
 +#endif // ADVANCE
 +
 +void st_init()
 +{
 +    //Initialize Dir Pins
 +  #if X_DIR_PIN > -1
 +    SET_OUTPUT(X_DIR_PIN);
 +  #endif
 +  #if Y_DIR_PIN > -1 
 +    SET_OUTPUT(Y_DIR_PIN);
 +  #endif
 +  #if Z_DIR_PIN > -1 
 +    SET_OUTPUT(Z_DIR_PIN);
 +  #endif
 +  #if E_DIR_PIN > -1 
 +    SET_OUTPUT(E_DIR_PIN);
 +  #endif
 +
 +  //Initialize Enable Pins - steppers default to disabled.
 +
 +  #if (X_ENABLE_PIN > -1)
 +    SET_OUTPUT(X_ENABLE_PIN);
 +    if(!X_ENABLE_ON) WRITE(X_ENABLE_PIN,HIGH);
 +  #endif
 +  #if (Y_ENABLE_PIN > -1)
 +    SET_OUTPUT(Y_ENABLE_PIN);
 +    if(!Y_ENABLE_ON) WRITE(Y_ENABLE_PIN,HIGH);
 +  #endif
 +  #if (Z_ENABLE_PIN > -1)
 +    SET_OUTPUT(Z_ENABLE_PIN);
 +    if(!Z_ENABLE_ON) WRITE(Z_ENABLE_PIN,HIGH);
 +  #endif
 +  #if (E_ENABLE_PIN > -1)
 +    SET_OUTPUT(E_ENABLE_PIN);
 +    if(!E_ENABLE_ON) WRITE(E_ENABLE_PIN,HIGH);
 +  #endif
 +
 +  //endstops and pullups
 +  #ifdef ENDSTOPPULLUPS
 +    #if X_MIN_PIN > -1
 +      SET_INPUT(X_MIN_PIN); 
 +      WRITE(X_MIN_PIN,HIGH);
 +    #endif
 +    #if X_MAX_PIN > -1
 +      SET_INPUT(X_MAX_PIN); 
 +      WRITE(X_MAX_PIN,HIGH);
 +    #endif
 +    #if Y_MIN_PIN > -1
 +      SET_INPUT(Y_MIN_PIN); 
 +      WRITE(Y_MIN_PIN,HIGH);
 +    #endif
 +    #if Y_MAX_PIN > -1
 +      SET_INPUT(Y_MAX_PIN); 
 +      WRITE(Y_MAX_PIN,HIGH);
 +    #endif
 +    #if Z_MIN_PIN > -1
 +      SET_INPUT(Z_MIN_PIN); 
 +      WRITE(Z_MIN_PIN,HIGH);
 +    #endif
 +    #if Z_MAX_PIN > -1
 +      SET_INPUT(Z_MAX_PIN); 
 +      WRITE(Z_MAX_PIN,HIGH);
 +    #endif
 +  #else //ENDSTOPPULLUPS
 +    #if X_MIN_PIN > -1
 +      SET_INPUT(X_MIN_PIN); 
 +    #endif
 +    #if X_MAX_PIN > -1
 +      SET_INPUT(X_MAX_PIN); 
 +    #endif
 +    #if Y_MIN_PIN > -1
 +      SET_INPUT(Y_MIN_PIN); 
 +    #endif
 +    #if Y_MAX_PIN > -1
 +      SET_INPUT(Y_MAX_PIN); 
 +    #endif
 +    #if Z_MIN_PIN > -1
 +      SET_INPUT(Z_MIN_PIN); 
 +    #endif
 +    #if Z_MAX_PIN > -1
 +      SET_INPUT(Z_MAX_PIN); 
 +    #endif
 +  #endif //ENDSTOPPULLUPS
 + 
 +
 +  //Initialize Step Pins
 +  #if (X_STEP_PIN > -1) 
 +    SET_OUTPUT(X_STEP_PIN);
 +  #endif  
 +  #if (Y_STEP_PIN > -1) 
 +    SET_OUTPUT(Y_STEP_PIN);
 +  #endif  
 +  #if (Z_STEP_PIN > -1) 
 +    SET_OUTPUT(Z_STEP_PIN);
 +  #endif  
 +  #if (E_STEP_PIN > -1) 
 +    SET_OUTPUT(E_STEP_PIN);
 +  #endif  
 +
 +  // waveform generation = 0100 = CTC
 +  TCCR1B &= ~(1<<WGM13);
 +  TCCR1B |=  (1<<WGM12);
 +  TCCR1A &= ~(1<<WGM11); 
 +  TCCR1A &= ~(1<<WGM10);
 +
 +  // output mode = 00 (disconnected)
 +  TCCR1A &= ~(3<<COM1A0); 
 +  TCCR1A &= ~(3<<COM1B0); 
 +  TCCR1B = (TCCR1B & ~(0x07<<CS10)) | (2<<CS10); // 2MHz timer
 +
 +  OCR1A = 0x4000;
 +  DISABLE_STEPPER_DRIVER_INTERRUPT();  
 +
 +  #ifdef ADVANCE
 +    e_steps = 0;
 +    TIMSK0 |= (1<<OCIE0A);
 +  #endif //ADVANCE
 +  sei();
 +}
 +
 +// Block until all buffered steps are executed
 +void st_synchronize()
 +{
 +  while(plan_get_current_block()) {
 +    manage_heater();
 +    manage_inactivity(1);
 +    LCD_STATUS;
 +  }   
- }\r
++}
index a39cc407ae890dc71e1071630ee77b08eb463425,8ea8ee008ff9dba460f027409c3bf01e84138c32..3e6edeec9acf267199abfb7abb7e9642f4bdff92
 -/*\r
 -  temperature.c - temperature control\r
 -  Part of Marlin\r
 -  \r
 - Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm\r
 - \r
 - This program is free software: you can redistribute it and/or modify\r
 - it under the terms of the GNU General Public License as published by\r
 - the Free Software Foundation, either version 3 of the License, or\r
 - (at your option) any later version.\r
 - \r
 - This program is distributed in the hope that it will be useful,\r
 - but WITHOUT ANY WARRANTY; without even the implied warranty of\r
 - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
 - GNU General Public License for more details.\r
 - \r
 - You should have received a copy of the GNU General Public License\r
 - along with this program.  If not, see <http://www.gnu.org/licenses/>.\r
 - */\r
 -\r
 -/*\r
 - This firmware is a mashup between Sprinter and grbl.\r
 -  (https://github.com/kliment/Sprinter)\r
 -  (https://github.com/simen/grbl/tree)\r
 - \r
 - It has preliminary support for Matthew Roberts advance algorithm \r
 -    http://reprap.org/pipermail/reprap-dev/2011-May/003323.html\r
 -\r
 - This firmware is optimized for gen6 electronics.\r
 - */\r
 -\r
 -#include "fastio.h"\r
 -#include "Configuration.h"\r
 -#include "pins.h"\r
 -#include "Marlin.h"\r
 -#include "ultralcd.h"\r
 -#include "temperature.h"\r
 -#include "watchdog.h"\r
 -\r
 -//===========================================================================\r
 -//=============================public variables============================\r
 -//===========================================================================\r
 -int target_raw[3] = {0, 0, 0};\r
 -int current_raw[3] = {0, 0, 0};\r
 -\r
 -#ifdef PIDTEMP\r
 -  \r
 -  // probably used external\r
 -  float HeaterPower;\r
 -  float pid_setpoint = 0.0;\r
 -\r
 -  \r
 -  float Kp=DEFAULT_Kp;\r
 -  float Ki=DEFAULT_Ki;\r
 -  float Kd=DEFAULT_Kd;\r
 -  float Kc=DEFAULT_Kc;\r
 -#endif //PIDTEMP\r
 -  \r
 -  \r
 -//===========================================================================\r
 -//=============================private variables============================\r
 -//===========================================================================\r
 -static bool temp_meas_ready = false;\r
 -\r
 -static unsigned long previous_millis_heater, previous_millis_bed_heater;\r
 -\r
 -#ifdef PIDTEMP\r
 -  //static cannot be external:\r
 -  static float temp_iState = 0;\r
 -  static float temp_dState = 0;\r
 -  static float pTerm;\r
 -  static float iTerm;\r
 -  static float dTerm;\r
 -  //int output;\r
 -  static float pid_error;\r
 -  static float temp_iState_min;\r
 -  static float temp_iState_max;\r
 -  static float pid_input;\r
 -  static float pid_output;\r
 -  static bool pid_reset;\r
 - \r
 -#endif //PIDTEMP\r
 -  \r
 -#ifdef WATCHPERIOD\r
 -  static int watch_raw[3] = {-1000,-1000,-1000};\r
 -  static unsigned long watchmillis = 0;\r
 -#endif //WATCHPERIOD\r
 -\r
 -#ifdef HEATER_0_MINTEMP\r
 -  static int minttemp_0 = temp2analog(HEATER_0_MINTEMP);\r
 -#endif //MINTEMP\r
 -#ifdef HEATER_0_MAXTEMP\r
 -  static int maxttemp_0 = temp2analog(HEATER_0_MAXTEMP);\r
 -#endif //MAXTEMP\r
 -\r
 -#ifdef HEATER_1_MINTEMP\r
 -  static int minttemp_1 = temp2analog(HEATER_1_MINTEMP);\r
 -#endif //MINTEMP\r
 -#ifdef HEATER_1_MAXTEMP\r
 -  static int maxttemp_1 = temp2analog(HEATER_1_MAXTEMP);\r
 -#endif //MAXTEMP\r
 -\r
 -#ifdef BED_MINTEMP\r
 -  static int bed_minttemp = temp2analog(BED_MINTEMP);\r
 -#endif //BED_MINTEMP\r
 -#ifdef BED_MAXTEMP\r
 -  static int bed_maxttemp = temp2analog(BED_MAXTEMP);\r
 -#endif //BED_MAXTEMP\r
 -\r
 -//===========================================================================\r
 -//=============================functions         ============================\r
 -//===========================================================================\r
 -  \r
 -void manage_heater()\r
 -{\r
 -  #ifdef USE_WATCHDOG\r
 -    wd_reset();\r
 -  #endif\r
 -  \r
 -  float pid_input;\r
 -  float pid_output;\r
 -  if(temp_meas_ready != true)   //better readability\r
 -    return; \r
 -\r
 -  CRITICAL_SECTION_START;\r
 -    temp_meas_ready = false;\r
 -  CRITICAL_SECTION_END;\r
 -\r
 -  #ifdef PIDTEMP\r
 -    pid_input = analog2temp(current_raw[TEMPSENSOR_HOTEND_0]);\r
 -\r
 -    #ifndef PID_OPENLOOP\r
 -        pid_error = pid_setpoint - pid_input;\r
 -        if(pid_error > 10){\r
 -          pid_output = PID_MAX;\r
 -          pid_reset = true;\r
 -        }\r
 -        else if(pid_error < -10) {\r
 -          pid_output = 0;\r
 -          pid_reset = true;\r
 -        }\r
 -        else {\r
 -          if(pid_reset == true) {\r
 -            temp_iState = 0.0;\r
 -            pid_reset = false;\r
 -          }\r
 -          pTerm = Kp * pid_error;\r
 -          temp_iState += pid_error;\r
 -          temp_iState = constrain(temp_iState, temp_iState_min, temp_iState_max);\r
 -          iTerm = Ki * temp_iState;\r
 -          //K1 defined in Configuration.h in the PID settings\r
 -          #define K2 (1.0-K1)\r
 -          dTerm = (Kd * (pid_input - temp_dState))*K2 + (K1 * dTerm);\r
 -          temp_dState = pid_input;\r
 -          #ifdef PID_ADD_EXTRUSION_RATE\r
 -            pTerm+=Kc*current_block->speed_e; //additional heating if extrusion speed is high\r
 -          #endif\r
 -          pid_output = constrain(pTerm + iTerm - dTerm, 0, PID_MAX);\r
 -          \r
 -        }\r
 -    #endif //PID_OPENLOOP\r
 -    #ifdef PID_DEBUG\r
 -     //SERIAL_ECHOLN(" PIDDEBUG Input "<<pid_input<<" Output "<<pid_output" pTerm "<<pTerm<<" iTerm "<<iTerm<<" dTerm "<<dTerm);  \r
 -    #endif //PID_DEBUG\r
 -    HeaterPower=pid_output;\r
 -    analogWrite(HEATER_0_PIN, pid_output);\r
 -  #endif //PIDTEMP\r
 -\r
 -  #ifndef PIDTEMP\r
 -    if(current_raw[0] >= target_raw[0])\r
 -    {\r
 -      WRITE(HEATER_0_PIN,LOW);\r
 -    }\r
 -    else \r
 -    {\r
 -      WRITE(HEATER_0_PIN,HIGH);\r
 -    }\r
 -  #endif\r
 -    \r
 -  if(millis() - previous_millis_bed_heater < BED_CHECK_INTERVAL)\r
 -    return;\r
 -  previous_millis_bed_heater = millis();\r
 -  \r
 -  #if TEMP_1_PIN > -1\r
 -    if(current_raw[TEMPSENSOR_BED] >= target_raw[TEMPSENSOR_BED])\r
 -    {\r
 -      WRITE(HEATER_1_PIN,LOW);\r
 -    }\r
 -    else \r
 -    {\r
 -      WRITE(HEATER_1_PIN,HIGH);\r
 -    }\r
 -  #endif\r
 -}\r
 -\r
 -// Takes hot end temperature value as input and returns corresponding raw value. \r
 -// For a thermistor, it uses the RepRap thermistor temp table.\r
 -// This is needed because PID in hydra firmware hovers around a given analog value, not a temp value.\r
 -// This function is derived from inversing the logic from a portion of getTemperature() in FiveD RepRap firmware.\r
 -int temp2analog(int celsius) {\r
 -  #ifdef HEATER_0_USES_THERMISTOR\r
 -    int raw = 0;\r
 -    byte i;\r
 -    \r
 -    for (i=1; i<NUMTEMPS_HEATER_0; i++)\r
 -    {\r
 -      if (heater_0_temptable[i][1] < celsius)\r
 -      {\r
 -        raw = heater_0_temptable[i-1][0] + \r
 -          (celsius - heater_0_temptable[i-1][1]) * \r
 -          (heater_0_temptable[i][0] - heater_0_temptable[i-1][0]) /\r
 -          (heater_0_temptable[i][1] - heater_0_temptable[i-1][1]);  \r
 -        break;\r
 -      }\r
 -    }\r
 -\r
 -    // Overflow: Set to last value in the table\r
 -    if (i == NUMTEMPS_HEATER_0) raw = heater_0_temptable[i-1][0];\r
 -\r
 -    return (1023 * OVERSAMPLENR) - raw;\r
 -  #elif defined HEATER_0_USES_AD595\r
 -    return celsius * (1024.0 / (5.0 * 100.0) ) * OVERSAMPLENR;\r
 -  #endif\r
 -}\r
 -\r
 -// Takes bed temperature value as input and returns corresponding raw value. \r
 -// For a thermistor, it uses the RepRap thermistor temp table.\r
 -// This is needed because PID in hydra firmware hovers around a given analog value, not a temp value.\r
 -// This function is derived from inversing the logic from a portion of getTemperature() in FiveD RepRap firmware.\r
 -int temp2analogBed(int celsius) {\r
 -  #ifdef BED_USES_THERMISTOR\r
 -\r
 -    int raw = 0;\r
 -    byte i;\r
 -    \r
 -    for (i=1; i<BNUMTEMPS; i++)\r
 -    {\r
 -      if (bedtemptable[i][1] < celsius)\r
 -      {\r
 -        raw = bedtemptable[i-1][0] + \r
 -          (celsius - bedtemptable[i-1][1]) * \r
 -          (bedtemptable[i][0] - bedtemptable[i-1][0]) /\r
 -          (bedtemptable[i][1] - bedtemptable[i-1][1]);\r
 -      \r
 -        break;\r
 -      }\r
 -    }\r
 -\r
 -    // Overflow: Set to last value in the table\r
 -    if (i == BNUMTEMPS) raw = bedtemptable[i-1][0];\r
 -\r
 -    return (1023 * OVERSAMPLENR) - raw;\r
 -  #elif defined BED_USES_AD595\r
 -    return lround(celsius * (1024.0 * OVERSAMPLENR/ (5.0 * 100.0) ) );\r
 -  #endif\r
 -}\r
 -\r
 -// Derived from RepRap FiveD extruder::getTemperature()\r
 -// For hot end temperature measurement.\r
 -float analog2temp(int raw) {\r
 -  #ifdef HEATER_0_USES_THERMISTOR\r
 -    float celsius = 0;\r
 -    byte i;  \r
 -    raw = (1023 * OVERSAMPLENR) - raw;\r
 -    for (i=1; i<NUMTEMPS_HEATER_0; i++)\r
 -    {\r
 -      if (heater_0_temptable[i][0] > raw)\r
 -      {\r
 -        celsius  = heater_0_temptable[i-1][1] + \r
 -          (raw - heater_0_temptable[i-1][0]) * \r
 -          (float)(heater_0_temptable[i][1] - heater_0_temptable[i-1][1]) /\r
 -          (float)(heater_0_temptable[i][0] - heater_0_temptable[i-1][0]);\r
 -\r
 -        break;\r
 -      }\r
 -    }\r
 -\r
 -    // Overflow: Set to last value in the table\r
 -    if (i == NUMTEMPS_HEATER_0) celsius = heater_0_temptable[i-1][1];\r
 -\r
 -    return celsius;\r
 -  #elif defined HEATER_0_USES_AD595\r
 -    return raw * ((5.0 * 100.0) / 1024.0) / OVERSAMPLENR;\r
 -  #endif\r
 -}\r
 -\r
 -// Derived from RepRap FiveD extruder::getTemperature()\r
 -// For bed temperature measurement.\r
 -float analog2tempBed(int raw) {\r
 -  #ifdef BED_USES_THERMISTOR\r
 -    int celsius = 0;\r
 -    byte i;\r
 -\r
 -    raw = (1023 * OVERSAMPLENR) - raw;\r
 -\r
 -    for (i=1; i<BNUMTEMPS; i++)\r
 -    {\r
 -      if (bedtemptable[i][0] > raw)\r
 -      {\r
 -        celsius  = bedtemptable[i-1][1] + \r
 -          (raw - bedtemptable[i-1][0]) * \r
 -          (bedtemptable[i][1] - bedtemptable[i-1][1]) /\r
 -          (bedtemptable[i][0] - bedtemptable[i-1][0]);\r
 -\r
 -        break;\r
 -      }\r
 -    }\r
 -\r
 -    // Overflow: Set to last value in the table\r
 -    if (i == BNUMTEMPS) celsius = bedtemptable[i-1][1];\r
 -\r
 -    return celsius;\r
 -    \r
 -  #elif defined BED_USES_AD595\r
 -    return raw * ((5.0 * 100.0) / 1024.0) / OVERSAMPLENR;\r
 -  #endif\r
 -}\r
 -\r
 -void tp_init()\r
 -{\r
 -  #if (HEATER_0_PIN > -1) \r
 -    SET_OUTPUT(HEATER_0_PIN);\r
 -  #endif  \r
 -  #if (HEATER_1_PIN > -1) \r
 -    SET_OUTPUT(HEATER_1_PIN);\r
 -  #endif  \r
 -  #if (HEATER_2_PIN > -1) \r
 -    SET_OUTPUT(HEATER_2_PIN);\r
 -  #endif  \r
 -\r
 -  #ifdef PIDTEMP\r
 -    temp_iState_min = 0.0;\r
 -    temp_iState_max = PID_INTEGRAL_DRIVE_MAX / Ki;\r
 -  #endif //PIDTEMP\r
 -\r
 -  // Set analog inputs\r
 -  ADCSRA = 1<<ADEN | 1<<ADSC | 1<<ADIF | 0x07;\r
 -  \r
 -  // Use timer0 for temperature measurement\r
 -  // Interleave temperature interrupt with millies interrupt\r
 -  OCR0B = 128;\r
 -  TIMSK0 |= (1<<OCIE0B);  \r
 -}\r
 -\r
 -\r
 -\r
 -void setWatch() \r
 -{  \r
 -#ifdef WATCHPERIOD\r
 -  if(isHeatingHotend0())\r
 -  {\r
 -    watchmillis = max(1,millis());\r
 -    watch_raw[TEMPSENSOR_HOTEND_0] = current_raw[TEMPSENSOR_HOTEND_0];\r
 -  }\r
 -  else\r
 -  {\r
 -    watchmillis = 0;\r
 -  } \r
 -#endif \r
 -}\r
 -\r
 -\r
 -void disable_heater()\r
 -{\r
 -  #if TEMP_0_PIN > -1\r
 -  target_raw[0]=0;\r
 -   #if HEATER_0_PIN > -1  \r
 -     WRITE(HEATER_0_PIN,LOW);\r
 -   #endif\r
 -  #endif\r
 -     \r
 -  #if TEMP_1_PIN > -1\r
 -    target_raw[1]=0;\r
 -    #if HEATER_1_PIN > -1 \r
 -      WRITE(HEATER_1_PIN,LOW);\r
 -    #endif\r
 -  #endif\r
 -      \r
 -  #if TEMP_2_PIN > -1\r
 -    target_raw[2]=0;\r
 -    #if HEATER_2_PIN > -1  \r
 -      WRITE(HEATER_2_PIN,LOW);\r
 -    #endif\r
 -  #endif \r
 -}\r
 -\r
 -// Timer 0 is shared with millies\r
 -ISR(TIMER0_COMPB_vect)\r
 -{\r
 -  //these variables are only accesible from the ISR, but static, so they don't loose their value\r
 -  static unsigned char temp_count = 0;\r
 -  static unsigned long raw_temp_0_value = 0;\r
 -  static unsigned long raw_temp_1_value = 0;\r
 -  static unsigned long raw_temp_2_value = 0;\r
 -  static unsigned char temp_state = 0;\r
 -  \r
 -  switch(temp_state) {\r
 -    case 0: // Prepare TEMP_0\r
 -      #if (TEMP_0_PIN > -1)\r
 -        #if TEMP_0_PIN < 8\r
 -          DIDR0 = 1 << TEMP_0_PIN; \r
 -        #else\r
 -          DIDR2 = 1<<(TEMP_0_PIN - 8); \r
 -          ADCSRB = 1<<MUX5;\r
 -        #endif\r
 -        ADMUX = ((1 << REFS0) | (TEMP_0_PIN & 0x07));\r
 -        ADCSRA |= 1<<ADSC; // Start conversion\r
 -      #endif\r
 -      #ifdef ULTIPANEL\r
 -        buttons_check();\r
 -      #endif\r
 -      temp_state = 1;\r
 -      break;\r
 -    case 1: // Measure TEMP_0\r
 -      #if (TEMP_0_PIN > -1)\r
 -        raw_temp_0_value += ADC;\r
 -      #endif\r
 -      temp_state = 2;\r
 -      break;\r
 -    case 2: // Prepare TEMP_1\r
 -      #if (TEMP_1_PIN > -1)\r
 -        #if TEMP_1_PIN < 7\r
 -          DIDR0 = 1<<TEMP_1_PIN; \r
 -        #else\r
 -          DIDR2 = 1<<(TEMP_1_PIN - 8); \r
 -          ADCSRB = 1<<MUX5;\r
 -        #endif\r
 -        ADMUX = ((1 << REFS0) | (TEMP_1_PIN & 0x07));\r
 -        ADCSRA |= 1<<ADSC; // Start conversion\r
 -      #endif\r
 -      #ifdef ULTIPANEL\r
 -        buttons_check();\r
 -      #endif\r
 -      temp_state = 3;\r
 -      break;\r
 -    case 3: // Measure TEMP_1\r
 -      #if (TEMP_1_PIN > -1)\r
 -        raw_temp_1_value += ADC;\r
 -      #endif\r
 -      temp_state = 4;\r
 -      break;\r
 -    case 4: // Prepare TEMP_2\r
 -      #if (TEMP_2_PIN > -1)\r
 -        #if TEMP_2_PIN < 7\r
 -          DIDR0 = 1 << TEMP_2_PIN; \r
 -        #else\r
 -          DIDR2 = 1<<(TEMP_2_PIN - 8); \r
 -          ADCSRB = 1<<MUX5;\r
 -        #endif\r
 -        ADMUX = ((1 << REFS0) | (TEMP_2_PIN & 0x07));\r
 -        ADCSRA |= 1<<ADSC; // Start conversion\r
 -      #endif\r
 -      #ifdef ULTIPANEL\r
 -        buttons_check();\r
 -      #endif\r
 -      temp_state = 5;\r
 -      break;\r
 -    case 5: // Measure TEMP_2\r
 -      #if (TEMP_2_PIN > -1)\r
 -        raw_temp_2_value += ADC;\r
 -      #endif\r
 -      temp_state = 0;\r
 -      temp_count++;\r
 -      break;\r
 -    default:\r
 -      SERIAL_ERROR_START;\r
 -      SERIAL_ERRORLNPGM("Temp measurement error!");\r
 -      break;\r
 -  }\r
 -    \r
 -  if(temp_count >= 16) // 6 ms * 16 = 96ms.\r
 -  {\r
 -    #ifdef HEATER_0_USES_AD595\r
 -      current_raw[0] = raw_temp_0_value;\r
 -    #else\r
 -      current_raw[0] = 16383 - raw_temp_0_value;\r
 -    #endif\r
 -    \r
 -    #ifdef HEATER_1_USES_AD595\r
 -      current_raw[2] = raw_temp_2_value;\r
 -    #else\r
 -      current_raw[2] = 16383 - raw_temp_2_value;\r
 -    #endif\r
 -    \r
 -    #ifdef BED_USES_AD595\r
 -      current_raw[1] = raw_temp_1_value;\r
 -    #else\r
 -      current_raw[1] = 16383 - raw_temp_1_value;\r
 -    #endif\r
 -    \r
 -    temp_meas_ready = true;\r
 -    temp_count = 0;\r
 -    raw_temp_0_value = 0;\r
 -    raw_temp_1_value = 0;\r
 -    raw_temp_2_value = 0;\r
 -    #ifdef HEATER_0_MAXTEMP\r
 -      #if (HEATER_0_PIN > -1)\r
 -        if(current_raw[TEMPSENSOR_HOTEND_0] >= maxttemp_0) {\r
 -          target_raw[TEMPSENSOR_HOTEND_0] = 0;\r
 -          analogWrite(HEATER_0_PIN, 0);\r
 -          SERIAL_ERROR_START;\r
 -          SERIAL_ERRORLNPGM("Temperature extruder 0 switched off. MAXTEMP triggered !!");\r
 -          kill();\r
 -        }\r
 -      #endif\r
 -    #endif\r
 -  #ifdef HEATER_1_MAXTEMP\r
 -    #if (HEATER_1_PIN > -1)\r
 -      if(current_raw[TEMPSENSOR_HOTEND_1] >= maxttemp_1) {\r
 -        target_raw[TEMPSENSOR_HOTEND_1] = 0;\r
 -      if(current_raw[2] >= maxttemp_1) {\r
 -        analogWrite(HEATER_2_PIN, 0);\r
 -        SERIAL_ERROR_START;\r
 -        SERIAL_ERRORLNPGM("Temperature extruder 1 switched off. MAXTEMP triggered !!");\r
 -        kill()\r
 -      }\r
 -    #endif\r
 -  #endif //MAXTEMP\r
 -  \r
 -  #ifdef HEATER_0_MINTEMP\r
 -    #if (HEATER_0_PIN > -1)\r
 -      if(current_raw[TEMPSENSOR_HOTEND_0] <= minttemp_0) {\r
 -        target_raw[TEMPSENSOR_HOTEND_0] = 0;\r
 -        analogWrite(HEATER_0_PIN, 0);\r
 -        SERIAL_ERROR_START;\r
 -        SERIAL_ERRORLNPGM("Temperature extruder 0 switched off. MINTEMP triggered !!");\r
 -        kill();\r
 -      }\r
 -    #endif\r
 -  #endif\r
 -  \r
 -  #ifdef HEATER_1_MINTEMP\r
 -    #if (HEATER_2_PIN > -1)\r
 -      if(current_raw[TEMPSENSOR_HOTEND_1] <= minttemp_1) {\r
 -        target_raw[TEMPSENSOR_HOTEND_1] = 0;\r
 -        analogWrite(HEATER_2_PIN, 0);\r
 -        SERIAL_ERROR_START;\r
 -        SERIAL_ERRORLNPGM("Temperature extruder 1 switched off. MINTEMP triggered !!");\r
 -        kill();\r
 -      }\r
 -    #endif\r
 -  #endif //MAXTEMP\r
 -  \r
 -  #ifdef BED_MINTEMP\r
 -    #if (HEATER_1_PIN > -1)\r
 -      if(current_raw[1] <= bed_minttemp) {\r
 -        target_raw[1] = 0;\r
 -        WRITE(HEATER_1_PIN, 0);\r
 -        SERIAL_ERROR_START;\r
 -        SERIAL_ERRORLNPGM("Temperatur heated bed switched off. MINTEMP triggered !!");\r
 -        kill();\r
 -      }\r
 -    #endif\r
 -  #endif\r
 -  \r
 -  #ifdef BED_MAXTEMP\r
 -    #if (HEATER_1_PIN > -1)\r
 -      if(current_raw[1] >= bed_maxttemp) {\r
 -        target_raw[1] = 0;\r
 -        WRITE(HEATER_1_PIN, 0);\r
 -        SERIAL_ERROR_START;\r
 -        SERIAL_ERRORLNPGM("Temperature heated bed switched off. MAXTEMP triggered !!");\r
 -        kill();\r
 -      }\r
 -    #endif\r
 -  #endif\r
 -  }\r
 -}\r
 -\r
 -\r
 +/*
 +  temperature.c - temperature control
 +  Part of Marlin
 +  
 + Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
 + 
 + This program is free software: you can redistribute it and/or modify
 + it under the terms of the GNU General Public License as published by
 + the Free Software Foundation, either version 3 of the License, or
 + (at your option) any later version.
 + 
 + This program is distributed in the hope that it will be useful,
 + but WITHOUT ANY WARRANTY; without even the implied warranty of
 + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 + GNU General Public License for more details.
 + 
 + You should have received a copy of the GNU General Public License
 + along with this program.  If not, see <http://www.gnu.org/licenses/>.
 + */
 +
 +/*
 + This firmware is a mashup between Sprinter and grbl.
 +  (https://github.com/kliment/Sprinter)
 +  (https://github.com/simen/grbl/tree)
 + 
 + It has preliminary support for Matthew Roberts advance algorithm 
 +    http://reprap.org/pipermail/reprap-dev/2011-May/003323.html
 +
 + This firmware is optimized for gen6 electronics.
 + */
 +#include <avr/pgmspace.h>
 +
 +#include "fastio.h"
 +#include "Configuration.h"
 +#include "pins.h"
 +#include "Marlin.h"
 +#include "ultralcd.h"
- #include "streaming.h"
 +#include "temperature.h"
 +#include "watchdog.h"
 +
 +//===========================================================================
 +//=============================public variables============================
 +//===========================================================================
 +int target_raw[3] = {0, 0, 0};
 +int current_raw[3] = {0, 0, 0};
 +
 +#ifdef PIDTEMP
 +  
 +  // probably used external
 +  float HeaterPower;
 +  float pid_setpoint = 0.0;
 +
 +  
 +  float Kp=DEFAULT_Kp;
 +  float Ki=DEFAULT_Ki;
 +  float Kd=DEFAULT_Kd;
 +  #ifdef PID_ADD_EXTRUSION_RATE
 +    float Kc=DEFAULT_Kc;
 +  #endif
 +#endif //PIDTEMP
 +  
 +  
 +//===========================================================================
 +//=============================private variables============================
 +//===========================================================================
 +static bool temp_meas_ready = false;
 +
 +static unsigned long previous_millis_heater, previous_millis_bed_heater;
 +
 +#ifdef PIDTEMP
 +  //static cannot be external:
 +  static float temp_iState = 0;
 +  static float temp_dState = 0;
 +  static float pTerm;
 +  static float iTerm;
 +  static float dTerm;
 +  //int output;
 +  static float pid_error;
 +  static float temp_iState_min;
 +  static float temp_iState_max;
 +  static float pid_input;
 +  static float pid_output;
 +  static bool pid_reset;
 + 
 +#endif //PIDTEMP
 +  
 +#ifdef WATCHPERIOD
 +  static int watch_raw[3] = {-1000,-1000,-1000};
 +  static unsigned long watchmillis = 0;
 +#endif //WATCHPERIOD
 +
 +#ifdef HEATER_0_MINTEMP
 +  static int minttemp_0 = temp2analog(HEATER_0_MINTEMP);
 +#endif //MINTEMP
 +#ifdef HEATER_0_MAXTEMP
 +  static int maxttemp_0 = temp2analog(HEATER_0_MAXTEMP);
 +#endif //MAXTEMP
 +
 +#ifdef HEATER_1_MINTEMP
 +  static int minttemp_1 = temp2analog(HEATER_1_MINTEMP);
 +#endif //MINTEMP
 +#ifdef HEATER_1_MAXTEMP
 +  static int maxttemp_1 = temp2analog(HEATER_1_MAXTEMP);
 +#endif //MAXTEMP
 +
 +#ifdef BED_MINTEMP
 +  static int bed_minttemp = temp2analog(BED_MINTEMP);
 +#endif //BED_MINTEMP
 +#ifdef BED_MAXTEMP
 +  static int bed_maxttemp = temp2analog(BED_MAXTEMP);
 +#endif //BED_MAXTEMP
 +
 +//===========================================================================
 +//=============================functions         ============================
 +//===========================================================================
 +  
 +void manage_heater()
 +{
 +  #ifdef USE_WATCHDOG
 +    wd_reset();
 +  #endif
 +  
 +  float pid_input;
 +  float pid_output;
 +  if(temp_meas_ready != true)   //better readability
 +    return; 
 +
 +  CRITICAL_SECTION_START;
 +    temp_meas_ready = false;
 +  CRITICAL_SECTION_END;
 +
 +  #ifdef PIDTEMP
 +    pid_input = analog2temp(current_raw[TEMPSENSOR_HOTEND_0]);
 +
 +    #ifndef PID_OPENLOOP
 +        pid_error = pid_setpoint - pid_input;
 +        if(pid_error > 10){
 +          pid_output = PID_MAX;
 +          pid_reset = true;
 +        }
 +        else if(pid_error < -10) {
 +          pid_output = 0;
 +          pid_reset = true;
 +        }
 +        else {
 +          if(pid_reset == true) {
 +            temp_iState = 0.0;
 +            pid_reset = false;
 +          }
 +          pTerm = Kp * pid_error;
 +          temp_iState += pid_error;
 +          temp_iState = constrain(temp_iState, temp_iState_min, temp_iState_max);
 +          iTerm = Ki * temp_iState;
 +          //K1 defined in Configuration.h in the PID settings
 +          #define K2 (1.0-K1)
 +          dTerm = (Kd * (pid_input - temp_dState))*K2 + (K1 * dTerm);
 +          temp_dState = pid_input;
 +//          #ifdef PID_ADD_EXTRUSION_RATE
 +//            pTerm+=Kc*current_block->speed_e; //additional heating if extrusion speed is high
 +//          #endif
 +          pid_output = constrain(pTerm + iTerm - dTerm, 0, PID_MAX);
++          
 +        }
 +    #endif //PID_OPENLOOP
 +    #ifdef PID_DEBUG
-      SERIAL_ECHOLN(" PIDDEBUG Input "<<pid_input<<" Output "<<pid_output" pTerm "<<pTerm<<" iTerm "<<iTerm<<" dTerm "<<dTerm);  
++     //SERIAL_ECHOLN(" PIDDEBUG Input "<<pid_input<<" Output "<<pid_output" pTerm "<<pTerm<<" iTerm "<<iTerm<<" dTerm "<<dTerm);  
 +    #endif //PID_DEBUG
++    HeaterPower=pid_output;
 +    analogWrite(HEATER_0_PIN, pid_output);
 +  #endif //PIDTEMP
 +
 +  #ifndef PIDTEMP
 +    if(current_raw[0] >= target_raw[0])
 +    {
 +      WRITE(HEATER_0_PIN,LOW);
 +    }
 +    else 
 +    {
 +      WRITE(HEATER_0_PIN,HIGH);
 +    }
 +  #endif
 +    
 +  if(millis() - previous_millis_bed_heater < BED_CHECK_INTERVAL)
 +    return;
 +  previous_millis_bed_heater = millis();
 +  
 +  #if TEMP_1_PIN > -1
 +    if(current_raw[TEMPSENSOR_BED] >= target_raw[TEMPSENSOR_BED])
 +    {
 +      WRITE(HEATER_1_PIN,LOW);
 +    }
 +    else 
 +    {
 +      WRITE(HEATER_1_PIN,HIGH);
 +    }
 +  #endif
 +}
 +
 +// Takes hot end temperature value as input and returns corresponding raw value. 
 +// For a thermistor, it uses the RepRap thermistor temp table.
 +// This is needed because PID in hydra firmware hovers around a given analog value, not a temp value.
 +// This function is derived from inversing the logic from a portion of getTemperature() in FiveD RepRap firmware.
 +int temp2analog(int celsius) {
 +  #ifdef HEATER_0_USES_THERMISTOR
 +    int raw = 0;
 +    byte i;
 +
 +    for (i=1; i<NUMTEMPS_HEATER_0; i++)
 +    {
 +      if (pgm_read_word(&(heater_0_temptable[i][1])) < celsius)
 +      {
 +        raw = pgm_read_word(&(heater_0_temptable[i-1][0])) + 
 +          (celsius - pgm_read_word(&(heater_0_temptable[i-1][1]))) * 
 +          (pgm_read_word(&(heater_0_temptable[i][0])) - pgm_read_word(&(heater_0_temptable[i-1][0]))) /
 +          (pgm_read_word(&(heater_0_temptable[i][1])) - pgm_read_word(&(heater_0_temptable[i-1][1])));  
 +        break;
 +      }
 +    }
 +
 +    // Overflow: Set to last value in the table
 +    if (i == NUMTEMPS_HEATER_0) raw = pgm_read_word(&(heater_0_temptable[i-1][0]));
 +
 +    return (1023 * OVERSAMPLENR) - raw;
 +  #elif defined HEATER_0_USES_AD595
 +    return celsius * (1024.0 / (5.0 * 100.0) ) * OVERSAMPLENR;
 +  #endif
 +}
 +
 +// Takes bed temperature value as input and returns corresponding raw value. 
 +// For a thermistor, it uses the RepRap thermistor temp table.
 +// This is needed because PID in hydra firmware hovers around a given analog value, not a temp value.
 +// This function is derived from inversing the logic from a portion of getTemperature() in FiveD RepRap firmware.
 +int temp2analogBed(int celsius) {
 +  #ifdef BED_USES_THERMISTOR
 +
 +    int raw = 0;
 +    byte i;
 +    
 +    for (i=1; i<BNUMTEMPS; i++)
 +    {
 +      if (pgm_read_word(&)bedtemptable[i][1])) < celsius)
 +      {
 +        raw = pgm_read_word(&(bedtemptable[i-1][0])) + 
 +          (celsius - pgm_read_word(&(bedtemptable[i-1][1]))) * 
 +          (pgm_read_word(&(bedtemptable[i][0])) - pgm_read_word(&(bedtemptable[i-1][0]))) /
 +          (pgm_read_word(&(bedtemptable[i][1])) - pgm_read_word(&(bedtemptable[i-1][1])));
 +      
 +        break;
 +      }
 +    }
 +
 +    // Overflow: Set to last value in the table
 +    if (i == BNUMTEMPS) raw = pgm_read_word(&(bedtemptable[i-1][0]));
 +
 +    return (1023 * OVERSAMPLENR) - raw;
 +  #elif defined BED_USES_AD595
-     return celsius * (1024.0 / (5.0 * 100.0) ) * OVERSAMPLENR;
++    return lround(celsius * (1024.0 * OVERSAMPLENR/ (5.0 * 100.0) ) );
 +  #endif
 +}
 +
 +// Derived from RepRap FiveD extruder::getTemperature()
 +// For hot end temperature measurement.
 +float analog2temp(int raw) {
 +  #ifdef HEATER_0_USES_THERMISTOR
 +    float celsius = 0;
 +    byte i;  
 +    raw = (1023 * OVERSAMPLENR) - raw;
 +    for (i=1; i<NUMTEMPS_HEATER_0; i++)
 +    {
 +      if ((short)pgm_read_word(&heater_0_temptable[i][0]) > raw)
 +      {
 +        celsius  = (short)pgm_read_word(&heater_0_temptable[i-1][1]) + 
 +          (raw - (short)pgm_read_word(&heater_0_temptable[i-1][0])) * 
 +          (float)((short)pgm_read_word(&heater_0_temptable[i][1]) - (short)pgm_read_word(&heater_0_temptable[i-1][1])) /
 +          (float)((short)pgm_read_word(&heater_0_temptable[i][0]) - (short)pgm_read_word(&heater_0_temptable[i-1][0]));
 +        break;
 +      }
 +    }
 +
 +    // Overflow: Set to last value in the table
 +    if (i == NUMTEMPS_HEATER_0) celsius = (short)pgm_read_word(&(heater_0_temptable[i-1][1]));
 +
 +    return celsius;
 +  #elif defined HEATER_0_USES_AD595
 +    return raw * ((5.0 * 100.0) / 1024.0) / OVERSAMPLENR;
 +  #endif
 +}
 +
 +// Derived from RepRap FiveD extruder::getTemperature()
 +// For bed temperature measurement.
 +float analog2tempBed(int raw) {
 +  #ifdef BED_USES_THERMISTOR
 +    int celsius = 0;
 +    byte i;
 +
 +    raw = (1023 * OVERSAMPLENR) - raw;
 +
 +    for (i=1; i<BNUMTEMPS; i++)
 +    {
 +      if (pgm_read_word(&(bedtemptable[i][0])) > raw)
 +      {
 +        celsius  = pgm_read_word(&(bedtemptable[i-1][1])) + 
 +          (raw - pgm_read_word(&(bedtemptable[i-1][0]))) * 
 +          (pgm_read_word(&(bedtemptable[i][1])) - pgm_read_word(&(bedtemptable[i-1][1]))) /
 +          (pgm_read_word(&(bedtemptable[i][0])) - pgm_read_word(&(bedtemptable[i-1][0])));
 +
 +        break;
 +      }
 +    }
 +
 +    // Overflow: Set to last value in the table
 +    if (i == BNUMTEMPS) celsius = pgm_read_word(&(bedtemptable[i-1][1]));
 +
 +    return celsius;
 +    
 +  #elif defined BED_USES_AD595
 +    return raw * ((5.0 * 100.0) / 1024.0) / OVERSAMPLENR;
 +  #endif
 +}
 +
 +void tp_init()
 +{
 +  #if (HEATER_0_PIN > -1) 
 +    SET_OUTPUT(HEATER_0_PIN);
 +  #endif  
 +  #if (HEATER_1_PIN > -1) 
 +    SET_OUTPUT(HEATER_1_PIN);
 +  #endif  
 +  #if (HEATER_2_PIN > -1) 
 +    SET_OUTPUT(HEATER_2_PIN);
 +  #endif  
 +
 +  #ifdef PIDTEMP
 +    temp_iState_min = 0.0;
 +    temp_iState_max = PID_INTEGRAL_DRIVE_MAX / Ki;
 +  #endif //PIDTEMP
 +
 +  // Set analog inputs
 +  ADCSRA = 1<<ADEN | 1<<ADSC | 1<<ADIF | 0x07;
 +  
 +  // Use timer0 for temperature measurement
 +  // Interleave temperature interrupt with millies interrupt
 +  OCR0B = 128;
 +  TIMSK0 |= (1<<OCIE0B);  
 +}
 +
 +
 +
 +void setWatch() 
 +{  
 +#ifdef WATCHPERIOD
 +  if(isHeatingHotend0())
 +  {
 +    watchmillis = max(1,millis());
 +    watch_raw[TEMPSENSOR_HOTEND_0] = current_raw[TEMPSENSOR_HOTEND_0];
 +  }
 +  else
 +  {
 +    watchmillis = 0;
 +  } 
 +#endif 
 +}
 +
 +
 +void disable_heater()
 +{
 +  #if TEMP_0_PIN > -1
 +  target_raw[0]=0;
 +   #if HEATER_0_PIN > -1  
 +     WRITE(HEATER_0_PIN,LOW);
 +   #endif
 +  #endif
 +     
 +  #if TEMP_1_PIN > -1
 +    target_raw[1]=0;
 +    #if HEATER_1_PIN > -1 
 +      WRITE(HEATER_1_PIN,LOW);
 +    #endif
 +  #endif
 +      
 +  #if TEMP_2_PIN > -1
 +    target_raw[2]=0;
 +    #if HEATER_2_PIN > -1  
 +      WRITE(HEATER_2_PIN,LOW);
 +    #endif
 +  #endif 
 +}
 +
 +// Timer 0 is shared with millies
 +ISR(TIMER0_COMPB_vect)
 +{
 +  //these variables are only accesible from the ISR, but static, so they don't loose their value
 +  static unsigned char temp_count = 0;
 +  static unsigned long raw_temp_0_value = 0;
 +  static unsigned long raw_temp_1_value = 0;
 +  static unsigned long raw_temp_2_value = 0;
 +  static unsigned char temp_state = 0;
 +  
 +  switch(temp_state) {
 +    case 0: // Prepare TEMP_0
 +      #if (TEMP_0_PIN > -1)
 +        #if TEMP_0_PIN < 8
 +          DIDR0 = 1 << TEMP_0_PIN; 
 +        #else
 +          DIDR2 = 1<<(TEMP_0_PIN - 8); 
 +          ADCSRB = 1<<MUX5;
 +        #endif
 +        ADMUX = ((1 << REFS0) | (TEMP_0_PIN & 0x07));
 +        ADCSRA |= 1<<ADSC; // Start conversion
 +      #endif
 +      #ifdef ULTIPANEL
 +        buttons_check();
 +      #endif
 +      temp_state = 1;
 +      break;
 +    case 1: // Measure TEMP_0
 +      #if (TEMP_0_PIN > -1)
 +        raw_temp_0_value += ADC;
 +      #endif
 +      temp_state = 2;
 +      break;
 +    case 2: // Prepare TEMP_1
 +      #if (TEMP_1_PIN > -1)
 +        #if TEMP_1_PIN < 7
 +          DIDR0 = 1<<TEMP_1_PIN; 
 +        #else
 +          DIDR2 = 1<<(TEMP_1_PIN - 8); 
 +          ADCSRB = 1<<MUX5;
 +        #endif
 +        ADMUX = ((1 << REFS0) | (TEMP_1_PIN & 0x07));
 +        ADCSRA |= 1<<ADSC; // Start conversion
 +      #endif
 +      #ifdef ULTIPANEL
 +        buttons_check();
 +      #endif
 +      temp_state = 3;
 +      break;
 +    case 3: // Measure TEMP_1
 +      #if (TEMP_1_PIN > -1)
 +        raw_temp_1_value += ADC;
 +      #endif
 +      temp_state = 4;
 +      break;
 +    case 4: // Prepare TEMP_2
 +      #if (TEMP_2_PIN > -1)
 +        #if TEMP_2_PIN < 7
 +          DIDR0 = 1 << TEMP_2_PIN; 
 +        #else
 +          DIDR2 = 1<<(TEMP_2_PIN - 8); 
 +          ADCSRB = 1<<MUX5;
 +        #endif
 +        ADMUX = ((1 << REFS0) | (TEMP_2_PIN & 0x07));
 +        ADCSRA |= 1<<ADSC; // Start conversion
 +      #endif
 +      #ifdef ULTIPANEL
 +        buttons_check();
 +      #endif
 +      temp_state = 5;
 +      break;
 +    case 5: // Measure TEMP_2
 +      #if (TEMP_2_PIN > -1)
 +        raw_temp_2_value += ADC;
 +      #endif
 +      temp_state = 0;
 +      temp_count++;
 +      break;
 +    default:
-       SERIAL_ERRORLN("Temp measurement error!");
++      SERIAL_ERROR_START;
++      SERIAL_ERRORLNPGM("Temp measurement error!");
 +      break;
 +  }
 +    
 +  if(temp_count >= 16) // 6 ms * 16 = 96ms.
 +  {
 +    #ifdef HEATER_0_USES_AD595
 +      current_raw[0] = raw_temp_0_value;
 +    #else
 +      current_raw[0] = 16383 - raw_temp_0_value;
 +    #endif
 +    
 +    #ifdef HEATER_1_USES_AD595
 +      current_raw[2] = raw_temp_2_value;
 +    #else
 +      current_raw[2] = 16383 - raw_temp_2_value;
 +    #endif
 +    
 +    #ifdef BED_USES_AD595
 +      current_raw[1] = raw_temp_1_value;
 +    #else
 +      current_raw[1] = 16383 - raw_temp_1_value;
 +    #endif
 +    
 +    temp_meas_ready = true;
 +    temp_count = 0;
 +    raw_temp_0_value = 0;
 +    raw_temp_1_value = 0;
 +    raw_temp_2_value = 0;
 +    #ifdef HEATER_0_MAXTEMP
 +      #if (HEATER_0_PIN > -1)
 +        if(current_raw[TEMPSENSOR_HOTEND_0] >= maxttemp_0) {
 +          target_raw[TEMPSENSOR_HOTEND_0] = 0;
 +          analogWrite(HEATER_0_PIN, 0);
-           SERIAL_ERRORLN("Temperature extruder 0 switched off. MAXTEMP triggered !!");
++          SERIAL_ERROR_START;
++          SERIAL_ERRORLNPGM("Temperature extruder 0 switched off. MAXTEMP triggered !!");
 +          kill();
 +        }
 +      #endif
 +    #endif
 +  #ifdef HEATER_1_MAXTEMP
 +    #if (HEATER_1_PIN > -1)
 +      if(current_raw[TEMPSENSOR_HOTEND_1] >= maxttemp_1) {
 +        target_raw[TEMPSENSOR_HOTEND_1] = 0;
 +      if(current_raw[2] >= maxttemp_1) {
 +        analogWrite(HEATER_2_PIN, 0);
-         SERIAL_ERRORLN("Temperature extruder 1 switched off. MAXTEMP triggered !!");
++        SERIAL_ERROR_START;
++        SERIAL_ERRORLNPGM("Temperature extruder 1 switched off. MAXTEMP triggered !!");
 +        kill()
 +      }
 +    #endif
 +  #endif //MAXTEMP
 +  
 +  #ifdef HEATER_0_MINTEMP
 +    #if (HEATER_0_PIN > -1)
 +      if(current_raw[TEMPSENSOR_HOTEND_0] <= minttemp_0) {
 +        target_raw[TEMPSENSOR_HOTEND_0] = 0;
 +        analogWrite(HEATER_0_PIN, 0);
-         SERIAL_ERRORLN("Temperature extruder 0 switched off. MINTEMP triggered !!");
++        SERIAL_ERROR_START;
++        SERIAL_ERRORLNPGM("Temperature extruder 0 switched off. MINTEMP triggered !!");
 +        kill();
 +      }
 +    #endif
 +  #endif
 +  
 +  #ifdef HEATER_1_MINTEMP
 +    #if (HEATER_2_PIN > -1)
 +      if(current_raw[TEMPSENSOR_HOTEND_1] <= minttemp_1) {
 +        target_raw[TEMPSENSOR_HOTEND_1] = 0;
 +        analogWrite(HEATER_2_PIN, 0);
-         SERIAL_ERRORLN("Temperature extruder 1 switched off. MINTEMP triggered !!");
++        SERIAL_ERROR_START;
++        SERIAL_ERRORLNPGM("Temperature extruder 1 switched off. MINTEMP triggered !!");
 +        kill();
 +      }
 +    #endif
 +  #endif //MAXTEMP
 +  
 +  #ifdef BED_MINTEMP
 +    #if (HEATER_1_PIN > -1)
 +      if(current_raw[1] <= bed_minttemp) {
 +        target_raw[1] = 0;
 +        WRITE(HEATER_1_PIN, 0);
-         SERIAL_ERRORLN("Temperatur heated bed switched off. MINTEMP triggered !!");
++        SERIAL_ERROR_START;
++        SERIAL_ERRORLNPGM("Temperatur heated bed switched off. MINTEMP triggered !!");
 +        kill();
 +      }
 +    #endif
 +  #endif
 +  
 +  #ifdef BED_MAXTEMP
 +    #if (HEATER_1_PIN > -1)
 +      if(current_raw[1] >= bed_maxttemp) {
 +        target_raw[1] = 0;
 +        WRITE(HEATER_1_PIN, 0);
-         SERIAL_ERRORLN("Temperature heated bed switched off. MAXTEMP triggered !!");
++        SERIAL_ERROR_START;
++        SERIAL_ERRORLNPGM("Temperature heated bed switched off. MAXTEMP triggered !!");
 +        kill();
 +      }
 +    #endif
 +  #endif
 +  }
 +}
 +
\r
++