Marlin/Marlin.pde

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