API tools faq
paste
Advertisement
Guest User

Tonokip_Firmware.pde Rony

a guest
Jan 8th, 2013
63
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 55.12 KB | None | 0 0
raw download clone embed print report
  1. // Tonokip RepRap firmware rewrite based off of Hydra-mmm firmware.
  2. // Licence: GPL
  3.  
  4. #include "Tonokip_Firmware.h"
  5. #include "configuration.h"
  6. #include "pins.h"
  7.  
  8. #ifdef SDSUPPORT
  9. #include "SdFat.h"
  10. #endif
  11.  
  12. // look here for descriptions of gcodes: http://linuxcnc.org/handbook/gcode/g-code.html
  13. // http://objects.reprap.org/wiki/Mendel_User_Manual:_RepRapGCodes
  14.  
  15. //Implemented Codes
  16. //-------------------
  17. // G0  -> G1
  18. // G1  - Coordinated Movement X Y Z E
  19. // G4  - Dwell S<seconds> or P<milliseconds>
  20. // G28 - Home all Axis
  21. // G90 - Use Absolute Coordinates
  22. // G91 - Use Relative Coordinates
  23. // G92 - Set current position to cordinates given
  24.  
  25. //RepRap M Codes
  26. // M104 - Set extruder target temp
  27. // M105 - Read current temp
  28. // M106 - Fan on
  29. // M107 - Fan off
  30. // M109 - Wait for extruder current temp to reach target temp.
  31. // M114 - Display current position
  32.  
  33. //Custom M Codes
  34. // M80  - Turn on Power Supply
  35. // M20  - List SD card
  36. // M21  - Init SD card
  37. // M22  - Release SD card
  38. // M23  - Select SD file (M23 filename.g)
  39. // M24  - Start/resume SD print
  40. // M25  - Pause SD print
  41. // M26  - Set SD position in bytes (M26 S12345)
  42. // M27  - Report SD print status
  43. // M28  - Start SD write (M28 filename.g)
  44. // M29  - Stop SD write
  45. // M81  - Turn off Power Supply
  46. // M82  - Set E codes absolute (default)
  47. // M83  - Set E codes relative while in Absolute Coordinates (G90) mode
  48. // M84  - Disable steppers until next move,
  49. //        or use S<seconds> to specify an inactivity timeout, after which the steppers will be disabled.  S0 to disable the timeout.
  50. // M85  - Set inactivity shutdown timer with parameter S<seconds>. To disable set zero (default)
  51. // M92  - Set axis_steps_per_unit - same syntax as G92
  52. // M115 - Capabilities string
  53. // M140 - Set bed target temp
  54. // M190 - Wait for bed current temp to reach target temp.
  55. // M201 - Set max acceleration in units/s^2 for print moves (M201 X1000 Y1000)
  56. // M202 - Set max acceleration in units/s^2 for travel moves (M202 X1000 Y1000)
  57.  
  58.  
  59. //Stepper Movement Variables
  60. bool direction_x, direction_y, direction_z, direction_e;
  61. unsigned long previous_micros = 0, previous_micros_x = 0, previous_micros_y = 0, previous_micros_z = 0, previous_micros_e = 0, previous_millis_heater, previous_millis_bed_heater;
  62. unsigned long x_steps_to_take, y_steps_to_take, z_steps_to_take, e_steps_to_take;
  63. #ifdef RAMP_ACCELERATION
  64.   unsigned long max_x_interval = 100000000.0 / (min_units_per_second * x_steps_per_unit);
  65.   unsigned long max_y_interval = 100000000.0 / (min_units_per_second * y_steps_per_unit);
  66.   unsigned long max_interval;
  67.   unsigned long x_steps_per_sqr_second = max_acceleration_units_per_sq_second * x_steps_per_unit;
  68.   unsigned long y_steps_per_sqr_second = max_acceleration_units_per_sq_second * y_steps_per_unit;
  69.   unsigned long x_travel_steps_per_sqr_second = max_travel_acceleration_units_per_sq_second * x_steps_per_unit;
  70.   unsigned long y_travel_steps_per_sqr_second = max_travel_acceleration_units_per_sq_second * y_steps_per_unit;
  71.   unsigned long steps_per_sqr_second, plateau_steps;
  72. #endif
  73. #ifdef EXP_ACCELERATION
  74.   unsigned long long_full_velocity_units = full_velocity_units * 100;
  75.   unsigned long long_travel_move_full_velocity_units = travel_move_full_velocity_units * 100;
  76.   unsigned long max_x_interval = 100000000.0 / (min_units_per_second * x_steps_per_unit);
  77.   unsigned long max_y_interval = 100000000.0 / (min_units_per_second * y_steps_per_unit);
  78.   unsigned long max_interval;
  79.   unsigned long x_min_constant_speed_steps = min_constant_speed_units * x_steps_per_unit,
  80.     y_min_constant_speed_steps = min_constant_speed_units * y_steps_per_unit, min_constant_speed_steps;
  81. #endif
  82. boolean acceleration_enabled = false, accelerating = false;
  83. unsigned long interval;
  84. float destination_x = 0.0, destination_y = 0.0, destination_z = 0.0, destination_e = 0.0;
  85. float current_x = 0.0, current_y = 0.0, current_z = 0.0, current_e = 0.0;
  86. long x_interval, y_interval, z_interval, e_interval; // for speed delay
  87. float feedrate = 1500, next_feedrate, z_feedrate, saved_feedrate;
  88. float time_for_move;
  89. long gcode_N, gcode_LastN;
  90. bool relative_mode = false;  //Determines Absolute or Relative Coordinates
  91. bool relative_mode_e = false;  //Determines Absolute or Relative E Codes while in Absolute Coordinates mode. E is always relative in Relative Coordinates mode.
  92. long timediff = 0;
  93. #ifdef STEP_DELAY_RATIO
  94.   long long_step_delay_ratio = STEP_DELAY_RATIO * 100;
  95. #endif
  96.  
  97.  
  98. // comm variables
  99. #define MAX_CMD_SIZE 96
  100. #define BUFSIZE 8
  101. char cmdbuffer[BUFSIZE][MAX_CMD_SIZE];
  102. bool fromsd[BUFSIZE];
  103. int bufindr = 0;
  104. int bufindw = 0;
  105. int buflen = 0;
  106. int i = 0;
  107. char serial_char;
  108. int serial_count = 0;
  109. boolean comment_mode = false;
  110. char *strchr_pointer; // just a pointer to find chars in the cmd string like X, Y, Z, E, etc
  111.  
  112. // Manage heater variables. For a thermistor or AD595 thermocouple, raw values refer to the
  113. // reading from the analog pin. For a MAX6675 thermocouple, the raw value is the temperature in 0.25
  114. // degree increments (i.e. 100=25 deg).
  115.  
  116. int target_raw = 0;
  117. int current_raw = 0;
  118. int target_bed_raw = 0;
  119. int current_bed_raw = 0;
  120. float tt = 0, bt = 0;
  121. #ifdef PIDTEMP
  122.   int temp_iState = 0;
  123.   int temp_dState = 0;
  124.   int pTerm;
  125.   int iTerm;
  126.   int dTerm;
  127.       //int output;
  128.   int error;
  129.   int temp_iState_min = 100 * -PID_INTEGRAL_DRIVE_MAX / PID_IGAIN;
  130.   int temp_iState_max = 100 * PID_INTEGRAL_DRIVE_MAX / PID_IGAIN;
  131. #endif
  132. #ifdef SMOOTHING
  133.   uint32_t nma = SMOOTHFACTOR * analogRead(TEMP_0_PIN);
  134. #endif
  135. #ifdef WATCHPERIOD
  136.   int watch_raw = -1000;
  137.   unsigned long watchmillis = 0;
  138. #endif
  139. #ifdef MINTEMP
  140.   int minttemp = temp2analog(MINTEMP);
  141. #endif
  142. #ifdef MAXTEMP
  143. int maxttemp = temp2analog(MAXTEMP);
  144. #endif
  145.        
  146. //Inactivity shutdown variables
  147. unsigned long previous_millis_cmd = 0;
  148. unsigned long max_inactive_time = 0;
  149. unsigned long stepper_inactive_time = 0;
  150.  
  151. #ifdef SDSUPPORT
  152.   Sd2Card card;
  153.   SdVolume volume;
  154.   SdFile root;
  155.   SdFile file;
  156.   uint32_t filesize = 0;
  157.   uint32_t sdpos = 0;
  158.   bool sdmode = false;
  159.   bool sdactive = false;
  160.   bool savetosd = false;
  161.   int16_t n;
  162.  
  163.   void initsd(){
  164.   sdactive = false;
  165.   #if SDSS >- 1
  166.     if(root.isOpen())
  167.         root.close();
  168.     if (!card.init(SPI_FULL_SPEED,SDSS)){
  169.         //if (!card.init(SPI_HALF_SPEED,SDSS))
  170.           Serial.println("SD init fail");
  171.     }
  172.     else if (!volume.init(&card))
  173.           Serial.println("volume.init failed");
  174.     else if (!root.openRoot(&volume))
  175.           Serial.println("openRoot failed");
  176.     else
  177.             sdactive = true;
  178.   #endif
  179.   }
  180.  
  181.   inline void write_command(char *buf){
  182.       char* begin = buf;
  183.       char* npos = 0;
  184.       char* end = buf + strlen(buf) - 1;
  185.      
  186.       file.writeError = false;
  187.       if((npos = strchr(buf, 'N')) != NULL){
  188.           begin = strchr(npos, ' ') + 1;
  189.           end = strchr(npos, '*') - 1;
  190.       }
  191.       end[1] = '\r';
  192.       end[2] = '\n';
  193.       end[3] = '\0';
  194.       //Serial.println(begin);
  195.       file.write(begin);
  196.       if (file.writeError){
  197.           Serial.println("error writing to file");
  198.       }
  199.   }
  200. #endif
  201.  
  202.  
  203. void setup()
  204. {
  205.   Serial.begin(BAUDRATE);
  206.   Serial.println("start");
  207.   for(int i = 0; i < BUFSIZE; i++){
  208.       fromsd[i] = false;
  209.   }
  210.   //Initialize Step Pins
  211.   if(X_STEP_PIN > -1) pinMode(X_STEP_PIN,OUTPUT);
  212.   if(Y_STEP_PIN > -1) pinMode(Y_STEP_PIN,OUTPUT);
  213.   if(Z_STEP_PIN > -1) pinMode(Z_STEP_PIN,OUTPUT);
  214.   if(E_STEP_PIN > -1) pinMode(E_STEP_PIN,OUTPUT);
  215.  
  216.   //Initialize Dir Pins
  217.   if(X_DIR_PIN > -1) pinMode(X_DIR_PIN,OUTPUT);
  218.   if(Y_DIR_PIN > -1) pinMode(Y_DIR_PIN,OUTPUT);
  219.   if(Z_DIR_PIN > -1) pinMode(Z_DIR_PIN,OUTPUT);
  220.   if(E_DIR_PIN > -1) pinMode(E_DIR_PIN,OUTPUT);
  221.  
  222.   //Steppers default to disabled.
  223.   if(X_ENABLE_PIN > -1) if(!X_ENABLE_ON) digitalWrite(X_ENABLE_PIN,HIGH);
  224.   if(Y_ENABLE_PIN > -1) if(!Y_ENABLE_ON) digitalWrite(Y_ENABLE_PIN,HIGH);
  225.   if(Z_ENABLE_PIN > -1) if(!Z_ENABLE_ON) digitalWrite(Z_ENABLE_PIN,HIGH);
  226.   if(E_ENABLE_PIN > -1) if(!E_ENABLE_ON) digitalWrite(E_ENABLE_PIN,HIGH);
  227.  
  228.   //endstop pullups
  229.   #ifdef ENDSTOPPULLUPS
  230.     if(X_MIN_PIN > -1) { pinMode(X_MIN_PIN,INPUT); digitalWrite(X_MIN_PIN,HIGH);}
  231.     if(Y_MIN_PIN > -1) { pinMode(Y_MIN_PIN,INPUT); digitalWrite(Y_MIN_PIN,HIGH);}
  232.     if(Z_MIN_PIN > -1) { pinMode(Z_MIN_PIN,INPUT); digitalWrite(Z_MIN_PIN,HIGH);}
  233.     if(X_MAX_PIN > -1) { pinMode(X_MAX_PIN,INPUT); digitalWrite(X_MAX_PIN,HIGH);}
  234.     if(Y_MAX_PIN > -1) { pinMode(Y_MAX_PIN,INPUT); digitalWrite(Y_MAX_PIN,HIGH);}
  235.     if(Z_MAX_PIN > -1) { pinMode(Z_MAX_PIN,INPUT); digitalWrite(Z_MAX_PIN,HIGH);}
  236.   #endif
  237.   //Initialize Enable Pins
  238.   if(X_ENABLE_PIN > -1) pinMode(X_ENABLE_PIN,OUTPUT);
  239.   if(Y_ENABLE_PIN > -1) pinMode(Y_ENABLE_PIN,OUTPUT);
  240.   if(Z_ENABLE_PIN > -1) pinMode(Z_ENABLE_PIN,OUTPUT);
  241.   if(E_ENABLE_PIN > -1) pinMode(E_ENABLE_PIN,OUTPUT);
  242.  
  243.   if(HEATER_0_PIN > -1) pinMode(HEATER_0_PIN,OUTPUT);
  244.   if(HEATER_1_PIN > -1) pinMode(HEATER_1_PIN,OUTPUT);
  245.  
  246. #ifdef HEATER_USES_MAX6675
  247.   digitalWrite(SCK_PIN,0);
  248.   pinMode(SCK_PIN,OUTPUT);
  249.  
  250.   digitalWrite(MOSI_PIN,1);
  251.   pinMode(MOSI_PIN,OUTPUT);
  252.  
  253.   digitalWrite(MISO_PIN,1);
  254.   pinMode(MISO_PIN,INPUT);
  255.  
  256.   digitalWrite(MAX6675_SS,1);
  257.   pinMode(MAX6675_SS,OUTPUT);
  258. #endif  
  259.  
  260. #ifdef SDSUPPORT
  261.  
  262.   //power to SD reader
  263.   #if SDPOWER > -1
  264.     pinMode(SDPOWER,OUTPUT);
  265.     digitalWrite(SDPOWER,HIGH);
  266.   #endif
  267.   initsd();
  268.  
  269. #endif
  270. }
  271.  
  272.  
  273. void loop()
  274. {
  275.   if(buflen<3)
  276.     get_command();
  277.  
  278.   if(buflen){
  279. #ifdef SDSUPPORT
  280.     if(savetosd){
  281.         if(strstr(cmdbuffer[bufindr],"M29") == NULL){
  282.             write_command(cmdbuffer[bufindr]);
  283.             Serial.println("ok");
  284.         }else{
  285.             file.sync();
  286.             file.close();
  287.             savetosd = false;
  288.             Serial.println("Done saving file.");
  289.         }
  290.     }else{
  291.         process_commands();
  292.     }
  293. #else
  294.     process_commands();
  295. #endif
  296.     buflen = (buflen-1);
  297.     bufindr = (bufindr + 1)%BUFSIZE;
  298.     }
  299.   //check heater every n milliseconds
  300.       manage_heater();
  301.       manage_inactivity(1);
  302.   }
  303.  
  304.  
  305. inline void get_command()
  306. {
  307.   while( Serial.available() > 0  && buflen < BUFSIZE) {
  308.     serial_char = Serial.read();
  309.     if(serial_char == '\n' || serial_char == '\r' || serial_char == ':' || serial_count >= (MAX_CMD_SIZE - 1) )
  310.     {
  311.       if(!serial_count) return; //if empty line
  312.       cmdbuffer[bufindw][serial_count] = 0; //terminate string
  313.       if(!comment_mode){
  314.     fromsd[bufindw] = false;
  315.   if(strstr(cmdbuffer[bufindw], "N") != NULL)
  316.   {
  317.     strchr_pointer = strchr(cmdbuffer[bufindw], 'N');
  318.     gcode_N = (strtol(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL, 10));
  319.     if(gcode_N != gcode_LastN+1 && (strstr(cmdbuffer[bufindw], "M110") == NULL) ) {
  320.       Serial.print("Serial Error: Line Number is not Last Line Number+1, Last Line:");
  321.       Serial.println(gcode_LastN);
  322.       //Serial.println(gcode_N);
  323.       FlushSerialRequestResend();
  324.       serial_count = 0;
  325.       return;
  326.     }
  327.    
  328.     if(strstr(cmdbuffer[bufindw], "*") != NULL)
  329.     {
  330.       byte checksum = 0;
  331.       byte count = 0;
  332.       while(cmdbuffer[bufindw][count] != '*') checksum = checksum^cmdbuffer[bufindw][count++];
  333.       strchr_pointer = strchr(cmdbuffer[bufindw], '*');
  334.  
  335.       if( (int)(strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)) != checksum) {
  336.         Serial.print("Error: checksum mismatch, Last Line:");
  337.         Serial.println(gcode_LastN);
  338.         FlushSerialRequestResend();
  339.         serial_count = 0;
  340.         return;
  341.       }
  342.       //if no errors, continue parsing
  343.     }
  344.     else
  345.     {
  346.       Serial.print("Error: No Checksum with line number, Last Line:");
  347.       Serial.println(gcode_LastN);
  348.       FlushSerialRequestResend();
  349.       serial_count = 0;
  350.       return;
  351.     }
  352.    
  353.     gcode_LastN = gcode_N;
  354.     //if no errors, continue parsing
  355.   }
  356.   else  // if we don't receive 'N' but still see '*'
  357.   {
  358.     if((strstr(cmdbuffer[bufindw], "*") != NULL))
  359.     {
  360.       Serial.print("Error: No Line Number with checksum, Last Line:");
  361.       Serial.println(gcode_LastN);
  362.       serial_count = 0;
  363.       return;
  364.     }
  365.   }
  366.     if((strstr(cmdbuffer[bufindw], "G") != NULL)){
  367.         strchr_pointer = strchr(cmdbuffer[bufindw], 'G');
  368.         switch((int)((strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)))){
  369.         case 0:
  370.         case 1:
  371.               #ifdef SDSUPPORT
  372.               if(savetosd)
  373.                 break;
  374.               #endif
  375.               Serial.println("ok");
  376.               break;
  377.         default:
  378.             break;
  379.         }
  380.  
  381.     }
  382.         bufindw = (bufindw + 1)%BUFSIZE;
  383.         buflen += 1;
  384.        
  385.       }
  386.       comment_mode = false; //for new command
  387.       serial_count = 0; //clear buffer
  388.     }
  389.     else
  390.     {
  391.       if(serial_char == ';') comment_mode = true;
  392.       if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;
  393.     }
  394.   }
  395. #ifdef SDSUPPORT
  396. if(!sdmode || serial_count!=0){
  397.     return;
  398. }
  399.   while( filesize > sdpos  && buflen < BUFSIZE) {
  400.     n = file.read();
  401.     serial_char = (char)n;
  402.     if(serial_char == '\n' || serial_char == '\r' || serial_char == ':' || serial_count >= (MAX_CMD_SIZE - 1) || n == -1)
  403.     {
  404.         sdpos = file.curPosition();
  405.         if(sdpos >= filesize){
  406.             sdmode = false;
  407.             Serial.println("Done printing file");
  408.         }
  409.       if(!serial_count) return; //if empty line
  410.       cmdbuffer[bufindw][serial_count] = 0; //terminate string
  411.       if(!comment_mode){
  412.         fromsd[bufindw] = true;
  413.         buflen += 1;
  414.         bufindw = (bufindw + 1)%BUFSIZE;
  415.       }
  416.       comment_mode = false; //for new command
  417.       serial_count = 0; //clear buffer
  418.     }
  419.     else
  420.     {
  421.       if(serial_char == ';') comment_mode = true;
  422.       if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;
  423.     }
  424. }
  425. #endif
  426.  
  427. }
  428.  
  429.  
  430. inline float code_value() { return (strtod(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL)); }
  431. inline long code_value_long() { return (strtol(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL, 10)); }
  432. inline bool code_seen(char code_string[]) { return (strstr(cmdbuffer[bufindr], code_string) != NULL); }  //Return True if the string was found
  433.  
  434. inline bool code_seen(char code)
  435. {
  436.   strchr_pointer = strchr(cmdbuffer[bufindr], code);
  437.   return (strchr_pointer != NULL);  //Return True if a character was found
  438. }
  439.  //experimental feedrate calc
  440. float d = 0;
  441. float xdiff = 0, ydiff = 0, zdiff = 0, ediff = 0;
  442.  
  443. inline void process_commands()
  444. {
  445.   unsigned long codenum; //throw away variable
  446.   char *starpos = NULL;
  447.  
  448.   if(code_seen('G'))
  449.   {
  450.     switch((int)code_value())
  451.     {
  452.       case 0: // G0 -> G1
  453.       case 1: // G1
  454.         get_coordinates(); // For X Y Z E F
  455.         prepare_move();
  456.         previous_millis_cmd = millis();
  457.         //ClearToSend();
  458.         return;
  459.         //break;
  460.       case 4: // G4 dwell
  461.         codenum = 0;
  462.         if(code_seen('P')) codenum = code_value(); // milliseconds to wait
  463.         if(code_seen('S')) codenum = code_value() * 1000; // seconds to wait
  464.         codenum += millis();  // keep track of when we started waiting
  465.         while(millis()  < codenum ){
  466.           manage_heater();
  467.         }
  468.         break;
  469.       case 28: //G28 Home all Axis one at a time
  470.         saved_feedrate = feedrate;
  471.         destination_x = 0;
  472.         current_x = 0;
  473.         destination_y = 0;
  474.         current_y = 0;
  475.         destination_z = 0;
  476.         current_z = 0;
  477.         destination_e = 0;
  478.         current_e = 0;
  479.         feedrate = 0;
  480.  
  481.    
  482.         if((X_MIN_PIN > -1 && X_HOME_DIR==-1) || (X_MAX_PIN > -1 && X_HOME_DIR==1)) {
  483.           current_x = 0;
  484.           destination_x = 1.5 * X_MAX_LENGTH * X_HOME_DIR;
  485.           feedrate = min_units_per_second * 60;
  486.           prepare_move();
  487.          
  488.           current_x = 0;
  489.           destination_x = -1 * X_HOME_DIR;
  490.           prepare_move();
  491.          
  492.           destination_x = 10 * X_HOME_DIR;
  493.           prepare_move();
  494.          
  495.           current_x = 0;
  496.           destination_x = 0;
  497.           feedrate = 0;
  498.         }
  499.        
  500.         if((Y_MIN_PIN > -1 && Y_HOME_DIR==-1) || (Y_MAX_PIN > -1 && Y_HOME_DIR==1)) {
  501.           current_y = 0;
  502.           destination_y = 1.5 * Y_MAX_LENGTH * Y_HOME_DIR;
  503.           feedrate = min_units_per_second * 60;
  504.           prepare_move();
  505.          
  506.           current_y = 0;
  507.           destination_y = -1 * Y_HOME_DIR;
  508.           prepare_move();
  509.          
  510.           destination_y = 10 * Y_HOME_DIR;
  511.           prepare_move();
  512.          
  513.           current_y = 0;
  514.           destination_y = 0;
  515.           feedrate = 0;
  516.         }
  517.        
  518.         if((Z_MIN_PIN > -1 && Z_HOME_DIR==-1) || (Z_MAX_PIN > -1 && Z_HOME_DIR==1)) {
  519.           current_z = 0;
  520.           destination_z = 1.5 * Z_MAX_LENGTH * Z_HOME_DIR;
  521.           feedrate = max_z_feedrate/2;
  522.           prepare_move();
  523.          
  524.           current_z = 0;
  525.           destination_z = -1 * Z_HOME_DIR;
  526.           prepare_move();
  527.          
  528.           destination_z = 10 * Z_HOME_DIR;
  529.           prepare_move();
  530.          
  531.           current_z = 0;
  532.           destination_z = 0;
  533.           feedrate = 0;
  534.         }
  535.        
  536.         feedrate = saved_feedrate;
  537.         previous_millis_cmd = millis();
  538.         break;
  539.       case 90: // G90
  540.         relative_mode = false;
  541.         break;
  542.       case 91: // G91
  543.         relative_mode = true;
  544.         break;
  545.       case 92: // G92
  546.         if(code_seen('X')) current_x = code_value();
  547.         if(code_seen('Y')) current_y = code_value();
  548.         if(code_seen('Z')) current_z = code_value();
  549.         if(code_seen('E')) current_e = code_value();
  550.         break;
  551.        
  552.     }
  553.   }
  554.  
  555.   else if(code_seen('M'))
  556.   {
  557.    
  558.     switch( (int)code_value() )
  559.     {
  560. #ifdef SDSUPPORT
  561.        
  562.       case 20: // M20 - list SD card
  563.         Serial.println("Begin file list");
  564.         root.ls();
  565.         Serial.println("End file list");
  566.         break;
  567.       case 21: // M21 - init SD card
  568.         sdmode = false;
  569.         initsd();
  570.         break;
  571.       case 22: //M22 - release SD card
  572.         sdmode = false;
  573.         sdactive = false;
  574.         break;
  575.       case 23: //M23 - Select file
  576.         if(sdactive){
  577.             sdmode = false;
  578.             file.close();
  579.             starpos = (strchr(strchr_pointer + 4,'*'));
  580.             if(starpos!=NULL)
  581.                 *(starpos-1)='\0';
  582.             if (file.open(&root, strchr_pointer + 4, O_READ)) {
  583.                 Serial.print("File opened:");
  584.                 Serial.print(strchr_pointer + 4);
  585.                 Serial.print(" Size:");
  586.                 Serial.println(file.fileSize());
  587.                 sdpos = 0;
  588.                 filesize = file.fileSize();
  589.                 Serial.println("File selected");
  590.             }
  591.             else{
  592.                 Serial.println("file.open failed");
  593.             }
  594.         }
  595.         break;
  596.       case 24: //M24 - Start SD print
  597.         if(sdactive){
  598.             sdmode = true;
  599.         }
  600.         break;
  601.       case 25: //M25 - Pause SD print
  602.         if(sdmode){
  603.             sdmode = false;
  604.         }
  605.         break;
  606.       case 26: //M26 - Set SD index
  607.         if(sdactive && code_seen('S')){
  608.             sdpos = code_value_long();
  609.             file.seekSet(sdpos);
  610.         }
  611.         break;
  612.       case 27: //M27 - Get SD status
  613.         if(sdactive){
  614.             Serial.print("SD printing byte ");
  615.             Serial.print(sdpos);
  616.             Serial.print("/");
  617.             Serial.println(filesize);
  618.         }else{
  619.             Serial.println("Not SD printing");
  620.         }
  621.         break;
  622.             case 28: //M28 - Start SD write
  623.         if(sdactive){
  624.           char* npos = 0;
  625.             file.close();
  626.             sdmode = false;
  627.             starpos = (strchr(strchr_pointer + 4,'*'));
  628.             if(starpos != NULL){
  629.               npos = strchr(cmdbuffer[bufindr], 'N');
  630.               strchr_pointer = strchr(npos,' ') + 1;
  631.               *(starpos-1) = '\0';
  632.             }
  633.       if (!file.open(&root, strchr_pointer+4, O_CREAT | O_APPEND | O_WRITE | O_TRUNC))
  634.             {
  635.             Serial.print("open failed, File: ");
  636.             Serial.print(strchr_pointer + 4);
  637.             Serial.print(".");
  638.             }else{
  639.             savetosd = true;
  640.             Serial.print("Writing to file: ");
  641.             Serial.println(strchr_pointer + 4);
  642.             }
  643.         }
  644.         break;
  645.       case 29: //M29 - Stop SD write
  646.         //processed in write to file routine above
  647.         //savetosd = false;
  648.         break;
  649. #endif
  650.       case 104: // M104
  651.         if (code_seen('S')) target_raw = temp2analog(code_value());
  652.         #ifdef WATCHPERIOD
  653.             if(target_raw > current_raw){
  654.                 watchmillis = max(1,millis());
  655.                 watch_raw = current_raw;
  656.             }else{
  657.                 watchmillis = 0;
  658.             }
  659.         #endif
  660.         break;
  661.       case 140: // M140 set bed temp
  662.         if (code_seen('S')) target_bed_raw = temp2analogBed(code_value());
  663.         break;
  664.       case 105: // M105
  665.         #if (TEMP_0_PIN > -1) || defined (HEATER_USES_MAX6675)
  666.           tt = analog2temp(current_raw);
  667.         #endif
  668.         #if TEMP_1_PIN > -1
  669.           bt = analog2tempBed(current_bed_raw);
  670.         #endif
  671.         #if (TEMP_0_PIN > -1) || defined (HEATER_USES_MAX6675)
  672.           Serial.print("T:");
  673.           Serial.println(tt);
  674.           #if TEMP_1_PIN > -1
  675.        
  676.             Serial.print("ok T:");
  677.             Serial.print(tt);
  678.             Serial.print(" B:");
  679.             Serial.println(bt);
  680.           #endif
  681.         #else
  682.           Serial.println("No thermistors - no temp");
  683.         #endif
  684.         return;
  685.         //break;
  686.       case 109: // M109 - Wait for extruder heater to reach target.
  687.         if (code_seen('S')) target_raw = temp2analog(code_value());
  688.         #ifdef WATCHPERIOD
  689.             if(target_raw>current_raw){
  690.                 watchmillis = max(1,millis());
  691.                 watch_raw = current_raw;
  692.             }else{
  693.                 watchmillis = 0;
  694.             }
  695.         #endif
  696.         codenum = millis();
  697.         while(current_raw < target_raw) {
  698.           if( (millis() - codenum) > 1000 ) //Print Temp Reading every 1 second while heating up.
  699.           {
  700.             Serial.print("T:");
  701.             Serial.println( analog2temp(current_raw) );
  702.             codenum = millis();
  703.           }
  704.           manage_heater();
  705.         }
  706.         break;
  707.       case 190: // M190 - Wait bed for heater to reach target.
  708.       #if TEMP_1_PIN > -1
  709.         if (code_seen('S')) target_bed_raw = temp2analog(code_value());
  710.         codenum = millis();
  711.         while(current_bed_raw < target_bed_raw) {
  712.           if( (millis()-codenum) > 1000 ) //Print Temp Reading every 1 second while heating up.
  713.           {
  714.             tt=analog2temp(current_raw);
  715.             Serial.print("T:");
  716.             Serial.println( tt );
  717.             Serial.print("ok T:");
  718.             Serial.print( tt );
  719.             Serial.print(" B:");
  720.             Serial.println( analog2temp(current_bed_raw) );
  721.             codenum = millis();
  722.           }
  723.             manage_heater();
  724.         }
  725.       #endif
  726.       break;
  727.       case 106: //M106 Fan On
  728.         if (code_seen('S')){
  729.             digitalWrite(FAN_PIN, HIGH);
  730.             analogWrite(FAN_PIN, constrain(code_value(),0,255) );
  731.         }
  732.         else
  733.             digitalWrite(FAN_PIN, HIGH);
  734.         break;
  735.       case 107: //M107 Fan Off
  736.         analogWrite(FAN_PIN, 0);
  737.        
  738.         digitalWrite(FAN_PIN, LOW);
  739.         break;
  740.       case 80: // M81 - ATX Power On
  741.         if(PS_ON_PIN > -1) pinMode(PS_ON_PIN,OUTPUT); //GND
  742.         break;
  743.       case 81: // M81 - ATX Power Off
  744.         if(PS_ON_PIN > -1) pinMode(PS_ON_PIN,INPUT); //Floating
  745.         break;
  746.       case 82:
  747.         relative_mode_e = false;
  748.         break;
  749.       case 83:
  750.         relative_mode_e = true;
  751.         break;
  752.       case 84:
  753.         if(code_seen('S')){ stepper_inactive_time = code_value() * 1000; }
  754.         else{ disable_x(); disable_y(); disable_z(); disable_e(); }
  755.         break;
  756.       case 85: // M85
  757.         code_seen('S');
  758.         max_inactive_time = code_value() * 1000;
  759.         break;
  760.       case 92: // M92
  761.         if(code_seen('X')) x_steps_per_unit = code_value();
  762.         if(code_seen('Y')) y_steps_per_unit = code_value();
  763.         if(code_seen('Z')) z_steps_per_unit = code_value();
  764.         if(code_seen('E')) e_steps_per_unit = code_value();
  765.         break;
  766.       case 115: // M115
  767.         Serial.println("FIRMWARE_NAME:Sprinter FIRMWARE_URL:http%%3A/github.com/kliment/Sprinter/ PROTOCOL_VERSION:1.0 MACHINE_TYPE:Mendel EXTRUDER_COUNT:1");
  768.         break;
  769.       case 114: // M114
  770.     Serial.print("X:");
  771.         Serial.print(current_x);
  772.     Serial.print("Y:");
  773.         Serial.print(current_y);
  774.     Serial.print("Z:");
  775.         Serial.print(current_z);
  776.     Serial.print("E:");
  777.         Serial.println(current_e);
  778.         break;
  779.       #ifdef RAMP_ACCELERATION
  780.       case 201: // M201
  781.         if(code_seen('X')) x_steps_per_sqr_second = code_value() * x_steps_per_unit;
  782.         if(code_seen('Y')) y_steps_per_sqr_second = code_value() * y_steps_per_unit;
  783.         break;
  784.       case 202: // M202
  785.         if(code_seen('X')) x_travel_steps_per_sqr_second = code_value() * x_steps_per_unit;
  786.         if(code_seen('Y')) y_travel_steps_per_sqr_second = code_value() * y_steps_per_unit;
  787.         break;
  788.       #endif
  789.     }
  790.    
  791.   }
  792.   else{
  793.       Serial.println("Unknown command:");
  794.       Serial.println(cmdbuffer[bufindr]);
  795.   }
  796.  
  797.   ClearToSend();
  798.      
  799. }
  800.  
  801. inline void FlushSerialRequestResend()
  802. {
  803.   //char cmdbuffer[bufindr][100]="Resend:";
  804.   Serial.flush();
  805.   Serial.print("Resend:");
  806.   Serial.println(gcode_LastN + 1);
  807.   ClearToSend();
  808. }
  809.  
  810. inline void ClearToSend()
  811. {
  812.   previous_millis_cmd = millis();
  813.   #ifdef SDSUPPORT
  814.   if(fromsd[bufindr])
  815.     return;
  816.   #endif
  817.   Serial.println("ok");
  818. }
  819.  
  820. inline void get_coordinates()
  821. {
  822.   if(code_seen('X')) destination_x = (float)code_value() + relative_mode*current_x;
  823.   else destination_x = current_x;                                                       //Are these else lines really needed?
  824.   if(code_seen('Y')) destination_y = (float)code_value() + relative_mode*current_y;
  825.   else destination_y = current_y;
  826.   if(code_seen('Z')) destination_z = (float)code_value() + relative_mode*current_z;
  827.   else destination_z = current_z;
  828.   if(code_seen('E')) destination_e = (float)code_value() + (relative_mode_e || relative_mode)*current_e;
  829.   else destination_e = current_e;
  830.   if(code_seen('F')) {
  831.     next_feedrate = code_value();
  832.     if(next_feedrate > 0.0) feedrate = next_feedrate;
  833.   }
  834. }
  835.  
  836. inline void prepare_move()
  837. {
  838.   //Find direction
  839.   if(destination_x >= current_x) direction_x = 1;
  840.   else direction_x = 0;
  841.   if(destination_y >= current_y) direction_y = 1;
  842.   else direction_y = 0;
  843.   if(destination_z >= current_z) direction_z = 1;
  844.   else direction_z = 0;
  845.   if(destination_e >= current_e) direction_e = 1;
  846.   else direction_e = 0;
  847.  
  848.  
  849.   if (min_software_endstops) {
  850.     if (destination_x < 0) destination_x = 0.0;
  851.     if (destination_y < 0) destination_y = 0.0;
  852.     if (destination_z < 0) destination_z = 0.0;
  853.   }
  854.  
  855.   if (max_software_endstops) {
  856.     if (destination_x > X_MAX_LENGTH) destination_x = X_MAX_LENGTH;
  857.     if (destination_y > Y_MAX_LENGTH) destination_y = Y_MAX_LENGTH;
  858.     if (destination_z > Z_MAX_LENGTH) destination_z = Z_MAX_LENGTH;
  859.   }
  860.  
  861.   if(feedrate > max_feedrate) feedrate = max_feedrate;
  862.  
  863.   if(feedrate > max_z_feedrate) z_feedrate = max_z_feedrate;
  864.   else z_feedrate = feedrate;
  865.  
  866.   xdiff = (destination_x - current_x);
  867.   ydiff = (destination_y - current_y);
  868.   zdiff = (destination_z - current_z);
  869.   ediff = (destination_e - current_e);
  870.   x_steps_to_take = abs(xdiff) * x_steps_per_unit;
  871.   y_steps_to_take = abs(ydiff) * y_steps_per_unit;
  872.   z_steps_to_take = abs(zdiff) * z_steps_per_unit;
  873.   e_steps_to_take = abs(ediff) * e_steps_per_unit;
  874.   if(feedrate < 10)
  875.       feedrate = 10;
  876.   /*
  877.   //experimental feedrate calc
  878.   if(abs(xdiff) > 0.1 && abs(ydiff) > 0.1)
  879.       d = sqrt(xdiff * xdiff + ydiff * ydiff);
  880.   else if(abs(xdiff) > 0.1)
  881.       d = abs(xdiff);
  882.   else if(abs(ydiff) > 0.1)
  883.       d = abs(ydiff);
  884.   else if(abs(zdiff) > 0.05)
  885.       d = abs(zdiff);
  886.   else if(abs(ediff) > 0.1)
  887.       d = abs(ediff);
  888.   else d = 1; //extremely slow move, should be okay for moves under 0.1mm
  889.   time_for_move = (xdiff / (feedrate / 60000000) );
  890.   //time = 60000000 * dist / feedrate
  891.   //int feedz = (60000000 * zdiff) / time_for_move;
  892.   //if(feedz > maxfeed)
  893.   */
  894.   #define X_TIME_FOR_MOVE ((float)x_steps_to_take / (x_steps_per_unit*feedrate/60000000))
  895.   #define Y_TIME_FOR_MOVE ((float)y_steps_to_take / (y_steps_per_unit*feedrate/60000000))
  896.   #define Z_TIME_FOR_MOVE ((float)z_steps_to_take / (z_steps_per_unit*z_feedrate/60000000))
  897.   #define E_TIME_FOR_MOVE ((float)e_steps_to_take / (e_steps_per_unit*feedrate/60000000))
  898.  
  899.   time_for_move = max(X_TIME_FOR_MOVE, Y_TIME_FOR_MOVE);
  900.   time_for_move = max(time_for_move, Z_TIME_FOR_MOVE);
  901.   if(time_for_move <= 0) time_for_move = max(time_for_move, E_TIME_FOR_MOVE);
  902.  
  903.   if(x_steps_to_take) x_interval = time_for_move / x_steps_to_take * 100;
  904.   if(y_steps_to_take) y_interval = time_for_move / y_steps_to_take * 100;
  905.   if(z_steps_to_take) z_interval = time_for_move / z_steps_to_take * 100;
  906.   if(e_steps_to_take && (x_steps_to_take + y_steps_to_take <= 0) ) e_interval = time_for_move / e_steps_to_take * 100;
  907.  
  908.   //#define DEBUGGING false
  909.   #if 0        
  910.   if(0) {
  911.     Serial.print("destination_x: "); Serial.println(destination_x);
  912.     Serial.print("current_x: "); Serial.println(current_x);
  913.     Serial.print("x_steps_to_take: "); Serial.println(x_steps_to_take);
  914.     Serial.print("X_TIME_FOR_MVE: "); Serial.println(X_TIME_FOR_MOVE);
  915.     Serial.print("x_interval: "); Serial.println(x_interval);
  916.     Serial.println("");
  917.     Serial.print("destination_y: "); Serial.println(destination_y);
  918.     Serial.print("current_y: "); Serial.println(current_y);
  919.     Serial.print("y_steps_to_take: "); Serial.println(y_steps_to_take);
  920.     Serial.print("Y_TIME_FOR_MVE: "); Serial.println(Y_TIME_FOR_MOVE);
  921.     Serial.print("y_interval: "); Serial.println(y_interval);
  922.     Serial.println("");
  923.     Serial.print("destination_z: "); Serial.println(destination_z);
  924.     Serial.print("current_z: "); Serial.println(current_z);
  925.     Serial.print("z_steps_to_take: "); Serial.println(z_steps_to_take);
  926.     Serial.print("Z_TIME_FOR_MVE: "); Serial.println(Z_TIME_FOR_MOVE);
  927.     Serial.print("z_interval: "); Serial.println(z_interval);
  928.     Serial.println("");
  929.     Serial.print("destination_e: "); Serial.println(destination_e);
  930.     Serial.print("current_e: "); Serial.println(current_e);
  931.     Serial.print("e_steps_to_take: "); Serial.println(e_steps_to_take);
  932.     Serial.print("E_TIME_FOR_MVE: "); Serial.println(E_TIME_FOR_MOVE);
  933.     Serial.print("e_interval: "); Serial.println(e_interval);
  934.     Serial.println("");
  935.   }
  936.   #endif
  937.  
  938.   linear_move(x_steps_to_take, y_steps_to_take, z_steps_to_take, e_steps_to_take); // make the move
  939. }
  940.  
  941. void linear_move(unsigned long x_steps_remaining, unsigned long y_steps_remaining, unsigned long z_steps_remaining, unsigned long e_steps_remaining) // make linear move with preset speeds and destinations, see G0 and G1
  942. {
  943.   //Determine direction of movement
  944.   if (destination_x > current_x) digitalWrite(X_DIR_PIN,!INVERT_X_DIR);
  945.   else digitalWrite(X_DIR_PIN,INVERT_X_DIR);
  946.   if (destination_y > current_y) digitalWrite(Y_DIR_PIN,!INVERT_Y_DIR);
  947.   else digitalWrite(Y_DIR_PIN,INVERT_Y_DIR);
  948.   if (destination_z > current_z) digitalWrite(Z_DIR_PIN,!INVERT_Z_DIR);
  949.   else digitalWrite(Z_DIR_PIN,INVERT_Z_DIR);
  950.   if (destination_e > current_e) digitalWrite(E_DIR_PIN,!INVERT_E_DIR);
  951.   else digitalWrite(E_DIR_PIN,INVERT_E_DIR);
  952.  
  953.   if(X_MIN_PIN > -1) if(!direction_x) if(digitalRead(X_MIN_PIN) != ENDSTOPS_INVERTING) x_steps_remaining=0;
  954.   if(Y_MIN_PIN > -1) if(!direction_y) if(digitalRead(Y_MIN_PIN) != ENDSTOPS_INVERTING) y_steps_remaining=0;
  955.   if(Z_MIN_PIN > -1) if(!direction_z) if(digitalRead(Z_MIN_PIN) != ENDSTOPS_INVERTING) z_steps_remaining=0;
  956.   if(X_MAX_PIN > -1) if(direction_x) if(digitalRead(X_MAX_PIN) != ENDSTOPS_INVERTING) x_steps_remaining=0;
  957.   if(Y_MAX_PIN > -1) if(direction_y) if(digitalRead(Y_MAX_PIN) != ENDSTOPS_INVERTING) y_steps_remaining=0;
  958.   if(Z_MAX_PIN > -1) if(direction_z) if(digitalRead(Z_MAX_PIN) != ENDSTOPS_INVERTING) z_steps_remaining=0;
  959.  
  960.  
  961.   //Only enable axis that are moving. If the axis doesn't need to move then it can stay disabled depending on configuration.
  962.   if(x_steps_remaining) enable_x();
  963.   if(y_steps_remaining) enable_y();
  964.   if(z_steps_remaining) { enable_z(); do_z_step(); z_steps_remaining--; }
  965.   if(e_steps_remaining) { enable_e(); do_e_step(); e_steps_remaining--; }
  966.  
  967.     //Define variables that are needed for the Bresenham algorithm. Please note that  Z is not currently included in the Bresenham algorithm.
  968.   unsigned int delta_x = x_steps_remaining;
  969.   unsigned long x_interval_nanos;
  970.   unsigned int delta_y = y_steps_remaining;
  971.   unsigned long y_interval_nanos;
  972.   unsigned int delta_z = z_steps_remaining;
  973.   unsigned long z_interval_nanos;
  974.   boolean steep_y = delta_y > delta_x;// && delta_y > delta_e && delta_y > delta_z;
  975.   boolean steep_x = delta_x >= delta_y;// && delta_x > delta_e && delta_x > delta_z;
  976.   //boolean steep_z = delta_z > delta_x && delta_z > delta_y && delta_z > delta_e;
  977.   int error_x;
  978.   int error_y;
  979.   int error_z;
  980.   #ifdef RAMP_ACCELERATION
  981.   long max_speed_steps_per_second;
  982.   long min_speed_steps_per_second;
  983.   #endif
  984.   #ifdef EXP_ACCELERATION
  985.   unsigned long virtual_full_velocity_steps;
  986.   unsigned long full_velocity_steps;
  987.   #endif
  988.   unsigned long steps_remaining;
  989.   unsigned long steps_to_take;
  990.  
  991.   //Do some Bresenham calculations depending on which axis will lead it.
  992.   if(steep_y) {
  993.    error_x = delta_y / 2;
  994.    interval = y_interval;
  995.    #ifdef RAMP_ACCELERATION
  996.    max_interval = max_y_interval;
  997.    if(e_steps_to_take > 0) steps_per_sqr_second = y_steps_per_sqr_second;
  998.    else steps_per_sqr_second = y_travel_steps_per_sqr_second;
  999.    max_speed_steps_per_second = 100000000 / interval;
  1000.    min_speed_steps_per_second = 100000000 / max_interval;
  1001.    float plateau_time = (max_speed_steps_per_second - min_speed_steps_per_second) / (float) steps_per_sqr_second;
  1002.    plateau_steps = (long) ((steps_per_sqr_second / 2.0 * plateau_time + min_speed_steps_per_second) * plateau_time);
  1003.    #endif
  1004.    #ifdef EXP_ACCELERATION
  1005.    if(e_steps_to_take > 0) virtual_full_velocity_steps = long_full_velocity_units * y_steps_per_unit /100;
  1006.    else virtual_full_velocity_steps = long_travel_move_full_velocity_units * y_steps_per_unit /100;
  1007.    full_velocity_steps = min(virtual_full_velocity_steps, (delta_y - y_min_constant_speed_steps) / 2);
  1008.    max_interval = max_y_interval;
  1009.    min_constant_speed_steps = y_min_constant_speed_steps;
  1010.    #endif
  1011.    steps_remaining = delta_y;
  1012.    steps_to_take = delta_y;
  1013.   } else if (steep_x) {
  1014.    error_y = delta_x / 2;
  1015.    interval = x_interval;
  1016.    #ifdef RAMP_ACCELERATION
  1017.    max_interval = max_x_interval;
  1018.    if(e_steps_to_take > 0) steps_per_sqr_second = x_steps_per_sqr_second;
  1019.    else steps_per_sqr_second = x_travel_steps_per_sqr_second;
  1020.    max_speed_steps_per_second = 100000000 / interval;
  1021.    min_speed_steps_per_second = 100000000 / max_interval;
  1022.    float plateau_time = (max_speed_steps_per_second - min_speed_steps_per_second) / (float) steps_per_sqr_second;
  1023.    plateau_steps = (long) ((steps_per_sqr_second / 2.0 * plateau_time + min_speed_steps_per_second) * plateau_time);
  1024.    #endif
  1025.    #ifdef EXP_ACCELERATION
  1026.    if(e_steps_to_take > 0) virtual_full_velocity_steps = long_full_velocity_units * x_steps_per_unit /100;
  1027.    else virtual_full_velocity_steps = long_travel_move_full_velocity_units * x_steps_per_unit /100;
  1028.    full_velocity_steps = min(virtual_full_velocity_steps, (delta_x - x_min_constant_speed_steps) / 2);
  1029.    max_interval = max_x_interval;
  1030.    min_constant_speed_steps = x_min_constant_speed_steps;
  1031.    #endif
  1032.    steps_remaining = delta_x;
  1033.    steps_to_take = delta_x;
  1034.   }
  1035.   unsigned long steps_done = 0;
  1036.   #ifdef RAMP_ACCELERATION
  1037.   plateau_steps *= 1.01; // This is to compensate we use discrete intervals
  1038.   acceleration_enabled = true;
  1039.   long full_interval = interval;
  1040.   if(interval > max_interval) acceleration_enabled = false;
  1041.   boolean decelerating = false;
  1042.   #endif
  1043.   #ifdef EXP_ACCELERATION
  1044.   acceleration_enabled = true;
  1045.   if(full_velocity_steps == 0) full_velocity_steps++;
  1046.   if(interval > max_interval) acceleration_enabled = false;
  1047.   unsigned long full_interval = interval;
  1048.   if(min_constant_speed_steps >= steps_to_take) {
  1049.     acceleration_enabled = false;
  1050.     full_interval = max(max_interval, interval); // choose the min speed between feedrate and acceleration start speed
  1051.   }
  1052.   if(full_velocity_steps < virtual_full_velocity_steps && acceleration_enabled) full_interval = max(interval,
  1053.       max_interval - ((max_interval - full_interval) * full_velocity_steps / virtual_full_velocity_steps)); // choose the min speed between feedrate and speed at full steps
  1054.   unsigned int steps_acceleration_check = 1;
  1055.   accelerating = acceleration_enabled;
  1056.   #endif
  1057.  
  1058.   unsigned long start_move_micros = micros();
  1059.   previous_micros_x = start_move_micros*100;
  1060.   previous_micros_y = previous_micros_x;
  1061.   previous_micros_z = previous_micros_x;
  1062.   previous_micros_e = previous_micros_x;
  1063.  
  1064.   //move until no more steps remain
  1065.   while(x_steps_remaining + y_steps_remaining + z_steps_remaining + e_steps_remaining > 0) {
  1066.     //If more that HEATER_CHECK_INTERVAL ms have passed since previous heating check, adjust temp
  1067.     manage_heater();
  1068.     manage_inactivity(2);
  1069.     #ifdef RAMP_ACCELERATION
  1070.     //If acceleration is enabled on this move and we are in the acceleration segment, calculate the current interval
  1071.     if (acceleration_enabled && steps_done == 0) {
  1072.         interval = max_interval;
  1073.     } else if (acceleration_enabled && steps_done <= plateau_steps) {
  1074.         long current_speed = (long) ((((long) steps_per_sqr_second) / 10000)
  1075.         * ((micros() - start_move_micros)  / 100) + (long) min_speed_steps_per_second);
  1076.         interval = 100000000 / current_speed;
  1077.       if (interval < full_interval) {
  1078.         accelerating = false;
  1079.         interval = full_interval;
  1080.       }
  1081.       if (steps_done >= steps_to_take / 2) {
  1082.     plateau_steps = steps_done;
  1083.     max_speed_steps_per_second = 100000000 / interval;
  1084.     accelerating = false;
  1085.       }
  1086.     } else if (acceleration_enabled && steps_remaining <= plateau_steps) { //(interval > minInterval * 100) {
  1087.       if (!accelerating) {
  1088.         start_move_micros = micros();
  1089.         accelerating = true;
  1090.         decelerating = true;
  1091.       }            
  1092.       long current_speed = (long) ((long) max_speed_steps_per_second - ((((long) steps_per_sqr_second) / 10000)
  1093.           * ((micros() - start_move_micros) / 100)));
  1094.       interval = 100000000 / current_speed;
  1095.       if (interval > max_interval)
  1096.     interval = max_interval;
  1097.     } else {
  1098.       //Else, we are just use the full speed interval as current interval
  1099.       interval = full_interval;
  1100.       accelerating = false;
  1101.     }
  1102.     #endif
  1103.     #ifdef EXP_ACCELERATION
  1104.     //If acceleration is enabled on this move and we are in the acceleration segment, calculate the current interval
  1105.     if (acceleration_enabled && steps_done < full_velocity_steps && steps_done / full_velocity_steps < 1 && (steps_done % steps_acceleration_check == 0)) {
  1106.       if(steps_done == 0) {
  1107.         interval = max_interval;
  1108.       } else {
  1109.         interval = max_interval - ((max_interval - full_interval) * steps_done / virtual_full_velocity_steps);
  1110.       }
  1111.     } else if (acceleration_enabled && steps_remaining < full_velocity_steps) {
  1112.       //Else, if acceleration is enabled on this move and we are in the deceleration segment, calculate the current interval
  1113.       if(steps_remaining == 0) {
  1114.         interval = max_interval;
  1115.       } else {
  1116.         interval = max_interval - ((max_interval - full_interval) * steps_remaining / virtual_full_velocity_steps);
  1117.       }
  1118.       accelerating = true;
  1119.     } else if (steps_done - full_velocity_steps >= 1 || !acceleration_enabled){
  1120.       //Else, we are just use the full speed interval as current interval
  1121.       interval = full_interval;
  1122.       accelerating = false;
  1123.     }
  1124.     #endif
  1125.  
  1126.     //If there are x or y steps remaining, perform Bresenham algorithm
  1127.     if(x_steps_remaining || y_steps_remaining) {
  1128.       if(X_MIN_PIN > -1) if(!direction_x) if(digitalRead(X_MIN_PIN) != ENDSTOPS_INVERTING) break;
  1129.       if(Y_MIN_PIN > -1) if(!direction_y) if(digitalRead(Y_MIN_PIN) != ENDSTOPS_INVERTING) break;
  1130.       if(X_MAX_PIN > -1) if(direction_x) if(digitalRead(X_MAX_PIN) != ENDSTOPS_INVERTING) break;
  1131.       if(Y_MAX_PIN > -1) if(direction_y) if(digitalRead(Y_MAX_PIN) != ENDSTOPS_INVERTING) break;
  1132.       if(steep_y) {
  1133.         timediff = micros() * 100 - previous_micros_y;
  1134.         while(timediff >= interval && y_steps_remaining > 0) {
  1135.           steps_done++;
  1136.           steps_remaining--;
  1137.           y_steps_remaining--; timediff -= interval;
  1138.           error_x = error_x - delta_x;
  1139.           do_y_step();
  1140.           if(error_x < 0) {
  1141.             do_x_step(); x_steps_remaining--;
  1142.             error_x = error_x + delta_y;
  1143.           }
  1144.           #ifdef RAMP_ACCELERATION
  1145.           if (steps_remaining == plateau_steps || (steps_done >= steps_to_take / 2 && accelerating && !decelerating)) break;
  1146.           #endif
  1147.           #ifdef STEP_DELAY_RATIO
  1148.           if(timediff >= interval) delayMicroseconds(long_step_delay_ratio * interval / 10000);
  1149.           #endif
  1150.           #ifdef STEP_DELAY_MICROS
  1151.           if(timediff >= interval) delayMicroseconds(STEP_DELAY_MICROS);
  1152.           #endif
  1153.         }
  1154.       } else if (steep_x) {
  1155.         timediff=micros() * 100 - previous_micros_x;
  1156.         while(timediff >= interval && x_steps_remaining>0) {
  1157.           steps_done++;
  1158.           steps_remaining--;
  1159.           x_steps_remaining--; timediff -= interval;
  1160.           error_y = error_y - delta_y;
  1161.           do_x_step();
  1162.           if(error_y < 0) {
  1163.             do_y_step(); y_steps_remaining--;
  1164.             error_y = error_y + delta_x;
  1165.           }
  1166.           #ifdef RAMP_ACCELERATION
  1167.           if (steps_remaining == plateau_steps || (steps_done >= steps_to_take / 2 && accelerating && !decelerating)) break;
  1168.           #endif
  1169.           #ifdef STEP_DELAY_RATIO
  1170.           if(timediff >= interval) delayMicroseconds(long_step_delay_ratio * interval / 10000);
  1171.           #endif
  1172.           #ifdef STEP_DELAY_MICROS
  1173.           if(timediff >= interval) delayMicroseconds(STEP_DELAY_MICROS);
  1174.           #endif
  1175.         }
  1176.       }
  1177.     }
  1178.     #ifdef RAMP_ACCELERATION
  1179.     if((x_steps_remaining>0 || y_steps_remaining>0) &&
  1180.         steps_to_take > 0 &&
  1181.         (steps_remaining == plateau_steps || (steps_done >= steps_to_take / 2 && accelerating && !decelerating))) continue;
  1182.     #endif
  1183.  
  1184.     //If there are z steps remaining, check if z steps must be taken
  1185.     if(z_steps_remaining) {
  1186.       if(Z_MIN_PIN > -1) if(!direction_z) if(digitalRead(Z_MIN_PIN) != ENDSTOPS_INVERTING) break;
  1187.       if(Z_MAX_PIN > -1) if(direction_z) if(digitalRead(Z_MAX_PIN) != ENDSTOPS_INVERTING) break;
  1188.       timediff = micros() * 100-previous_micros_z;
  1189.       while(timediff >= z_interval && z_steps_remaining) {
  1190.         do_z_step();
  1191.         z_steps_remaining--;
  1192.         timediff -= z_interval;
  1193.         #ifdef STEP_DELAY_RATIO
  1194.         if(timediff >= z_interval) delayMicroseconds(long_step_delay_ratio * z_interval / 10000);
  1195.         #endif
  1196.         #ifdef STEP_DELAY_MICROS
  1197.         if(timediff >= z_interval) delayMicroseconds(STEP_DELAY_MICROS);
  1198.         #endif
  1199.       }
  1200.     }
  1201.  
  1202.     //If there are e steps remaining, check if e steps must be taken
  1203.     if(e_steps_remaining){
  1204.       if (x_steps_to_take + y_steps_to_take <= 0) timediff = micros()*100 - previous_micros_e;
  1205.       unsigned int final_e_steps_remaining = 0;
  1206.       if (steep_x && x_steps_to_take > 0) final_e_steps_remaining = e_steps_to_take * x_steps_remaining / x_steps_to_take;
  1207.       else if (steep_y && y_steps_to_take > 0) final_e_steps_remaining = e_steps_to_take * y_steps_remaining / y_steps_to_take;
  1208.       //If this move has X or Y steps, let E follow the Bresenham pace
  1209.       if (final_e_steps_remaining > 0)  while(e_steps_remaining > final_e_steps_remaining) { do_e_step(); e_steps_remaining--;}
  1210.       else if (x_steps_to_take + y_steps_to_take > 0)  while(e_steps_remaining) { do_e_step(); e_steps_remaining--;}
  1211.       //Else, normally check if e steps must be taken
  1212.       else while (timediff >= e_interval && e_steps_remaining) {
  1213.         do_e_step();
  1214.         e_steps_remaining--;
  1215.         timediff -= e_interval;
  1216.         #ifdef STEP_DELAY_RATIO
  1217.         if(timediff >= e_interval) delayMicroseconds(long_step_delay_ratio * e_interval / 10000);
  1218.         #endif
  1219.         #ifdef STEP_DELAY_MICROS
  1220.         if(timediff >= e_interval) delayMicroseconds(STEP_DELAY_MICROS);
  1221.         #endif
  1222.       }
  1223.     }
  1224.   }
  1225.  
  1226.   if(DISABLE_X) disable_x();
  1227.   if(DISABLE_Y) disable_y();
  1228.   if(DISABLE_Z) disable_z();
  1229.   if(DISABLE_E) disable_e();
  1230.  
  1231.   // Update current position partly based on direction, we probably can combine this with the direction code above...
  1232.   if (destination_x > current_x) current_x = current_x + x_steps_to_take / x_steps_per_unit;
  1233.   else current_x = current_x - x_steps_to_take / x_steps_per_unit;
  1234.   if (destination_y > current_y) current_y = current_y + y_steps_to_take / y_steps_per_unit;
  1235.   else current_y = current_y - y_steps_to_take / y_steps_per_unit;
  1236.   if (destination_z > current_z) current_z = current_z + z_steps_to_take / z_steps_per_unit;
  1237.   else current_z = current_z - z_steps_to_take / z_steps_per_unit;
  1238.   if (destination_e > current_e) current_e = current_e + e_steps_to_take / e_steps_per_unit;
  1239.   else current_e = current_e - e_steps_to_take / e_steps_per_unit;
  1240. }
  1241.  
  1242.  
  1243. inline void do_x_step()
  1244. {
  1245.   digitalWrite(X_STEP_PIN, HIGH);
  1246.   previous_micros_x += interval;
  1247.   //delayMicroseconds(3);
  1248.   digitalWrite(X_STEP_PIN, LOW);
  1249. }
  1250.  
  1251. inline void do_y_step()
  1252. {
  1253.   digitalWrite(Y_STEP_PIN, HIGH);
  1254.   previous_micros_y += interval;
  1255.   //delayMicroseconds(3);
  1256.   digitalWrite(Y_STEP_PIN, LOW);
  1257. }
  1258.  
  1259. inline void do_z_step()
  1260. {
  1261.   digitalWrite(Z_STEP_PIN, HIGH);
  1262.   previous_micros_z += z_interval;
  1263.   //delayMicroseconds(3);
  1264.   digitalWrite(Z_STEP_PIN, LOW);
  1265. }
  1266.  
  1267. inline void do_e_step()
  1268. {
  1269.   digitalWrite(E_STEP_PIN, HIGH);
  1270.   previous_micros_e += e_interval;
  1271.   //delayMicroseconds(3);
  1272.   digitalWrite(E_STEP_PIN, LOW);
  1273. }
  1274.  
  1275. inline void disable_x() { if(X_ENABLE_PIN > -1) digitalWrite(X_ENABLE_PIN,!X_ENABLE_ON); }
  1276. inline void disable_y() { if(Y_ENABLE_PIN > -1) digitalWrite(Y_ENABLE_PIN,!Y_ENABLE_ON); }
  1277. inline void disable_z() { if(Z_ENABLE_PIN > -1) digitalWrite(Z_ENABLE_PIN,!Z_ENABLE_ON); }
  1278. inline void disable_e() { if(E_ENABLE_PIN > -1) digitalWrite(E_ENABLE_PIN,!E_ENABLE_ON); }
  1279. inline void  enable_x() { if(X_ENABLE_PIN > -1) digitalWrite(X_ENABLE_PIN, X_ENABLE_ON); }
  1280. inline void  enable_y() { if(Y_ENABLE_PIN > -1) digitalWrite(Y_ENABLE_PIN, Y_ENABLE_ON); }
  1281. inline void  enable_z() { if(Z_ENABLE_PIN > -1) digitalWrite(Z_ENABLE_PIN, Z_ENABLE_ON); }
  1282. inline void  enable_e() { if(E_ENABLE_PIN > -1) digitalWrite(E_ENABLE_PIN, E_ENABLE_ON); }
  1283.  
  1284. #define HEAT_INTERVAL 250
  1285. #ifdef HEATER_USES_MAX6675
  1286. unsigned long max6675_previous_millis = 0;
  1287. int max6675_temp = 2000;
  1288.  
  1289. inline int read_max6675()
  1290. {
  1291.   if (millis() - max6675_previous_millis < HEAT_INTERVAL)
  1292.     return max6675_temp;
  1293.  
  1294.   max6675_previous_millis = millis();
  1295.  
  1296.   max6675_temp = 0;
  1297.    
  1298.   #ifdef    PRR
  1299.     PRR &= ~(1<<PRSPI);
  1300.   #elif defined PRR0
  1301.     PRR0 &= ~(1<<PRSPI);
  1302.   #endif
  1303.  
  1304.   SPCR = (1<<MSTR) | (1<<SPE) | (1<<SPR0);
  1305.  
  1306.   // enable TT_MAX6675
  1307.   digitalWrite(MAX6675_SS, 0);
  1308.  
  1309.   // ensure 100ns delay - a bit extra is fine
  1310.   delay(1);
  1311.  
  1312.   // read MSB
  1313.   SPDR = 0;
  1314.   for (;(SPSR & (1<<SPIF)) == 0;);
  1315.   max6675_temp = SPDR;
  1316.   max6675_temp <<= 8;
  1317.  
  1318.   // read LSB
  1319.   SPDR = 0;
  1320.   for (;(SPSR & (1<<SPIF)) == 0;);
  1321.   max6675_temp |= SPDR;
  1322.  
  1323.   // disable TT_MAX6675
  1324.   digitalWrite(MAX6675_SS, 1);
  1325.  
  1326.   if (max6675_temp & 4)
  1327.   {
  1328.     // thermocouple open
  1329.     max6675_temp = 2000;
  1330.   }
  1331.   else
  1332.   {
  1333.     max6675_temp = max6675_temp >> 3;
  1334.   }
  1335.  
  1336.   return max6675_temp;
  1337. }
  1338. #endif
  1339.  
  1340.  
  1341. inline void manage_heater()
  1342. {
  1343.   if((millis() - previous_millis_heater) < HEATER_CHECK_INTERVAL )
  1344.     return;
  1345.   previous_millis_heater = millis();
  1346.   #ifdef HEATER_USES_THERMISTOR
  1347.     current_raw = analogRead(TEMP_0_PIN);
  1348.    
  1349.     // permet d'allumer la led bicolore
  1350.     if(analogRead(TEMP_1_PIN) < 500)
  1351.     {
  1352.         digitalWrite(LED_TEMP_WAIT, HIGH);
  1353.         digitalWrite(LED_TEMP_READY, LOW);
  1354.     }
  1355.     else
  1356.     {
  1357.         digitalWrite(LED_TEMP_WAIT, LOW);
  1358.         digitalWrite(LED_TEMP_READY, HIGH);
  1359.     }
  1360.    
  1361.     // When using thermistor, when the heater is colder than targer temp, we get a higher analog reading than target,
  1362.     // this switches it up so that the reading appears lower than target for the control logic.
  1363.     current_raw = 1023 - current_raw;
  1364.   #elif defined HEATER_USES_AD595
  1365.     current_raw = analogRead(TEMP_0_PIN);    
  1366.   #elif defined HEATER_USES_MAX6675
  1367.     current_raw = read_max6675();
  1368.   #endif
  1369.   #ifdef SMOOTHING
  1370.   nma = (nma + current_raw) - (nma / SMOOTHFACTOR);
  1371.   current_raw = nma / SMOOTHFACTOR;
  1372.   #endif
  1373.   #ifdef WATCHPERIOD
  1374.     if(watchmillis && millis() - watchmillis > WATCHPERIOD){
  1375.         if(watch_raw + 1 >= current_raw){
  1376.             target_raw = 0;
  1377.             digitalWrite(HEATER_0_PIN,LOW);
  1378.             digitalWrite(LED_PIN,LOW);
  1379.         }else{
  1380.             watchmillis = 0;
  1381.         }
  1382.     }
  1383.   #endif
  1384.   #ifdef MINTEMP
  1385.     if(current_raw <= minttemp)
  1386.         target_raw = 0;
  1387.   #endif
  1388.   #ifdef MAXTEMP
  1389.     if(current_raw >= maxttemp) {
  1390.         target_raw = 0;
  1391.     }
  1392.   #endif
  1393.   #if (TEMP_0_PIN > -1) || defined (HEATER_USES_MAX66675)
  1394.     #ifdef PIDTEMP
  1395.       error = target_raw - current_raw;
  1396.       pTerm = (PID_PGAIN * error) / 100;
  1397.       temp_iState += error;
  1398.       temp_iState = constrain(temp_iState, temp_iState_min, temp_iState_max);
  1399.       iTerm = (PID_IGAIN * temp_iState) / 100;
  1400.       dTerm = (PID_DGAIN * (current_raw - temp_dState)) / 100;
  1401.       temp_dState = current_raw;
  1402.       analogWrite(HEATER_0_PIN, constrain(pTerm + iTerm - dTerm, 0, PID_MAX));
  1403.     #else
  1404.       if(current_raw >= target_raw)
  1405.       {
  1406.         digitalWrite(HEATER_0_PIN,LOW);
  1407.         digitalWrite(LED_PIN,LOW);
  1408.       }
  1409.       else
  1410.       {
  1411.         digitalWrite(HEATER_0_PIN,HIGH);
  1412.         digitalWrite(LED_PIN,HIGH);
  1413.       }
  1414.     #endif
  1415.   #endif
  1416.    
  1417.   if(millis() - previous_millis_bed_heater < BED_CHECK_INTERVAL)
  1418.     return;
  1419.   previous_millis_bed_heater = millis();
  1420.  
  1421.   #ifdef BED_USES_THERMISTOR
  1422.  
  1423.     current_bed_raw = analogRead(TEMP_1_PIN);                  
  1424.  
  1425.     // If using thermistor, when the heater is colder than targer temp, we get a higher analog reading than target,
  1426.     // this switches it up so that the reading appears lower than target for the control logic.
  1427.     current_bed_raw = 1023 - current_bed_raw;
  1428.   #elif defined BED_USES_AD595
  1429.     current_bed_raw = analogRead(TEMP_1_PIN);                  
  1430.  
  1431.   #endif
  1432.  
  1433.  
  1434.   #if TEMP_1_PIN > -1
  1435.     if(current_bed_raw >= target_bed_raw)
  1436.     {
  1437.       digitalWrite(HEATER_1_PIN,LOW);
  1438.     }
  1439.     else
  1440.     {
  1441.       digitalWrite(HEATER_1_PIN,HIGH);
  1442.     }
  1443.   #endif
  1444. }
  1445.  
  1446. // Takes hot end temperature value as input and returns corresponding raw value.
  1447. // For a thermistor, it uses the RepRap thermistor temp table.
  1448. // This is needed because PID in hydra firmware hovers around a given analog value, not a temp value.
  1449. // This function is derived from inversing the logic from a portion of getTemperature() in FiveD RepRap firmware.
  1450. float temp2analog(int celsius) {
  1451.   #ifdef HEATER_USES_THERMISTOR
  1452.     int raw = 0;
  1453.     byte i;
  1454.    
  1455.     for (i=1; i<NUMTEMPS; i++)
  1456.     {
  1457.       if (temptable[i][1] < celsius)
  1458.       {
  1459.         raw = temptable[i-1][0] +
  1460.           (celsius - temptable[i-1][1]) *
  1461.           (temptable[i][0] - temptable[i-1][0]) /
  1462.           (temptable[i][1] - temptable[i-1][1]);
  1463.      
  1464.         break;
  1465.       }
  1466.     }
  1467.  
  1468.     // Overflow: Set to last value in the table
  1469.     if (i == NUMTEMPS) raw = temptable[i-1][0];
  1470.  
  1471.     return 1023 - raw;
  1472.   #elif defined HEATER_USES_AD595
  1473.     return celsius * (1024.0 / (5.0 * 100.0) );
  1474.   #elif defined HEATER_USES_MAX6675
  1475.     return celsius * 4.0;
  1476.   #endif
  1477. }
  1478.  
  1479. // Takes bed temperature value as input and returns corresponding raw value.
  1480. // For a thermistor, it uses the RepRap thermistor temp table.
  1481. // This is needed because PID in hydra firmware hovers around a given analog value, not a temp value.
  1482. // This function is derived from inversing the logic from a portion of getTemperature() in FiveD RepRap firmware.
  1483. float temp2analogBed(int celsius) {
  1484.   #ifdef BED_USES_THERMISTOR
  1485.  
  1486.     int raw = 0;
  1487.     byte i;
  1488.    
  1489.     for (i=1; i<BNUMTEMPS; i++)
  1490.     {
  1491.       if (bedtemptable[i][1] < celsius)
  1492.       {
  1493.         raw = bedtemptable[i-1][0] +
  1494.           (celsius - bedtemptable[i-1][1]) *
  1495.           (bedtemptable[i][0] - bedtemptable[i-1][0]) /
  1496.           (bedtemptable[i][1] - bedtemptable[i-1][1]);
  1497.      
  1498.         break;
  1499.       }
  1500.     }
  1501.  
  1502.     // Overflow: Set to last value in the table
  1503.     if (i == BNUMTEMPS) raw = bedtemptable[i-1][0];
  1504.  
  1505.     return 1023 - raw;
  1506.   #elif defined BED_USES_AD595
  1507.     return celsius * (1024.0 / (5.0 * 100.0) );
  1508.   #endif
  1509. }
  1510.  
  1511. // Derived from RepRap FiveD extruder::getTemperature()
  1512. // For hot end temperature measurement.
  1513. float analog2temp(int raw) {
  1514.   #ifdef HEATER_USES_THERMISTOR
  1515.     int celsius = 0;
  1516.     byte i;
  1517.    
  1518.     raw = 1023 - raw;
  1519.  
  1520.     for (i=1; i<NUMTEMPS; i++)
  1521.     {
  1522.       if (temptable[i][0] > raw)
  1523.       {
  1524.         celsius  = temptable[i-1][1] +
  1525.           (raw - temptable[i-1][0]) *
  1526.           (temptable[i][1] - temptable[i-1][1]) /
  1527.           (temptable[i][0] - temptable[i-1][0]);
  1528.  
  1529.         break;
  1530.       }
  1531.     }
  1532.  
  1533.     // Overflow: Set to last value in the table
  1534.     if (i == NUMTEMPS) celsius = temptable[i-1][1];
  1535.  
  1536.     return celsius;
  1537.   #elif defined HEATER_USES_AD595
  1538.     return raw * ((5.0 * 100.0) / 1024.0);
  1539.   #elif defined HEATER_USES_MAX6675
  1540.     return raw * 0.25;
  1541.   #endif
  1542. }
  1543.  
  1544. // Derived from RepRap FiveD extruder::getTemperature()
  1545. // For bed temperature measurement.
  1546. float analog2tempBed(int raw) {
  1547.   #ifdef BED_USES_THERMISTOR
  1548.     int celsius = 0;
  1549.     byte i;
  1550.  
  1551.     raw = 1023 - raw;
  1552.  
  1553.     for (i=1; i<NUMTEMPS; i++)
  1554.     {
  1555.       if (bedtemptable[i][0] > raw)
  1556.       {
  1557.         celsius  = bedtemptable[i-1][1] +
  1558.           (raw - bedtemptable[i-1][0]) *
  1559.           (bedtemptable[i][1] - bedtemptable[i-1][1]) /
  1560.           (bedtemptable[i][0] - bedtemptable[i-1][0]);
  1561.  
  1562.         break;
  1563.       }
  1564.     }
  1565.  
  1566.     // Overflow: Set to last value in the table
  1567.     if (i == NUMTEMPS) celsius = bedtemptable[i-1][1];
  1568.  
  1569.     return celsius;
  1570.    
  1571.   #elif defined BED_USES_AD595
  1572.     return raw * ((5.0 * 100.0) / 1024.0);
  1573.   #endif
  1574. }
  1575.  
  1576. inline void kill()
  1577. {
  1578.   #if TEMP_0_PIN > -1
  1579.   target_raw=0;
  1580.   digitalWrite(HEATER_0_PIN,LOW);
  1581.   #endif
  1582.   #if TEMP_1_PIN > -1
  1583.   target_bed_raw=0;
  1584.   if(HEATER_1_PIN > -1) digitalWrite(HEATER_1_PIN,LOW);
  1585.   #endif
  1586.   disable_x();
  1587.   disable_y();
  1588.   disable_z();
  1589.   disable_e();
  1590.  
  1591.   if(PS_ON_PIN > -1) pinMode(PS_ON_PIN,INPUT);
  1592.  
  1593. }
  1594.  
  1595. inline void manage_inactivity(byte debug) {
  1596. if( (millis()-previous_millis_cmd) >  max_inactive_time ) if(max_inactive_time) kill();
  1597. if( (millis()-previous_millis_cmd) >  stepper_inactive_time ) if(stepper_inactive_time) { disable_x(); disable_y(); disable_z(); disable_e(); }
  1598. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!