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