Marlin/Marlin.pde

   1 /* -*- c++ -*- */
   2
   3 /*
   4     Reprap firmware based on Sprinter and grbl.
   5  Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
   6
   7  This program is free software: you can redistribute it and/or modify
   8  it under the terms of the GNU General Public License as published by
   9  the Free Software Foundation, either version 3 of the License, or
  10  (at your option) any later version.
  11
  12  This program is distributed in the hope that it will be useful,
  13  but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  GNU General Public License for more details.
  16
  17  You should have received a copy of the GNU General Public License
  18  along with this program.  If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 /*
  22  This firmware is a mashup between Sprinter and grbl.
  23   (https://github.com/kliment/Sprinter)
  24   (https://github.com/simen/grbl/tree)
  25
  26  It has preliminary support for Matthew Roberts advance algorithm
  27     http://reprap.org/pipermail/reprap-dev/2011-May/003323.html
  28  */
  29
  30 #include "Marlin.h"
  31
  32 #include "ultralcd.h"
  33 #include "planner.h"
  34 #include "stepper.h"
  35 #include "temperature.h"
  36 #include "motion_control.h"
  37 #include "cardreader.h"
  38 #include "watchdog.h"
  39 #include "EEPROMwrite.h"
  40 #include "language.h"
  41 #include "pins_arduino.h"
  42
  43 #define VERSION_STRING  "1.0.0 RC2"
  44
  45 // look here for descriptions of gcodes: http://linuxcnc.org/handbook/gcode/g-code.html
  46 // http://objects.reprap.org/wiki/Mendel_User_Manual:_RepRapGCodes
  47
  48 //Implemented Codes
  49 //-------------------
  50 // G0  -> G1
  51 // G1  - Coordinated Movement X Y Z E
  52 // G2  - CW ARC
  53 // G3  - CCW ARC
  54 // G4  - Dwell S<seconds> or P<milliseconds>
  55 // G10 - retract filament according to settings of M207
  56 // G11 - retract recover filament according to settings of M208
  57 // G28 - Home all Axis
  58 // G90 - Use Absolute Coordinates
  59 // G91 - Use Relative Coordinates
  60 // G92 - Set current position to cordinates given
  61
  62 //RepRap M Codes
  63 // M0   - Unconditional stop - Wait for user to press a button on the LCD (Only if ULTRA_LCD is enabled)
  64 // M1   - Same as M0
  65 // M104 - Set extruder target temp
  66 // M105 - Read current temp
  67 // M106 - Fan on
  68 // M107 - Fan off
  69 // M109 - Wait for extruder current temp to reach target temp.
  70 // M114 - Display current position
  71
  72 //Custom M Codes
  73 // M17  - Enable/Power all stepper motors
  74 // M18  - Disable all stepper motors; same as M84
  75 // M20  - List SD card
  76 // M21  - Init SD card
  77 // M22  - Release SD card
  78 // M23  - Select SD file (M23 filename.g)
  79 // M24  - Start/resume SD print
  80 // M25  - Pause SD print
  81 // M26  - Set SD position in bytes (M26 S12345)
  82 // M27  - Report SD print status
  83 // M28  - Start SD write (M28 filename.g)
  84 // M29  - Stop SD write
  85 // M30  - Delete file from SD (M30 filename.g)
  86 // M31  - Output time since last M109 or SD card start to serial
  87 // M42  - Change pin status via gcode
  88 // M80  - Turn on Power Supply
  89 // M81  - Turn off Power Supply
  90 // M82  - Set E codes absolute (default)
  91 // M83  - Set E codes relative while in Absolute Coordinates (G90) mode
  92 // M84  - Disable steppers until next move,
  93 //        or use S<seconds> to specify an inactivity timeout, after which the steppers will be disabled.  S0 to disable the timeout.
  94 // M85  - Set inactivity shutdown timer with parameter S<seconds>. To disable set zero (default)
  95 // M92  - Set axis_steps_per_unit - same syntax as G92
  96 // M114 - Output current position to serial port
  97 // M115 - Capabilities string
  98 // M117 - display message
  99 // M119 - Output Endstop status to serial port
 100 // M140 - Set bed target temp
 101 // M190 - Wait for bed current temp to reach target temp.
 102 // M200 - Set filament diameter
 103 // M201 - Set max acceleration in units/s^2 for print moves (M201 X1000 Y1000)
 104 // M202 - Set max acceleration in units/s^2 for travel moves (M202 X1000 Y1000) Unused in Marlin!!
 105 // M203 - Set maximum feedrate that your machine can sustain (M203 X200 Y200 Z300 E10000) in mm/sec
 106 // M204 - Set default acceleration: S normal moves T filament only moves (M204 S3000 T7000) im mm/sec^2  also sets minimum segment time in ms (B20000) to prevent buffer underruns and M20 minimum feedrate
 107 // M205 -  advanced settings:  minimum travel speed S=while printing T=travel only,  B=minimum segment time X= maximum xy jerk, Z=maximum Z jerk, E=maximum E jerk
 108 // M206 - set additional homeing offset
 109 // M207 - set retract length S[positive mm] F[feedrate mm/sec] Z[additional zlift/hop]
 110 // M208 - set recover=unretract length S[positive mm surplus to the M207 S*] F[feedrate mm/sec]
 111 // M209 - S<1=true/0=false> enable automatic retract detect if the slicer did not support G10/11: every normal extrude-only move will be classified as retract depending on the direction.
 112 // M220 S<factor in percent>- set speed factor override percentage
 113 // M221 S<factor in percent>- set extrude factor override percentage
 114 // M240 - Trigger a camera to take a photograph
 115 // M301 - Set PID parameters P I and D
 116 // M302 - Allow cold extrudes
 117 // M303 - PID relay autotune S<temperature> sets the target temperature. (default target temperature = 150C)
 118 // M400 - Finish all moves
 119 // M500 - stores paramters in EEPROM
 120 // M501 - reads parameters from EEPROM (if you need reset them after you changed them temporarily).
 121 // M502 - reverts to the default "factory settings".  You still need to store them in EEPROM afterwards if you want to.
 122 // M503 - print the current settings (from memory not from eeprom)
 123 // M999 - Restart after being stopped by error
 124
 125 //Stepper Movement Variables
 126
 127 //===========================================================================
 128 //=============================imported variables============================
 129 //===========================================================================
 130
 131
 132 //===========================================================================
 133 //=============================public variables=============================
 134 //===========================================================================
 135 #ifdef SDSUPPORT
 136 CardReader card;
 137 #endif
 138 float homing_feedrate[] = HOMING_FEEDRATE;
 139 bool axis_relative_modes[] = AXIS_RELATIVE_MODES;
 140 volatile int feedmultiply=100; //100->1 200->2
 141 int saved_feedmultiply;
 142 volatile bool feedmultiplychanged=false;
 143 volatile int extrudemultiply=100; //100->1 200->2
 144 float current_position[NUM_AXIS] = { 0.0, 0.0, 0.0, 0.0 };
 145 float add_homeing[3]={0,0,0};
 146 float min_pos[3] = { X_MIN_POS, Y_MIN_POS, Z_MIN_POS };
 147 float max_pos[3] = { X_MAX_POS, Y_MAX_POS, Z_MAX_POS };
 148 uint8_t active_extruder = 0;
 149 unsigned char FanSpeed=0;
 150
 151 #ifdef FWRETRACT
 152   bool autoretract_enabled=true;
 153   bool retracted=false;
 154   float retract_length=3, retract_feedrate=17*60, retract_zlift=0.8;
 155   float retract_recover_length=0, retract_recover_feedrate=8*60;
 156 #endif
 157
 158 //===========================================================================
 159 //=============================private variables=============================
 160 //===========================================================================
 161 const char axis_codes[NUM_AXIS] = {'X', 'Y', 'Z', 'E'};
 162 static float destination[NUM_AXIS] = {  0.0, 0.0, 0.0, 0.0};
 163 static float offset[3] = {0.0, 0.0, 0.0};
 164 static bool home_all_axis = true;
 165 static float feedrate = 1500.0, next_feedrate, saved_feedrate;
 166 static long gcode_N, gcode_LastN, Stopped_gcode_LastN = 0;
 167
 168 static bool relative_mode = false;  //Determines Absolute or Relative Coordinates
 169 static bool relative_mode_e = false;  //Determines Absolute or Relative E Codes while in Absolute Coordinates mode. E is always relative in Relative Coordinates mode.
 170
 171 static char cmdbuffer[BUFSIZE][MAX_CMD_SIZE];
 172 static bool fromsd[BUFSIZE];
 173 static int bufindr = 0;
 174 static int bufindw = 0;
 175 static int buflen = 0;
 176 //static int i = 0;
 177 static char serial_char;
 178 static int serial_count = 0;
 179 static boolean comment_mode = false;
 180 static char *strchr_pointer; // just a pointer to find chars in the cmd string like X, Y, Z, E, etc
 181
 182 const int sensitive_pins[] = SENSITIVE_PINS; // Sensitive pin list for M42
 183
 184 //static float tt = 0;
 185 //static float bt = 0;
 186
 187 //Inactivity shutdown variables
 188 static unsigned long previous_millis_cmd = 0;
 189 static unsigned long max_inactive_time = 0;
 190 static unsigned long stepper_inactive_time = DEFAULT_STEPPER_DEACTIVE_TIME*1000l;
 191
 192 static unsigned long starttime=0;
 193 static unsigned long stoptime=0;
 194
 195 static uint8_t tmp_extruder;
 196
 197
 198 bool Stopped=false;
 199
 200 //===========================================================================
 201 //=============================ROUTINES=============================
 202 //===========================================================================
 203
 204 void get_arc_coordinates();
 205
 206 extern "C"{
 207   extern unsigned int __bss_end;
 208   extern unsigned int __heap_start;
 209   extern void *__brkval;
 210
 211   int freeMemory() {
 212     int free_memory;
 213
 214     if((int)__brkval == 0)
 215       free_memory = ((int)&free_memory) - ((int)&__bss_end);
 216     else
 217       free_memory = ((int)&free_memory) - ((int)__brkval);
 218
 219     return free_memory;
 220   }
 221 }
 222
 223 //adds an command to the main command buffer
 224 //thats really done in a non-safe way.
 225 //needs overworking someday
 226 void enquecommand(const char *cmd)
 227 {
 228   if(buflen < BUFSIZE)
 229   {
 230     //this is dangerous if a mixing of serial and this happsens
 231     strcpy(&(cmdbuffer[bufindw][0]),cmd);
 232     SERIAL_ECHO_START;
 233     SERIAL_ECHOPGM("enqueing \"");
 234     SERIAL_ECHO(cmdbuffer[bufindw]);
 235     SERIAL_ECHOLNPGM("\"");
 236     bufindw= (bufindw + 1)%BUFSIZE;
 237     buflen += 1;
 238   }
 239 }
 240
 241 void setup_photpin()
 242 {
 243   #ifdef PHOTOGRAPH_PIN
 244     #if (PHOTOGRAPH_PIN > -1)
 245     SET_OUTPUT(PHOTOGRAPH_PIN);
 246     WRITE(PHOTOGRAPH_PIN, LOW);
 247     #endif
 248   #endif
 249 }
 250
 251 void setup_powerhold()
 252 {
 253  #ifdef SUICIDE_PIN
 254    #if (SUICIDE_PIN> -1)
 255       SET_OUTPUT(SUICIDE_PIN);
 256       WRITE(SUICIDE_PIN, HIGH);
 257    #endif
 258  #endif
 259 }
 260
 261 void suicide()
 262 {
 263  #ifdef SUICIDE_PIN
 264     #if (SUICIDE_PIN> -1)
 265       SET_OUTPUT(SUICIDE_PIN);
 266       WRITE(SUICIDE_PIN, LOW);
 267     #endif
 268   #endif
 269 }
 270
 271 void setup()
 272 {
 273   setup_powerhold();
 274   MYSERIAL.begin(BAUDRATE);
 275   SERIAL_PROTOCOLLNPGM("start");
 276   SERIAL_ECHO_START;
 277
 278   // Check startup - does nothing if bootloader sets MCUSR to 0
 279   byte mcu = MCUSR;
 280   if(mcu & 1) SERIAL_ECHOLNPGM(MSG_POWERUP);
 281   if(mcu & 2) SERIAL_ECHOLNPGM(MSG_EXTERNAL_RESET);
 282   if(mcu & 4) SERIAL_ECHOLNPGM(MSG_BROWNOUT_RESET);
 283   if(mcu & 8) SERIAL_ECHOLNPGM(MSG_WATCHDOG_RESET);
 284   if(mcu & 32) SERIAL_ECHOLNPGM(MSG_SOFTWARE_RESET);
 285   MCUSR=0;
 286
 287   SERIAL_ECHOPGM(MSG_MARLIN);
 288   SERIAL_ECHOLNPGM(VERSION_STRING);
 289   #ifdef STRING_VERSION_CONFIG_H
 290     #ifdef STRING_CONFIG_H_AUTHOR
 291       SERIAL_ECHO_START;
 292       SERIAL_ECHOPGM(MSG_CONFIGURATION_VER);
 293       SERIAL_ECHOPGM(STRING_VERSION_CONFIG_H);
 294       SERIAL_ECHOPGM(MSG_AUTHOR);
 295       SERIAL_ECHOLNPGM(STRING_CONFIG_H_AUTHOR);
 296     #endif
 297   #endif
 298   SERIAL_ECHO_START;
 299   SERIAL_ECHOPGM(MSG_FREE_MEMORY);
 300   SERIAL_ECHO(freeMemory());
 301   SERIAL_ECHOPGM(MSG_PLANNER_BUFFER_BYTES);
 302   SERIAL_ECHOLN((int)sizeof(block_t)*BLOCK_BUFFER_SIZE);
 303   for(int8_t i = 0; i < BUFSIZE; i++)
 304   {
 305     fromsd[i] = false;
 306   }
 307
 308   EEPROM_RetrieveSettings(); // loads data from EEPROM if available
 309
 310   for(int8_t i=0; i < NUM_AXIS; i++)
 311   {
 312     axis_steps_per_sqr_second[i] = max_acceleration_units_per_sq_second[i] * axis_steps_per_unit[i];
 313   }
 314
 315
 316   tp_init();    // Initialize temperature loop
 317   plan_init();  // Initialize planner;
 318   st_init();    // Initialize stepper;
 319   wd_init();
 320   setup_photpin();
 321
 322   LCD_INIT;
 323 }
 324
 325
 326 void loop()
 327 {
 328   if(buflen < (BUFSIZE-1))
 329     get_command();
 330   #ifdef SDSUPPORT
 331   card.checkautostart(false);
 332   #endif
 333   if(buflen)
 334   {
 335     #ifdef SDSUPPORT
 336       if(card.saving)
 337       {
 338         if(strstr(cmdbuffer[bufindr],"M29") == NULL)
 339         {
 340           card.write_command(cmdbuffer[bufindr]);
 341           SERIAL_PROTOCOLLNPGM(MSG_OK);
 342         }
 343         else
 344         {
 345           card.closefile();
 346           SERIAL_PROTOCOLLNPGM(MSG_FILE_SAVED);
 347         }
 348       }
 349       else
 350       {
 351         process_commands();
 352       }
 353     #else
 354       process_commands();
 355     #endif //SDSUPPORT
 356     buflen = (buflen-1);
 357     bufindr = (bufindr + 1)%BUFSIZE;
 358   }
 359   //check heater every n milliseconds
 360   manage_heater();
 361   manage_inactivity(1);
 362   checkHitEndstops();
 363   LCD_STATUS;
 364 }
 365
 366 void get_command()
 367 {
 368   while( MYSERIAL.available() > 0  && buflen < BUFSIZE) {
 369     serial_char = MYSERIAL.read();
 370     if(serial_char == '\n' ||
 371        serial_char == '\r' ||
 372        (serial_char == ':' && comment_mode == false) ||
 373        serial_count >= (MAX_CMD_SIZE - 1) )
 374     {
 375       if(!serial_count) { //if empty line
 376         comment_mode = false; //for new command
 377         return;
 378       }
 379       cmdbuffer[bufindw][serial_count] = 0; //terminate string
 380       if(!comment_mode){
 381         comment_mode = false; //for new command
 382         fromsd[bufindw] = false;
 383         if(strstr(cmdbuffer[bufindw], "N") != NULL)
 384         {
 385           strchr_pointer = strchr(cmdbuffer[bufindw], 'N');
 386           gcode_N = (strtol(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL, 10));
 387           if(gcode_N != gcode_LastN+1 && (strstr(cmdbuffer[bufindw], "M110") == NULL) ) {
 388             SERIAL_ERROR_START;
 389             SERIAL_ERRORPGM(MSG_ERR_LINE_NO);
 390             SERIAL_ERRORLN(gcode_LastN);
 391             //Serial.println(gcode_N);
 392             FlushSerialRequestResend();
 393             serial_count = 0;
 394             return;
 395           }
 396
 397           if(strstr(cmdbuffer[bufindw], "*") != NULL)
 398           {
 399             byte checksum = 0;
 400             byte count = 0;
 401             while(cmdbuffer[bufindw][count] != '*') checksum = checksum^cmdbuffer[bufindw][count++];
 402             strchr_pointer = strchr(cmdbuffer[bufindw], '*');
 403
 404             if( (int)(strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)) != checksum) {
 405               SERIAL_ERROR_START;
 406               SERIAL_ERRORPGM(MSG_ERR_CHECKSUM_MISMATCH);
 407               SERIAL_ERRORLN(gcode_LastN);
 408               FlushSerialRequestResend();
 409               serial_count = 0;
 410               return;
 411             }
 412             //if no errors, continue parsing
 413           }
 414           else
 415           {
 416             SERIAL_ERROR_START;
 417             SERIAL_ERRORPGM(MSG_ERR_NO_CHECKSUM);
 418             SERIAL_ERRORLN(gcode_LastN);
 419             FlushSerialRequestResend();
 420             serial_count = 0;
 421             return;
 422           }
 423
 424           gcode_LastN = gcode_N;
 425           //if no errors, continue parsing
 426         }
 427         else  // if we don't receive 'N' but still see '*'
 428         {
 429           if((strstr(cmdbuffer[bufindw], "*") != NULL))
 430           {
 431             SERIAL_ERROR_START;
 432             SERIAL_ERRORPGM(MSG_ERR_NO_LINENUMBER_WITH_CHECKSUM);
 433             SERIAL_ERRORLN(gcode_LastN);
 434             serial_count = 0;
 435             return;
 436           }
 437         }
 438         if((strstr(cmdbuffer[bufindw], "G") != NULL)){
 439           strchr_pointer = strchr(cmdbuffer[bufindw], 'G');
 440           switch((int)((strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)))){
 441           case 0:
 442           case 1:
 443           case 2:
 444           case 3:
 445             if(Stopped == false) { // If printer is stopped by an error the G[0-3] codes are ignored.
 446               #ifdef SDSUPPORT
 447               if(card.saving)
 448                 break;
 449               #endif //SDSUPPORT
 450               SERIAL_PROTOCOLLNPGM(MSG_OK);
 451             }
 452             else {
 453               SERIAL_ERRORLNPGM(MSG_ERR_STOPPED);
 454               LCD_MESSAGEPGM(MSG_STOPPED);
 455             }
 456             break;
 457           default:
 458             break;
 459           }
 460
 461         }
 462         bufindw = (bufindw + 1)%BUFSIZE;
 463         buflen += 1;
 464       }
 465       serial_count = 0; //clear buffer
 466     }
 467     else
 468     {
 469       if(serial_char == ';') comment_mode = true;
 470       if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;
 471     }
 472   }
 473   #ifdef SDSUPPORT
 474   if(!card.sdprinting || serial_count!=0){
 475     return;
 476   }
 477   while( !card.eof()  && buflen < BUFSIZE) {
 478     int16_t n=card.get();
 479     serial_char = (char)n;
 480     if(serial_char == '\n' ||
 481        serial_char == '\r' ||
 482        (serial_char == ':' && comment_mode == false) ||
 483        serial_count >= (MAX_CMD_SIZE - 1)||n==-1)
 484     {
 485       if(card.eof()){
 486         SERIAL_PROTOCOLLNPGM(MSG_FILE_PRINTED);
 487         stoptime=millis();
 488         char time[30];
 489         unsigned long t=(stoptime-starttime)/1000;
 490         int sec,min;
 491         min=t/60;
 492         sec=t%60;
 493         sprintf(time,"%i min, %i sec",min,sec);
 494         SERIAL_ECHO_START;
 495         SERIAL_ECHOLN(time);
 496         LCD_MESSAGE(time);
 497         card.printingHasFinished();
 498         card.checkautostart(true);
 499
 500       }
 501       if(!serial_count)
 502       {
 503         comment_mode = false; //for new command
 504         return; //if empty line
 505       }
 506       cmdbuffer[bufindw][serial_count] = 0; //terminate string
 507 //      if(!comment_mode){
 508         fromsd[bufindw] = true;
 509         buflen += 1;
 510         bufindw = (bufindw + 1)%BUFSIZE;
 511 //      }
 512       comment_mode = false; //for new command
 513       serial_count = 0; //clear buffer
 514     }
 515     else
 516     {
 517       if(serial_char == ';') comment_mode = true;
 518       if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;
 519     }
 520   }
 521
 522   #endif //SDSUPPORT
 523
 524 }
 525
 526
 527 float code_value()
 528 {
 529   return (strtod(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL));
 530 }
 531
 532 long code_value_long()
 533 {
 534   return (strtol(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL, 10));
 535 }
 536
 537 bool code_seen(char code_string[]) //Return True if the string was found
 538 {
 539   return (strstr(cmdbuffer[bufindr], code_string) != NULL);
 540 }
 541
 542 bool code_seen(char code)
 543 {
 544   strchr_pointer = strchr(cmdbuffer[bufindr], code);
 545   return (strchr_pointer != NULL);  //Return True if a character was found
 546 }
 547
 548 #define DEFINE_PGM_READ_ANY(type, reader)               \
 549     static inline float pgm_read_any(const type *p)     \
 550         { return pgm_read_##reader##_near(p); }
 551
 552 DEFINE_PGM_READ_ANY(float,       float);
 553
 554 #define XYZ_CONSTS_FROM_CONFIG(type, array, CONFIG)     \
 555 static const PROGMEM type array##_P[3] =                \
 556     { X_##CONFIG, Y_##CONFIG, Z_##CONFIG };             \
 557 static inline type array(int axis)                      \
 558     { return pgm_read_any(&array##_P[axis]); }
 559
 560 XYZ_CONSTS_FROM_CONFIG(float, base_min_pos,    MIN_POS);
 561 XYZ_CONSTS_FROM_CONFIG(float, base_max_pos,    MAX_POS);
 562 XYZ_CONSTS_FROM_CONFIG(float, base_home_pos,   HOME_POS);
 563
 564 static void axis_is_at_home(int axis) {
 565   current_position[axis] = base_home_pos(axis) + add_homeing[axis];
 566   min_pos[axis] =          base_min_pos(axis) + add_homeing[axis];
 567   max_pos[axis] =          base_max_pos(axis) + add_homeing[axis];
 568 }
 569
 570 #define HOMEAXIS(LETTER) \
 571   if ((LETTER##_MIN_PIN > -1 && LETTER##_HOME_DIR==-1) || (LETTER##_MAX_PIN > -1 && LETTER##_HOME_DIR==1))\
 572     { \
 573     current_position[LETTER##_AXIS] = 0; \
 574     plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); \
 575     destination[LETTER##_AXIS] = 1.5 * LETTER##_MAX_LENGTH * LETTER##_HOME_DIR; \
 576     feedrate = homing_feedrate[LETTER##_AXIS]; \
 577     plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder); \
 578     st_synchronize();\
 579     \
 580     current_position[LETTER##_AXIS] = 0;\
 581     plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);\
 582     destination[LETTER##_AXIS] = -LETTER##_HOME_RETRACT_MM * LETTER##_HOME_DIR;\
 583     plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder); \
 584     st_synchronize();\
 585     \
 586     destination[LETTER##_AXIS] = 2*LETTER##_HOME_RETRACT_MM * LETTER##_HOME_DIR;\
 587     feedrate = homing_feedrate[LETTER##_AXIS]/2 ;  \
 588     plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder); \
 589     st_synchronize();\
 590     \
 591     axis_is_at_home(LETTER##_AXIS);                                     \
 592     destination[LETTER##_AXIS] = current_position[LETTER##_AXIS]; \
 593     feedrate = 0.0;\
 594     endstops_hit_on_purpose();\
 595   }
 596
 597 void process_commands()
 598 {
 599   unsigned long codenum; //throw away variable
 600   char *starpos = NULL;
 601
 602   if(code_seen('G'))
 603   {
 604     switch((int)code_value())
 605     {
 606     case 0: // G0 -> G1
 607     case 1: // G1
 608       if(Stopped == false) {
 609         get_coordinates(); // For X Y Z E F
 610         prepare_move();
 611         //ClearToSend();
 612         return;
 613       }
 614       //break;
 615     case 2: // G2  - CW ARC
 616       if(Stopped == false) {
 617         get_arc_coordinates();
 618         prepare_arc_move(true);
 619         return;
 620       }
 621     case 3: // G3  - CCW ARC
 622       if(Stopped == false) {
 623         get_arc_coordinates();
 624         prepare_arc_move(false);
 625         return;
 626       }
 627     case 4: // G4 dwell
 628       LCD_MESSAGEPGM(MSG_DWELL);
 629       codenum = 0;
 630       if(code_seen('P')) codenum = code_value(); // milliseconds to wait
 631       if(code_seen('S')) codenum = code_value() * 1000; // seconds to wait
 632
 633       st_synchronize();
 634       codenum += millis();  // keep track of when we started waiting
 635       previous_millis_cmd = millis();
 636       while(millis()  < codenum ){
 637         manage_heater();
 638         manage_inactivity(1);
 639                 LCD_STATUS;
 640       }
 641       break;
 642       #ifdef FWRETRACT
 643       case 10: // G10 retract
 644       if(!retracted)
 645       {
 646         destination[X_AXIS]=current_position[X_AXIS];
 647         destination[Y_AXIS]=current_position[Y_AXIS];
 648         destination[Z_AXIS]=current_position[Z_AXIS];
 649         current_position[Z_AXIS]+=-retract_zlift;
 650         destination[E_AXIS]=current_position[E_AXIS]-retract_length;
 651         feedrate=retract_feedrate;
 652         retracted=true;
 653         prepare_move();
 654       }
 655
 656       break;
 657       case 11: // G10 retract_recover
 658       if(!retracted)
 659       {
 660         destination[X_AXIS]=current_position[X_AXIS];
 661         destination[Y_AXIS]=current_position[Y_AXIS];
 662         destination[Z_AXIS]=current_position[Z_AXIS];
 663
 664         current_position[Z_AXIS]+=retract_zlift;
 665         current_position[E_AXIS]+=-retract_recover_length;
 666         feedrate=retract_recover_feedrate;
 667         retracted=false;
 668         prepare_move();
 669       }
 670       break;
 671       #endif //FWRETRACT
 672     case 28: //G28 Home all Axis one at a time
 673       saved_feedrate = feedrate;
 674       saved_feedmultiply = feedmultiply;
 675       feedmultiply = 100;
 676       previous_millis_cmd = millis();
 677
 678       enable_endstops(true);
 679
 680       for(int8_t i=0; i < NUM_AXIS; i++) {
 681         destination[i] = current_position[i];
 682       }
 683       feedrate = 0.0;
 684       home_all_axis = !((code_seen(axis_codes[0])) || (code_seen(axis_codes[1])) || (code_seen(axis_codes[2])));
 685
 686       #if Z_HOME_DIR > 0                      // If homing away from BED do Z first
 687       if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) {
 688         HOMEAXIS(Z);
 689       }
 690       #endif
 691
 692       #ifdef QUICK_HOME
 693       if((home_all_axis)||( code_seen(axis_codes[X_AXIS]) && code_seen(axis_codes[Y_AXIS])) )  //first diagonal move
 694       {
 695         current_position[X_AXIS] = 0;current_position[Y_AXIS] = 0;
 696
 697         plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
 698         destination[X_AXIS] = 1.5 * X_MAX_LENGTH * X_HOME_DIR;destination[Y_AXIS] = 1.5 * Y_MAX_LENGTH * Y_HOME_DIR;
 699         feedrate = homing_feedrate[X_AXIS];
 700         if(homing_feedrate[Y_AXIS]<feedrate)
 701           feedrate =homing_feedrate[Y_AXIS];
 702         plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
 703         st_synchronize();
 704
 705         axis_is_at_home(X_AXIS);
 706         axis_is_at_home(Y_AXIS);
 707         plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
 708         destination[X_AXIS] = current_position[X_AXIS];
 709         destination[Y_AXIS] = current_position[Y_AXIS];
 710         plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
 711         feedrate = 0.0;
 712         st_synchronize();
 713         endstops_hit_on_purpose();
 714       }
 715       #endif
 716
 717       if((home_all_axis) || (code_seen(axis_codes[X_AXIS])))
 718       {
 719         HOMEAXIS(X);
 720       }
 721
 722       if((home_all_axis) || (code_seen(axis_codes[Y_AXIS]))) {
 723         HOMEAXIS(Y);
 724       }
 725
 726       #if Z_HOME_DIR < 0                      // If homing towards BED do Z last
 727       if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) {
 728         HOMEAXIS(Z);
 729       }
 730       #endif
 731
 732       if(code_seen(axis_codes[X_AXIS]))
 733       {
 734         if(code_value_long() != 0) {
 735           current_position[X_AXIS]=code_value()+add_homeing[0];
 736         }
 737       }
 738
 739       if(code_seen(axis_codes[Y_AXIS])) {
 740         if(code_value_long() != 0) {
 741           current_position[Y_AXIS]=code_value()+add_homeing[1];
 742         }
 743       }
 744
 745       if(code_seen(axis_codes[Z_AXIS])) {
 746         if(code_value_long() != 0) {
 747           current_position[Z_AXIS]=code_value()+add_homeing[2];
 748         }
 749       }
 750       plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
 751
 752       #ifdef ENDSTOPS_ONLY_FOR_HOMING
 753         enable_endstops(false);
 754       #endif
 755
 756       feedrate = saved_feedrate;
 757       feedmultiply = saved_feedmultiply;
 758       previous_millis_cmd = millis();
 759       endstops_hit_on_purpose();
 760       break;
 761     case 90: // G90
 762       relative_mode = false;
 763       break;
 764     case 91: // G91
 765       relative_mode = true;
 766       break;
 767     case 92: // G92
 768       if(!code_seen(axis_codes[E_AXIS]))
 769         st_synchronize();
 770       for(int8_t i=0; i < NUM_AXIS; i++) {
 771         if(code_seen(axis_codes[i])) {
 772            if(i == E_AXIS) {
 773              current_position[i] = code_value();
 774              plan_set_e_position(current_position[E_AXIS]);
 775            }
 776            else {
 777              current_position[i] = code_value()+add_homeing[i];
 778              plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
 779            }
 780         }
 781       }
 782       break;
 783     }
 784   }
 785
 786   else if(code_seen('M'))
 787   {
 788     switch( (int)code_value() )
 789     {
 790 #ifdef ULTRA_LCD
 791     case 0: // M0 - Unconditional stop - Wait for user button press on LCD
 792     case 1: // M1 - Conditional stop - Wait for user button press on LCD
 793     {
 794       LCD_MESSAGEPGM(MSG_USERWAIT);
 795       codenum = 0;
 796       if(code_seen('P')) codenum = code_value(); // milliseconds to wait
 797       if(code_seen('S')) codenum = code_value() * 1000; // seconds to wait
 798
 799       st_synchronize();
 800       previous_millis_cmd = millis();
 801           if (codenum > 0)
 802           {
 803         codenum += millis();  // keep track of when we started waiting
 804         while(millis()  < codenum && !CLICKED){
 805           manage_heater();
 806           manage_inactivity(1);
 807                   LCD_STATUS;
 808                 }
 809       }else{
 810         while(!CLICKED) {
 811           manage_heater();
 812           manage_inactivity(1);
 813                   LCD_STATUS;
 814                 }
 815           }
 816     }
 817     break;
 818 #endif
 819     case 17:
 820         LCD_MESSAGEPGM(MSG_NO_MOVE);
 821         enable_x();
 822         enable_y();
 823         enable_z();
 824         enable_e0();
 825         enable_e1();
 826         enable_e2();
 827       break;
 828
 829 #ifdef SDSUPPORT
 830     case 20: // M20 - list SD card
 831       SERIAL_PROTOCOLLNPGM(MSG_BEGIN_FILE_LIST);
 832       card.ls();
 833       SERIAL_PROTOCOLLNPGM(MSG_END_FILE_LIST);
 834       break;
 835     case 21: // M21 - init SD card
 836
 837       card.initsd();
 838
 839       break;
 840     case 22: //M22 - release SD card
 841       card.release();
 842
 843       break;
 844     case 23: //M23 - Select file
 845       starpos = (strchr(strchr_pointer + 4,'*'));
 846       if(starpos!=NULL)
 847         *(starpos-1)='\0';
 848       card.openFile(strchr_pointer + 4,true);
 849       break;
 850     case 24: //M24 - Start SD print
 851       card.startFileprint();
 852       starttime=millis();
 853       break;
 854     case 25: //M25 - Pause SD print
 855       card.pauseSDPrint();
 856       break;
 857     case 26: //M26 - Set SD index
 858       if(card.cardOK && code_seen('S')) {
 859         card.setIndex(code_value_long());
 860       }
 861       break;
 862     case 27: //M27 - Get SD status
 863       card.getStatus();
 864       break;
 865     case 28: //M28 - Start SD write
 866       starpos = (strchr(strchr_pointer + 4,'*'));
 867       if(starpos != NULL){
 868         char* npos = strchr(cmdbuffer[bufindr], 'N');
 869         strchr_pointer = strchr(npos,' ') + 1;
 870         *(starpos-1) = '\0';
 871       }
 872       card.openFile(strchr_pointer+4,false);
 873       break;
 874     case 29: //M29 - Stop SD write
 875       //processed in write to file routine above
 876       //card,saving = false;
 877       break;
 878     case 30: //M30 <filename> Delete File
 879         if (card.cardOK){
 880                 card.closefile();
 881                 starpos = (strchr(strchr_pointer + 4,'*'));
 882                 if(starpos != NULL){
 883                 char* npos = strchr(cmdbuffer[bufindr], 'N');
 884                 strchr_pointer = strchr(npos,' ') + 1;
 885                 *(starpos-1) = '\0';
 886          }
 887          card.removeFile(strchr_pointer + 4);
 888         }
 889         break;
 890
 891 #endif //SDSUPPORT
 892
 893     case 31: //M31 take time since the start of the SD print or an M109 command
 894       {
 895       stoptime=millis();
 896       char time[30];
 897       unsigned long t=(stoptime-starttime)/1000;
 898       int sec,min;
 899       min=t/60;
 900       sec=t%60;
 901       sprintf(time,"%i min, %i sec",min,sec);
 902       SERIAL_ECHO_START;
 903       SERIAL_ECHOLN(time);
 904       LCD_MESSAGE(time);
 905       autotempShutdown();
 906       }
 907       break;
 908     case 42: //M42 -Change pin status via gcode
 909       if (code_seen('S'))
 910       {
 911         int pin_status = code_value();
 912         if (code_seen('P') && pin_status >= 0 && pin_status <= 255)
 913         {
 914           int pin_number = code_value();
 915           for(int8_t i = 0; i < (int8_t)sizeof(sensitive_pins); i++)
 916           {
 917             if (sensitive_pins[i] == pin_number)
 918             {
 919               pin_number = -1;
 920               break;
 921             }
 922           }
 923
 924           if (pin_number > -1)
 925           {
 926             pinMode(pin_number, OUTPUT);
 927             digitalWrite(pin_number, pin_status);
 928             analogWrite(pin_number, pin_status);
 929           }
 930         }
 931       }
 932      break;
 933     case 104: // M104
 934       tmp_extruder = active_extruder;
 935       if(code_seen('T')) {
 936         tmp_extruder = code_value();
 937         if(tmp_extruder >= EXTRUDERS) {
 938           SERIAL_ECHO_START;
 939           SERIAL_ECHO(MSG_M104_INVALID_EXTRUDER);
 940           SERIAL_ECHOLN(tmp_extruder);
 941           break;
 942         }
 943       }
 944       if (code_seen('S')) setTargetHotend(code_value(), tmp_extruder);
 945       setWatch();
 946       break;
 947     case 140: // M140 set bed temp
 948       if (code_seen('S')) setTargetBed(code_value());
 949       break;
 950     case 105 : // M105
 951       tmp_extruder = active_extruder;
 952       if(code_seen('T')) {
 953         tmp_extruder = code_value();
 954         if(tmp_extruder >= EXTRUDERS) {
 955           SERIAL_ECHO_START;
 956           SERIAL_ECHO(MSG_M105_INVALID_EXTRUDER);
 957           SERIAL_ECHOLN(tmp_extruder);
 958           break;
 959         }
 960       }
 961       #if (TEMP_0_PIN > -1)
 962         SERIAL_PROTOCOLPGM("ok T:");
 963         SERIAL_PROTOCOL_F(degHotend(tmp_extruder),1);
 964         SERIAL_PROTOCOLPGM(" /");
 965         SERIAL_PROTOCOL_F(degTargetHotend(tmp_extruder),1);
 966         #if TEMP_BED_PIN > -1
 967           SERIAL_PROTOCOLPGM(" B:");
 968           SERIAL_PROTOCOL_F(degBed(),1);
 969           SERIAL_PROTOCOLPGM(" /");
 970           SERIAL_PROTOCOL_F(degTargetBed(),1);
 971         #endif //TEMP_BED_PIN
 972       #else
 973         SERIAL_ERROR_START;
 974         SERIAL_ERRORLNPGM(MSG_ERR_NO_THERMISTORS);
 975       #endif
 976       #ifdef PIDTEMP
 977         SERIAL_PROTOCOLPGM(" @:");
 978         SERIAL_PROTOCOL(getHeaterPower(tmp_extruder));
 979       #endif
 980         SERIAL_PROTOCOLLN("");
 981       return;
 982       break;
 983     case 109:
 984     {// M109 - Wait for extruder heater to reach target.
 985       tmp_extruder = active_extruder;
 986       if(code_seen('T')) {
 987         tmp_extruder = code_value();
 988         if(tmp_extruder >= EXTRUDERS) {
 989           SERIAL_ECHO_START;
 990           SERIAL_ECHO(MSG_M109_INVALID_EXTRUDER);
 991           SERIAL_ECHOLN(tmp_extruder);
 992           break;
 993         }
 994       }
 995       LCD_MESSAGEPGM(MSG_HEATING);
 996       #ifdef AUTOTEMP
 997         autotemp_enabled=false;
 998       #endif
 999       if (code_seen('S')) setTargetHotend(code_value(), tmp_extruder);
1000       #ifdef AUTOTEMP
1001         if (code_seen('S')) autotemp_min=code_value();
1002         if (code_seen('B')) autotemp_max=code_value();
1003         if (code_seen('F'))
1004         {
1005           autotemp_factor=code_value();
1006           autotemp_enabled=true;
1007         }
1008       #endif
1009
1010       setWatch();
1011       codenum = millis();
1012
1013       /* See if we are heating up or cooling down */
1014       bool target_direction = isHeatingHotend(tmp_extruder); // true if heating, false if cooling
1015
1016       #ifdef TEMP_RESIDENCY_TIME
1017         long residencyStart;
1018         residencyStart = -1;
1019         /* continue to loop until we have reached the target temp
1020           _and_ until TEMP_RESIDENCY_TIME hasn't passed since we reached it */
1021         while((residencyStart == -1) ||
1022               (residencyStart >= 0 && (((unsigned int) (millis() - residencyStart)) < (TEMP_RESIDENCY_TIME * 1000UL))) ) {
1023       #else
1024         while ( target_direction ? (isHeatingHotend(tmp_extruder)) : (isCoolingHotend(tmp_extruder)&&(CooldownNoWait==false)) ) {
1025       #endif //TEMP_RESIDENCY_TIME
1026           if( (millis() - codenum) > 1000UL )
1027           { //Print Temp Reading and remaining time every 1 second while heating up/cooling down
1028             SERIAL_PROTOCOLPGM("T:");
1029             SERIAL_PROTOCOL_F(degHotend(tmp_extruder),1);
1030             SERIAL_PROTOCOLPGM(" E:");
1031             SERIAL_PROTOCOL((int)tmp_extruder);
1032             #ifdef TEMP_RESIDENCY_TIME
1033               SERIAL_PROTOCOLPGM(" W:");
1034               if(residencyStart > -1)
1035               {
1036                  codenum = ((TEMP_RESIDENCY_TIME * 1000UL) - (millis() - residencyStart)) / 1000UL;
1037                  SERIAL_PROTOCOLLN( codenum );
1038               }
1039               else
1040               {
1041                  SERIAL_PROTOCOLLN( "?" );
1042               }
1043             #else
1044               SERIAL_PROTOCOLLN("");
1045             #endif
1046             codenum = millis();
1047           }
1048           manage_heater();
1049           manage_inactivity(1);
1050           LCD_STATUS;
1051         #ifdef TEMP_RESIDENCY_TIME
1052             /* start/restart the TEMP_RESIDENCY_TIME timer whenever we reach target temp for the first time
1053               or when current temp falls outside the hysteresis after target temp was reached */
1054           if ((residencyStart == -1 &&  target_direction && (degHotend(tmp_extruder) >= (degTargetHotend(tmp_extruder)-TEMP_WINDOW))) ||
1055               (residencyStart == -1 && !target_direction && (degHotend(tmp_extruder) <= (degTargetHotend(tmp_extruder)+TEMP_WINDOW))) ||
1056               (residencyStart > -1 && labs(degHotend(tmp_extruder) - degTargetHotend(tmp_extruder)) > TEMP_HYSTERESIS) )
1057           {
1058             residencyStart = millis();
1059           }
1060         #endif //TEMP_RESIDENCY_TIME
1061         }
1062         LCD_MESSAGEPGM(MSG_HEATING_COMPLETE);
1063         starttime=millis();
1064         previous_millis_cmd = millis();
1065       }
1066       break;
1067     case 190: // M190 - Wait for bed heater to reach target.
1068     #if TEMP_BED_PIN > -1
1069         LCD_MESSAGEPGM(MSG_BED_HEATING);
1070         if (code_seen('S')) setTargetBed(code_value());
1071         codenum = millis();
1072         while(isHeatingBed())
1073         {
1074           if(( millis() - codenum) > 1000 ) //Print Temp Reading every 1 second while heating up.
1075           {
1076             float tt=degHotend(active_extruder);
1077             SERIAL_PROTOCOLPGM("T:");
1078             SERIAL_PROTOCOL(tt);
1079             SERIAL_PROTOCOLPGM(" E:");
1080             SERIAL_PROTOCOL((int)active_extruder);
1081             SERIAL_PROTOCOLPGM(" B:");
1082             SERIAL_PROTOCOL_F(degBed(),1);
1083             SERIAL_PROTOCOLLN("");
1084             codenum = millis();
1085           }
1086           manage_heater();
1087           manage_inactivity(1);
1088           LCD_STATUS;
1089         }
1090         LCD_MESSAGEPGM(MSG_BED_DONE);
1091         previous_millis_cmd = millis();
1092     #endif
1093         break;
1094
1095     #if FAN_PIN > -1
1096       case 106: //M106 Fan On
1097         if (code_seen('S')){
1098            FanSpeed=constrain(code_value(),0,255);
1099         }
1100         else {
1101           FanSpeed=255;
1102         }
1103         break;
1104       case 107: //M107 Fan Off
1105         FanSpeed = 0;
1106         break;
1107     #endif //FAN_PIN
1108
1109     #if (PS_ON_PIN > -1)
1110       case 80: // M80 - ATX Power On
1111         SET_OUTPUT(PS_ON_PIN); //GND
1112         WRITE(PS_ON_PIN, LOW);
1113         break;
1114       #endif
1115
1116       case 81: // M81 - ATX Power Off
1117
1118       #if defined SUICIDE_PIN && SUICIDE_PIN > -1
1119         st_synchronize();
1120         suicide();
1121       #elif (PS_ON_PIN > -1)
1122         SET_INPUT(PS_ON_PIN); //Floating
1123       #endif
1124                 break;
1125
1126     case 82:
1127       axis_relative_modes[3] = false;
1128       break;
1129     case 83:
1130       axis_relative_modes[3] = true;
1131       break;
1132     case 18: //compatibility
1133     case 84: // M84
1134       if(code_seen('S')){
1135         stepper_inactive_time = code_value() * 1000;
1136       }
1137       else
1138       {
1139         bool all_axis = !((code_seen(axis_codes[0])) || (code_seen(axis_codes[1])) || (code_seen(axis_codes[2]))|| (code_seen(axis_codes[3])));
1140         if(all_axis)
1141         {
1142           st_synchronize();
1143           disable_e0();
1144           disable_e1();
1145           disable_e2();
1146           finishAndDisableSteppers();
1147         }
1148         else
1149         {
1150           st_synchronize();
1151           if(code_seen('X')) disable_x();
1152           if(code_seen('Y')) disable_y();
1153           if(code_seen('Z')) disable_z();
1154           #if ((E0_ENABLE_PIN != X_ENABLE_PIN) && (E1_ENABLE_PIN != Y_ENABLE_PIN)) // Only enable on boards that have seperate ENABLE_PINS
1155             if(code_seen('E')) {
1156               disable_e0();
1157               disable_e1();
1158               disable_e2();
1159             }
1160           #endif
1161           LCD_MESSAGEPGM(MSG_PART_RELEASE);
1162         }
1163       }
1164       break;
1165     case 85: // M85
1166       code_seen('S');
1167       max_inactive_time = code_value() * 1000;
1168       break;
1169     case 92: // M92
1170       for(int8_t i=0; i < NUM_AXIS; i++)
1171       {
1172         if(code_seen(axis_codes[i]))
1173
1174           if(i == 3) { // E
1175             float value = code_value();
1176             if(value < 20.0) {
1177               float factor = axis_steps_per_unit[i] / value; // increase e constants if M92 E14 is given for netfab.
1178               max_e_jerk *= factor;
1179               max_feedrate[i] *= factor;
1180               axis_steps_per_sqr_second[i] *= factor;
1181             }
1182             axis_steps_per_unit[i] = value;
1183           }
1184           else {
1185             axis_steps_per_unit[i] = code_value();
1186           }
1187       }
1188       break;
1189     case 115: // M115
1190       SerialprintPGM(MSG_M115_REPORT);
1191       break;
1192     case 117: // M117 display message
1193       LCD_MESSAGE(cmdbuffer[bufindr]+5);
1194       break;
1195     case 114: // M114
1196       SERIAL_PROTOCOLPGM("X:");
1197       SERIAL_PROTOCOL(current_position[X_AXIS]);
1198       SERIAL_PROTOCOLPGM("Y:");
1199       SERIAL_PROTOCOL(current_position[Y_AXIS]);
1200       SERIAL_PROTOCOLPGM("Z:");
1201       SERIAL_PROTOCOL(current_position[Z_AXIS]);
1202       SERIAL_PROTOCOLPGM("E:");
1203       SERIAL_PROTOCOL(current_position[E_AXIS]);
1204
1205       SERIAL_PROTOCOLPGM(MSG_COUNT_X);
1206       SERIAL_PROTOCOL(float(st_get_position(X_AXIS))/axis_steps_per_unit[X_AXIS]);
1207       SERIAL_PROTOCOLPGM("Y:");
1208       SERIAL_PROTOCOL(float(st_get_position(Y_AXIS))/axis_steps_per_unit[Y_AXIS]);
1209       SERIAL_PROTOCOLPGM("Z:");
1210       SERIAL_PROTOCOL(float(st_get_position(Z_AXIS))/axis_steps_per_unit[Z_AXIS]);
1211
1212       SERIAL_PROTOCOLLN("");
1213       break;
1214     case 120: // M120
1215       enable_endstops(false) ;
1216       break;
1217     case 121: // M121
1218       enable_endstops(true) ;
1219       break;
1220     case 119: // M119
1221       #if (X_MIN_PIN > -1)
1222         SERIAL_PROTOCOLPGM(MSG_X_MIN);
1223         SERIAL_PROTOCOL(((READ(X_MIN_PIN)^X_ENDSTOPS_INVERTING)?"H ":"L "));
1224       #endif
1225       #if (X_MAX_PIN > -1)
1226         SERIAL_PROTOCOLPGM(MSG_X_MAX);
1227         SERIAL_PROTOCOL(((READ(X_MAX_PIN)^X_ENDSTOPS_INVERTING)?"H ":"L "));
1228       #endif
1229       #if (Y_MIN_PIN > -1)
1230         SERIAL_PROTOCOLPGM(MSG_Y_MIN);
1231         SERIAL_PROTOCOL(((READ(Y_MIN_PIN)^Y_ENDSTOPS_INVERTING)?"H ":"L "));
1232       #endif
1233       #if (Y_MAX_PIN > -1)
1234         SERIAL_PROTOCOLPGM(MSG_Y_MAX);
1235         SERIAL_PROTOCOL(((READ(Y_MAX_PIN)^Y_ENDSTOPS_INVERTING)?"H ":"L "));
1236       #endif
1237       #if (Z_MIN_PIN > -1)
1238         SERIAL_PROTOCOLPGM(MSG_Z_MIN);
1239         SERIAL_PROTOCOL(((READ(Z_MIN_PIN)^Z_ENDSTOPS_INVERTING)?"H ":"L "));
1240       #endif
1241       #if (Z_MAX_PIN > -1)
1242         SERIAL_PROTOCOLPGM(MSG_Z_MAX);
1243         SERIAL_PROTOCOL(((READ(Z_MAX_PIN)^Z_ENDSTOPS_INVERTING)?"H ":"L "));
1244       #endif
1245       SERIAL_PROTOCOLLN("");
1246       break;
1247       //TODO: update for all axis, use for loop
1248     case 201: // M201
1249       for(int8_t i=0; i < NUM_AXIS; i++)
1250       {
1251         if(code_seen(axis_codes[i]))
1252         {
1253           max_acceleration_units_per_sq_second[i] = code_value();
1254           axis_steps_per_sqr_second[i] = code_value() * axis_steps_per_unit[i];
1255         }
1256       }
1257       break;
1258     #if 0 // Not used for Sprinter/grbl gen6
1259     case 202: // M202
1260       for(int8_t i=0; i < NUM_AXIS; i++) {
1261         if(code_seen(axis_codes[i])) axis_travel_steps_per_sqr_second[i] = code_value() * axis_steps_per_unit[i];
1262       }
1263       break;
1264     #endif
1265     case 203: // M203 max feedrate mm/sec
1266       for(int8_t i=0; i < NUM_AXIS; i++) {
1267         if(code_seen(axis_codes[i])) max_feedrate[i] = code_value();
1268       }
1269       break;
1270     case 204: // M204 acclereration S normal moves T filmanent only moves
1271       {
1272         if(code_seen('S')) acceleration = code_value() ;
1273         if(code_seen('T')) retract_acceleration = code_value() ;
1274       }
1275       break;
1276     case 205: //M205 advanced settings:  minimum travel speed S=while printing T=travel only,  B=minimum segment time X= maximum xy jerk, Z=maximum Z jerk
1277     {
1278       if(code_seen('S')) minimumfeedrate = code_value();
1279       if(code_seen('T')) mintravelfeedrate = code_value();
1280       if(code_seen('B')) minsegmenttime = code_value() ;
1281       if(code_seen('X')) max_xy_jerk = code_value() ;
1282       if(code_seen('Z')) max_z_jerk = code_value() ;
1283       if(code_seen('E')) max_e_jerk = code_value() ;
1284     }
1285     break;
1286     case 206: // M206 additional homeing offset
1287       for(int8_t i=0; i < 3; i++)
1288       {
1289         if(code_seen(axis_codes[i])) add_homeing[i] = code_value();
1290       }
1291       break;
1292     #ifdef FWRETRACT
1293     case 207: //M207 - set retract length S[positive mm] F[feedrate mm/sec] Z[additional zlift/hop]
1294     {
1295       if(code_seen('S'))
1296       {
1297         retract_length = code_value() ;
1298       }
1299       if(code_seen('F'))
1300       {
1301         retract_feedrate = code_value() ;
1302       }
1303       if(code_seen('Z'))
1304       {
1305         retract_zlift = code_value() ;
1306       }
1307     }break;
1308     case 208: // M208 - set retract recover length S[positive mm surplus to the M207 S*] F[feedrate mm/sec]
1309     {
1310       if(code_seen('S'))
1311       {
1312         retract_recover_length = code_value() ;
1313       }
1314       if(code_seen('F'))
1315       {
1316         retract_recover_feedrate = code_value() ;
1317       }
1318     }break;
1319
1320     case 209: // M209 - S<1=true/0=false> enable automatic retract detect if the slicer did not support G10/11: every normal extrude-only move will be classified as retract depending on the direction.
1321     {
1322       if(code_seen('S'))
1323       {
1324         int t= code_value() ;
1325         switch(t)
1326         {
1327           case 0: autoretract_enabled=false;retracted=false;break;
1328           case 1: autoretract_enabled=true;retracted=false;break;
1329           default:
1330             SERIAL_ECHO_START;
1331             SERIAL_ECHOPGM(MSG_UNKNOWN_COMMAND);
1332             SERIAL_ECHO(cmdbuffer[bufindr]);
1333             SERIAL_ECHOLNPGM("\"");
1334         }
1335       }
1336
1337     }break;
1338     #endif
1339     case 220: // M220 S<factor in percent>- set speed factor override percentage
1340     {
1341       if(code_seen('S'))
1342       {
1343         feedmultiply = code_value() ;
1344         feedmultiplychanged=true;
1345       }
1346     }
1347     break;
1348     case 221: // M221 S<factor in percent>- set extrude factor override percentage
1349     {
1350       if(code_seen('S'))
1351       {
1352         extrudemultiply = code_value() ;
1353       }
1354     }
1355     break;
1356
1357     #ifdef PIDTEMP
1358     case 301: // M301
1359       {
1360         if(code_seen('P')) Kp = code_value();
1361         if(code_seen('I')) Ki = code_value()*PID_dT;
1362         if(code_seen('D')) Kd = code_value()/PID_dT;
1363         #ifdef PID_ADD_EXTRUSION_RATE
1364         if(code_seen('C')) Kc = code_value();
1365         #endif
1366         updatePID();
1367         SERIAL_PROTOCOL(MSG_OK);
1368                 SERIAL_PROTOCOL(" p:");
1369         SERIAL_PROTOCOL(Kp);
1370         SERIAL_PROTOCOL(" i:");
1371         SERIAL_PROTOCOL(Ki/PID_dT);
1372         SERIAL_PROTOCOL(" d:");
1373         SERIAL_PROTOCOL(Kd*PID_dT);
1374         #ifdef PID_ADD_EXTRUSION_RATE
1375         SERIAL_PROTOCOL(" c:");
1376         SERIAL_PROTOCOL(Kc*PID_dT);
1377         #endif
1378         SERIAL_PROTOCOLLN("");
1379       }
1380       break;
1381     #endif //PIDTEMP
1382     case 240: // M240  Triggers a camera by emulating a Canon RC-1 : http://www.doc-diy.net/photo/rc-1_hacked/
1383      {
1384       #ifdef PHOTOGRAPH_PIN
1385         #if (PHOTOGRAPH_PIN > -1)
1386         const uint8_t NUM_PULSES=16;
1387         const float PULSE_LENGTH=0.01524;
1388         for(int i=0; i < NUM_PULSES; i++) {
1389           WRITE(PHOTOGRAPH_PIN, HIGH);
1390           _delay_ms(PULSE_LENGTH);
1391           WRITE(PHOTOGRAPH_PIN, LOW);
1392           _delay_ms(PULSE_LENGTH);
1393         }
1394         delay(7.33);
1395         for(int i=0; i < NUM_PULSES; i++) {
1396           WRITE(PHOTOGRAPH_PIN, HIGH);
1397           _delay_ms(PULSE_LENGTH);
1398           WRITE(PHOTOGRAPH_PIN, LOW);
1399           _delay_ms(PULSE_LENGTH);
1400         }
1401         #endif
1402       #endif
1403      }
1404     break;
1405
1406     case 302: // allow cold extrudes
1407     {
1408       allow_cold_extrudes(true);
1409     }
1410     break;
1411     case 303: // M303 PID autotune
1412     {
1413       float temp = 150.0;
1414       if (code_seen('S')) temp=code_value();
1415       PID_autotune(temp);
1416     }
1417     break;
1418     case 400: // M400 finish all moves
1419     {
1420       st_synchronize();
1421     }
1422     break;
1423     case 500: // Store settings in EEPROM
1424     {
1425         EEPROM_StoreSettings();
1426     }
1427     break;
1428     case 501: // Read settings from EEPROM
1429     {
1430       EEPROM_RetrieveSettings();
1431     }
1432     break;
1433     case 502: // Revert to default settings
1434     {
1435       EEPROM_RetrieveSettings(true);
1436     }
1437     break;
1438     case 503: // print settings currently in memory
1439     {
1440       EEPROM_printSettings();
1441     }
1442     break;
1443     case 999: // Restart after being stopped
1444       Stopped = false;
1445       gcode_LastN = Stopped_gcode_LastN;
1446       FlushSerialRequestResend();
1447     break;
1448     }
1449   }
1450
1451   else if(code_seen('T'))
1452   {
1453     tmp_extruder = code_value();
1454     if(tmp_extruder >= EXTRUDERS) {
1455       SERIAL_ECHO_START;
1456       SERIAL_ECHO("T");
1457       SERIAL_ECHO(tmp_extruder);
1458       SERIAL_ECHOLN(MSG_INVALID_EXTRUDER);
1459     }
1460     else {
1461       active_extruder = tmp_extruder;
1462       SERIAL_ECHO_START;
1463       SERIAL_ECHO(MSG_ACTIVE_EXTRUDER);
1464       SERIAL_PROTOCOLLN((int)active_extruder);
1465     }
1466   }
1467
1468   else
1469   {
1470     SERIAL_ECHO_START;
1471     SERIAL_ECHOPGM(MSG_UNKNOWN_COMMAND);
1472     SERIAL_ECHO(cmdbuffer[bufindr]);
1473     SERIAL_ECHOLNPGM("\"");
1474   }
1475
1476   ClearToSend();
1477 }
1478
1479 void FlushSerialRequestResend()
1480 {
1481   //char cmdbuffer[bufindr][100]="Resend:";
1482   MYSERIAL.flush();
1483   SERIAL_PROTOCOLPGM(MSG_RESEND);
1484   SERIAL_PROTOCOLLN(gcode_LastN + 1);
1485   ClearToSend();
1486 }
1487
1488 void ClearToSend()
1489 {
1490   previous_millis_cmd = millis();
1491   #ifdef SDSUPPORT
1492   if(fromsd[bufindr])
1493     return;
1494   #endif //SDSUPPORT
1495   SERIAL_PROTOCOLLNPGM(MSG_OK);
1496 }
1497
1498 void get_coordinates()
1499 {
1500   bool seen[4]={false,false,false,false};
1501   for(int8_t i=0; i < NUM_AXIS; i++) {
1502     if(code_seen(axis_codes[i]))
1503     {
1504       destination[i] = (float)code_value() + (axis_relative_modes[i] || relative_mode)*current_position[i];
1505       seen[i]=true;
1506     }
1507     else destination[i] = current_position[i]; //Are these else lines really needed?
1508   }
1509   if(code_seen('F')) {
1510     next_feedrate = code_value();
1511     if(next_feedrate > 0.0) feedrate = next_feedrate;
1512   }
1513   #ifdef FWRETRACT
1514   if(autoretract_enabled)
1515   if( !(seen[X_AXIS] || seen[Y_AXIS] || seen[Z_AXIS]) && seen[E_AXIS])
1516   {
1517     float echange=destination[E_AXIS]-current_position[E_AXIS];
1518     if(echange<-MIN_RETRACT) //retract
1519     {
1520       if(!retracted)
1521       {
1522
1523       destination[Z_AXIS]+=retract_zlift; //not sure why chaninging current_position negatively does not work.
1524       //if slicer retracted by echange=-1mm and you want to retract 3mm, corrrectede=-2mm additionally
1525       float correctede=-echange-retract_length;
1526       //to generate the additional steps, not the destination is changed, but inversely the current position
1527       current_position[E_AXIS]+=-correctede;
1528       feedrate=retract_feedrate;
1529       retracted=true;
1530       }
1531
1532     }
1533     else
1534       if(echange>MIN_RETRACT) //retract_recover
1535     {
1536       if(retracted)
1537       {
1538       //current_position[Z_AXIS]+=-retract_zlift;
1539       //if slicer retracted_recovered by echange=+1mm and you want to retract_recover 3mm, corrrectede=2mm additionally
1540       float correctede=-echange+1*retract_length+retract_recover_length; //total unretract=retract_length+retract_recover_length[surplus]
1541       current_position[E_AXIS]+=correctede; //to generate the additional steps, not the destination is changed, but inversely the current position
1542       feedrate=retract_recover_feedrate;
1543       retracted=false;
1544       }
1545     }
1546
1547   }
1548   #endif //FWRETRACT
1549 }
1550
1551 void get_arc_coordinates()
1552 {
1553    get_coordinates();
1554    if(code_seen('I')) {
1555      offset[0] = code_value();
1556    }
1557    else {
1558      offset[0] = 0.0;
1559    }
1560    if(code_seen('J')) {
1561      offset[1] = code_value();
1562    }
1563    else {
1564      offset[1] = 0.0;
1565    }
1566 }
1567
1568 void clamp_to_software_endstops(float target[3])
1569 {
1570   if (min_software_endstops) {
1571     if (target[X_AXIS] < min_pos[X_AXIS]) target[X_AXIS] = min_pos[X_AXIS];
1572     if (target[Y_AXIS] < min_pos[Y_AXIS]) target[Y_AXIS] = min_pos[Y_AXIS];
1573     if (target[Z_AXIS] < min_pos[Z_AXIS]) target[Z_AXIS] = min_pos[Z_AXIS];
1574   }
1575
1576   if (max_software_endstops) {
1577     if (target[X_AXIS] > max_pos[X_AXIS]) target[X_AXIS] = max_pos[X_AXIS];
1578     if (target[Y_AXIS] > max_pos[Y_AXIS]) target[Y_AXIS] = max_pos[Y_AXIS];
1579     if (target[Z_AXIS] > max_pos[Z_AXIS]) target[Z_AXIS] = max_pos[Z_AXIS];
1580   }
1581 }
1582
1583 void prepare_move()
1584 {
1585   clamp_to_software_endstops(destination);
1586
1587   previous_millis_cmd = millis();
1588   plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder);
1589   for(int8_t i=0; i < NUM_AXIS; i++) {
1590     current_position[i] = destination[i];
1591   }
1592 }
1593
1594 void prepare_arc_move(char isclockwise) {
1595   float r = hypot(offset[X_AXIS], offset[Y_AXIS]); // Compute arc radius for mc_arc
1596
1597   // Trace the arc
1598   mc_arc(current_position, destination, offset, X_AXIS, Y_AXIS, Z_AXIS, feedrate*feedmultiply/60/100.0, r, isclockwise, active_extruder);
1599
1600   // As far as the parser is concerned, the position is now == target. In reality the
1601   // motion control system might still be processing the action and the real tool position
1602   // in any intermediate location.
1603   for(int8_t i=0; i < NUM_AXIS; i++) {
1604     current_position[i] = destination[i];
1605   }
1606   previous_millis_cmd = millis();
1607 }
1608
1609 #ifdef CONTROLLERFAN_PIN
1610 unsigned long lastMotor = 0; //Save the time for when a motor was turned on last
1611 unsigned long lastMotorCheck = 0;
1612
1613 void controllerFan()
1614 {
1615   if ((millis() - lastMotorCheck) >= 2500) //Not a time critical function, so we only check every 2500ms
1616   {
1617     lastMotorCheck = millis();
1618
1619     if(!READ(X_ENABLE_PIN) || !READ(Y_ENABLE_PIN) || !READ(Z_ENABLE_PIN)
1620     #if EXTRUDERS > 2
1621        || !READ(E2_ENABLE_PIN)
1622     #endif
1623     #if EXTRUDER > 1
1624        || !READ(E2_ENABLE_PIN)
1625     #endif
1626        || !READ(E0_ENABLE_PIN)) //If any of the drivers are enabled...
1627     {
1628       lastMotor = millis(); //... set time to NOW so the fan will turn on
1629     }
1630
1631     if ((millis() - lastMotor) >= (CONTROLLERFAN_SEC*1000UL) || lastMotor == 0) //If the last time any driver was enabled, is longer since than CONTROLLERSEC...
1632     {
1633       WRITE(CONTROLLERFAN_PIN, LOW); //... turn the fan off
1634     }
1635     else
1636     {
1637       WRITE(CONTROLLERFAN_PIN, HIGH); //... turn the fan on
1638     }
1639   }
1640 }
1641 #endif
1642
1643 void manage_inactivity(byte debug)
1644 {
1645   if( (millis() - previous_millis_cmd) >  max_inactive_time )
1646     if(max_inactive_time)
1647       kill();
1648   if(stepper_inactive_time)  {
1649     if( (millis() - previous_millis_cmd) >  stepper_inactive_time )
1650     {
1651       if(blocks_queued() == false) {
1652         disable_x();
1653         disable_y();
1654         disable_z();
1655         disable_e0();
1656         disable_e1();
1657         disable_e2();
1658       }
1659     }
1660   }
1661   #ifdef CONTROLLERFAN_PIN
1662     controllerFan(); //Check if fan should be turned on to cool stepper drivers down
1663   #endif
1664   #ifdef EXTRUDER_RUNOUT_PREVENT
1665     if( (millis() - previous_millis_cmd) >  EXTRUDER_RUNOUT_SECONDS*1000 )
1666     if(degHotend(active_extruder)>EXTRUDER_RUNOUT_MINTEMP)
1667     {
1668      bool oldstatus=READ(E0_ENABLE_PIN);
1669      enable_e0();
1670      float oldepos=current_position[E_AXIS];
1671      float oldedes=destination[E_AXIS];
1672      plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS],
1673                       current_position[E_AXIS]+EXTRUDER_RUNOUT_EXTRUDE*EXTRUDER_RUNOUT_ESTEPS/axis_steps_per_unit[E_AXIS],
1674                       EXTRUDER_RUNOUT_SPEED/60.*EXTRUDER_RUNOUT_ESTEPS/axis_steps_per_unit[E_AXIS], active_extruder);
1675      current_position[E_AXIS]=oldepos;
1676      destination[E_AXIS]=oldedes;
1677      plan_set_e_position(oldepos);
1678      previous_millis_cmd=millis();
1679      st_synchronize();
1680      WRITE(E0_ENABLE_PIN,oldstatus);
1681     }
1682   #endif
1683   check_axes_activity();
1684 }
1685
1686 void kill()
1687 {
1688   cli(); // Stop interrupts
1689   disable_heater();
1690
1691   disable_x();
1692   disable_y();
1693   disable_z();
1694   disable_e0();
1695   disable_e1();
1696   disable_e2();
1697
1698   if(PS_ON_PIN > -1) pinMode(PS_ON_PIN,INPUT);
1699   SERIAL_ERROR_START;
1700   SERIAL_ERRORLNPGM(MSG_ERR_KILLED);
1701   LCD_MESSAGEPGM(MSG_KILLED);
1702   suicide();
1703   while(1); // Wait for reset
1704 }
1705
1706 void Stop()
1707 {
1708   disable_heater();
1709   if(Stopped == false) {
1710     Stopped = true;
1711     Stopped_gcode_LastN = gcode_LastN; // Save last g_code for restart
1712     SERIAL_ERROR_START;
1713     SERIAL_ERRORLNPGM(MSG_ERR_STOPPED);
1714     LCD_MESSAGEPGM(MSG_STOPPED);
1715   }
1716 }
1717
1718 bool IsStopped() { return Stopped; };
1719
1720 #ifdef FAST_PWM_FAN
1721 void setPwmFrequency(uint8_t pin, int val)
1722 {
1723   val &= 0x07;
1724   switch(digitalPinToTimer(pin))
1725   {
1726
1727     #if defined(TCCR0A)
1728     case TIMER0A:
1729     case TIMER0B:
1730 //         TCCR0B &= ~(CS00 | CS01 | CS02);
1731 //         TCCR0B |= val;
1732          break;
1733     #endif
1734
1735     #if defined(TCCR1A)
1736     case TIMER1A:
1737     case TIMER1B:
1738 //         TCCR1B &= ~(CS10 | CS11 | CS12);
1739 //         TCCR1B |= val;
1740          break;
1741     #endif
1742
1743     #if defined(TCCR2)
1744     case TIMER2:
1745     case TIMER2:
1746          TCCR2 &= ~(CS10 | CS11 | CS12);
1747          TCCR2 |= val;
1748          break;
1749     #endif
1750
1751     #if defined(TCCR2A)
1752     case TIMER2A:
1753     case TIMER2B:
1754          TCCR2B &= ~(CS20 | CS21 | CS22);
1755          TCCR2B |= val;
1756          break;
1757     #endif
1758
1759     #if defined(TCCR3A)
1760     case TIMER3A:
1761     case TIMER3B:
1762     case TIMER3C:
1763          TCCR3B &= ~(CS30 | CS31 | CS32);
1764          TCCR3B |= val;
1765          break;
1766     #endif
1767
1768     #if defined(TCCR4A)
1769     case TIMER4A:
1770     case TIMER4B:
1771     case TIMER4C:
1772          TCCR4B &= ~(CS40 | CS41 | CS42);
1773          TCCR4B |= val;
1774          break;
1775    #endif
1776
1777     #if defined(TCCR5A)
1778     case TIMER5A:
1779     case TIMER5B:
1780     case TIMER5C:
1781          TCCR5B &= ~(CS50 | CS51 | CS52);
1782          TCCR5B |= val;
1783          break;
1784    #endif
1785
1786   }
1787 }
1788 #endif
1789
1790