API tools faq
paste
Advertisement
Guest User

Modificaciones al Marlin para autolevel sin subir el servo

a guest
Mar 23rd, 2014
334
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 123.20 KB | None | 0 0
raw download clone embed print report
  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. #ifdef ENABLE_AUTO_BED_LEVELING
  33. #include "vector_3.h"
  34. #ifdef AUTO_BED_LEVELING_GRID
  35. #include "qr_solve.h"
  36. #endif
  37. #endif // ENABLE_AUTO_BED_LEVELING
  38.  
  39. #include "ultralcd.h"
  40. #include "planner.h"
  41. #include "stepper.h"
  42. #include "temperature.h"
  43. #include "motion_control.h"
  44. #include "cardreader.h"
  45. #include "watchdog.h"
  46. #include "ConfigurationStore.h"
  47. #include "language.h"
  48. #include "pins_arduino.h"
  49. #include "math.h"
  50.  
  51. #ifdef BLINKM
  52. #include "BlinkM.h"
  53. #include "Wire.h"
  54. #endif
  55.  
  56. #if NUM_SERVOS > 0
  57. #include "Servo.h"
  58. #endif
  59.  
  60. #if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1
  61. #include <SPI.h>
  62. #endif
  63.  
  64. #define VERSION_STRING "1.0.0"
  65.  
  66. // look here for descriptions of G-codes: http://linuxcnc.org/handbook/gcode/g-code.html
  67. // http://objects.reprap.org/wiki/Mendel_User_Manual:_RepRapGCodes
  68.  
  69. //Implemented Codes
  70. //-------------------
  71. // G0 -> G1
  72. // G1 - Coordinated Movement X Y Z E
  73. // G2 - CW ARC
  74. // G3 - CCW ARC
  75. // G4 - Dwell S<seconds> or P<milliseconds>
  76. // G10 - retract filament according to settings of M207
  77. // G11 - retract recover filament according to settings of M208
  78. // G28 - Home all Axis
  79. // G29 - Detailed Z-Probe, probes the bed at 3 or more points. Will fail if you haven't homed yet.
  80. // G30 - Single Z Probe, probes bed at current XY location.
  81. // G90 - Use Absolute Coordinates
  82. // G91 - Use Relative Coordinates
  83. // G92 - Set current position to coordinates given
  84.  
  85. // M Codes
  86. // M0 - Unconditional stop - Wait for user to press a button on the LCD (Only if ULTRA_LCD is enabled)
  87. // M1 - Same as M0
  88. // M17 - Enable/Power all stepper motors
  89. // M18 - Disable all stepper motors; same as M84
  90. // M20 - List SD card
  91. // M21 - Init SD card
  92. // M22 - Release SD card
  93. // M23 - Select SD file (M23 filename.g)
  94. // M24 - Start/resume SD print
  95. // M25 - Pause SD print
  96. // M26 - Set SD position in bytes (M26 S12345)
  97. // M27 - Report SD print status
  98. // M28 - Start SD write (M28 filename.g)
  99. // M29 - Stop SD write
  100. // M30 - Delete file from SD (M30 filename.g)
  101. // M31 - Output time since last M109 or SD card start to serial
  102. // M32 - Select file and start SD print (Can be used _while_ printing from SD card files):
  103. // syntax "M32 /path/filename#", or "M32 S<startpos bytes> !filename#"
  104. // Call gcode file : "M32 P !filename#" and return to caller file after finishing (similar to #include).
  105. // The '#' is necessary when calling from within sd files, as it stops buffer prereading
  106. // M42 - Change pin status via gcode Use M42 Px Sy to set pin x to value y, when omitting Px the onboard led will be used.
  107. // M80 - Turn on Power Supply
  108. // M81 - Turn off Power Supply
  109. // M82 - Set E codes absolute (default)
  110. // M83 - Set E codes relative while in Absolute Coordinates (G90) mode
  111. // M84 - Disable steppers until next move,
  112. // or use S<seconds> to specify an inactivity timeout, after which the steppers will be disabled. S0 to disable the timeout.
  113. // M85 - Set inactivity shutdown timer with parameter S<seconds>. To disable set zero (default)
  114. // M92 - Set axis_steps_per_unit - same syntax as G92
  115. // M104 - Set extruder target temp
  116. // M105 - Read current temp
  117. // M106 - Fan on
  118. // M107 - Fan off
  119. // M109 - Sxxx Wait for extruder current temp to reach target temp. Waits only when heating
  120. // Rxxx Wait for extruder current temp to reach target temp. Waits when heating and cooling
  121. // M114 - Output current position to serial port
  122. // M115 - Capabilities string
  123. // M117 - display message
  124. // M119 - Output Endstop status to serial port
  125. // M126 - Solenoid Air Valve Open (BariCUDA support by jmil)
  126. // M127 - Solenoid Air Valve Closed (BariCUDA vent to atmospheric pressure by jmil)
  127. // M128 - EtoP Open (BariCUDA EtoP = electricity to air pressure transducer by jmil)
  128. // M129 - EtoP Closed (BariCUDA EtoP = electricity to air pressure transducer by jmil)
  129. // M140 - Set bed target temp
  130. // M150 - Set BlinkM Color Output R: Red<0-255> U(!): Green<0-255> B: Blue<0-255> over i2c, G for green does not work.
  131. // M190 - Sxxx Wait for bed current temp to reach target temp. Waits only when heating
  132. // Rxxx Wait for bed current temp to reach target temp. Waits when heating and cooling
  133. // M200 D<millimeters>- set filament diameter and set E axis units to cubic millimeters (use S0 to set back to millimeters).
  134. // M201 - Set max acceleration in units/s^2 for print moves (M201 X1000 Y1000)
  135. // M202 - Set max acceleration in units/s^2 for travel moves (M202 X1000 Y1000) Unused in Marlin!!
  136. // M203 - Set maximum feedrate that your machine can sustain (M203 X200 Y200 Z300 E10000) in mm/sec
  137. // M204 - Set default acceleration: S normal moves T filament only moves (M204 S3000 T7000) in mm/sec^2 also sets minimum segment time in ms (B20000) to prevent buffer under-runs and M20 minimum feedrate
  138. // 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
  139. // M206 - set additional homing offset
  140. // M207 - set retract length S[positive mm] F[feedrate mm/min] Z[additional zlift/hop], stays in mm regardless of M200 setting
  141. // M208 - set recover=unretract length S[positive mm surplus to the M207 S*] F[feedrate mm/sec]
  142. // 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.
  143. // M218 - set hotend offset (in mm): T<extruder_number> X<offset_on_X> Y<offset_on_Y>
  144. // M220 S<factor in percent>- set speed factor override percentage
  145. // M221 S<factor in percent>- set extrude factor override percentage
  146. // M226 P<pin number> S<pin state>- Wait until the specified pin reaches the state required
  147. // M240 - Trigger a camera to take a photograph
  148. // M250 - Set LCD contrast C<contrast value> (value 0..63)
  149. // M280 - set servo position absolute. P: servo index, S: angle or microseconds
  150. // M300 - Play beep sound S<frequency Hz> P<duration ms>
  151. // M301 - Set PID parameters P I and D
  152. // M302 - Allow cold extrudes, or set the minimum extrude S<temperature>.
  153. // M303 - PID relay autotune S<temperature> sets the target temperature. (default target temperature = 150C)
  154. // M304 - Set bed PID parameters P I and D
  155. // M400 - Finish all moves
  156. // M401 - Lower z-probe if present
  157. // M402 - Raise z-probe if present
  158. // M500 - stores parameters in EEPROM
  159. // M501 - reads parameters from EEPROM (if you need reset them after you changed them temporarily).
  160. // M502 - reverts to the default "factory settings". You still need to store them in EEPROM afterwards if you want to.
  161. // M503 - print the current settings (from memory not from EEPROM)
  162. // M540 - Use S[0|1] to enable or disable the stop SD card print on endstop hit (requires ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
  163. // M600 - Pause for filament change X[pos] Y[pos] Z[relative lift] E[initial retract] L[later retract distance for removal]
  164. // M665 - set delta configurations
  165. // M666 - set delta endstop adjustment
  166. // M605 - Set dual x-carriage movement mode: S<mode> [ X<duplication x-offset> R<duplication temp offset> ]
  167. // M907 - Set digital trimpot motor current using axis codes.
  168. // M908 - Control digital trimpot directly.
  169. // M350 - Set microstepping mode.
  170. // M351 - Toggle MS1 MS2 pins directly.
  171. // M928 - Start SD logging (M928 filename.g) - ended by M29
  172. // M999 - Restart after being stopped by error
  173.  
  174. //Stepper Movement Variables
  175.  
  176. //===========================================================================
  177. //=============================imported variables============================
  178. //===========================================================================
  179.  
  180.  
  181. //===========================================================================
  182. //=============================public variables=============================
  183. //===========================================================================
  184. #ifdef SDSUPPORT
  185. CardReader card;
  186. #endif
  187. float homing_feedrate[] = HOMING_FEEDRATE;
  188. bool axis_relative_modes[] = AXIS_RELATIVE_MODES;
  189. int feedmultiply=100; //100->1 200->2
  190. int saved_feedmultiply;
  191. int extrudemultiply=100; //100->1 200->2
  192. int extruder_multiply[EXTRUDERS] = {100
  193. #if EXTRUDERS > 1
  194. , 100
  195. #if EXTRUDERS > 2
  196. , 100
  197. #endif
  198. #endif
  199. };
  200. float volumetric_multiplier[EXTRUDERS] = {1.0
  201. #if EXTRUDERS > 1
  202. , 1.0
  203. #if EXTRUDERS > 2
  204. , 1.0
  205. #endif
  206. #endif
  207. };
  208. float current_position[NUM_AXIS] = { 0.0, 0.0, 0.0, 0.0 };
  209. float add_homeing[3]={0,0,0};
  210. #ifdef DELTA
  211. float endstop_adj[3]={0,0,0};
  212. #endif
  213. float min_pos[3] = { X_MIN_POS, Y_MIN_POS, Z_MIN_POS };
  214. float max_pos[3] = { X_MAX_POS, Y_MAX_POS, Z_MAX_POS };
  215. bool axis_known_position[3] = {false, false, false};
  216. float zprobe_zoffset;
  217.  
  218. // Extruder offset
  219. #if EXTRUDERS > 1
  220. #ifndef DUAL_X_CARRIAGE
  221. #define NUM_EXTRUDER_OFFSETS 2 // only in XY plane
  222. #else
  223. #define NUM_EXTRUDER_OFFSETS 3 // supports offsets in XYZ plane
  224. #endif
  225. float extruder_offset[NUM_EXTRUDER_OFFSETS][EXTRUDERS] = {
  226. #if defined(EXTRUDER_OFFSET_X) && defined(EXTRUDER_OFFSET_Y)
  227. EXTRUDER_OFFSET_X, EXTRUDER_OFFSET_Y
  228. #endif
  229. };
  230. #endif
  231. uint8_t active_extruder = 0;
  232. int fanSpeed=0;
  233. #ifdef SERVO_ENDSTOPS
  234. int servo_endstops[] = SERVO_ENDSTOPS;
  235. int servo_endstop_angles[] = SERVO_ENDSTOP_ANGLES;
  236. #endif
  237. #ifdef BARICUDA
  238. int ValvePressure=0;
  239. int EtoPPressure=0;
  240. #endif
  241.  
  242. #ifdef FWRETRACT
  243. bool autoretract_enabled=false;
  244. bool retracted=false;
  245. float retract_length = RETRACT_LENGTH;
  246. float retract_feedrate = RETRACT_FEEDRATE;
  247. float retract_zlift = RETRACT_ZLIFT;
  248. float retract_recover_length = RETRACT_RECOVER_LENGTH;
  249. float retract_recover_feedrate = RETRACT_RECOVER_FEEDRATE;
  250. #endif
  251.  
  252. #ifdef ULTIPANEL
  253. #ifdef PS_DEFAULT_OFF
  254. bool powersupply = false;
  255. #else
  256. bool powersupply = true;
  257. #endif
  258. #endif
  259.  
  260. #ifdef DELTA
  261. float delta[3] = {0.0, 0.0, 0.0};
  262. #define SIN_60 0.8660254037844386
  263. #define COS_60 0.5
  264. // these are the default values, can be overriden with M665
  265. float delta_radius= DELTA_RADIUS;
  266. float delta_tower1_x= -SIN_60*delta_radius; // front left tower
  267. float delta_tower1_y= -COS_60*delta_radius;
  268. float delta_tower2_x= SIN_60*delta_radius; // front right tower
  269. float delta_tower2_y= -COS_60*delta_radius;
  270. float delta_tower3_x= 0.0; // back middle tower
  271. float delta_tower3_y= delta_radius;
  272. float delta_diagonal_rod= DELTA_DIAGONAL_ROD;
  273. float delta_diagonal_rod_2= sq(delta_diagonal_rod);
  274. float delta_segments_per_second= DELTA_SEGMENTS_PER_SECOND;
  275. #endif
  276.  
  277. //===========================================================================
  278. //=============================Private Variables=============================
  279. //===========================================================================
  280. const char axis_codes[NUM_AXIS] = {'X', 'Y', 'Z', 'E'};
  281. static float destination[NUM_AXIS] = { 0.0, 0.0, 0.0, 0.0};
  282. static float offset[3] = {0.0, 0.0, 0.0};
  283. static bool home_all_axis = true;
  284. static float feedrate = 1500.0, next_feedrate, saved_feedrate;
  285. static long gcode_N, gcode_LastN, Stopped_gcode_LastN = 0;
  286.  
  287. static bool relative_mode = false; //Determines Absolute or Relative Coordinates
  288.  
  289. static char cmdbuffer[BUFSIZE][MAX_CMD_SIZE];
  290. static bool fromsd[BUFSIZE];
  291. static int bufindr = 0;
  292. static int bufindw = 0;
  293. static int buflen = 0;
  294. //static int i = 0;
  295. static char serial_char;
  296. static int serial_count = 0;
  297. static boolean comment_mode = false;
  298. static char *strchr_pointer; // just a pointer to find chars in the command string like X, Y, Z, E, etc
  299.  
  300. const int sensitive_pins[] = SENSITIVE_PINS; // Sensitive pin list for M42
  301.  
  302. //static float tt = 0;
  303. //static float bt = 0;
  304.  
  305. //Inactivity shutdown variables
  306. static unsigned long previous_millis_cmd = 0;
  307. static unsigned long max_inactive_time = 0;
  308. static unsigned long stepper_inactive_time = DEFAULT_STEPPER_DEACTIVE_TIME*1000l;
  309.  
  310. unsigned long starttime=0;
  311. unsigned long stoptime=0;
  312.  
  313. static uint8_t tmp_extruder;
  314.  
  315.  
  316. bool Stopped=false;
  317.  
  318. #if NUM_SERVOS > 0
  319. Servo servos[NUM_SERVOS];
  320. #endif
  321.  
  322. bool CooldownNoWait = true;
  323. bool target_direction;
  324.  
  325. //Insert variables if CHDK is defined
  326. #ifdef CHDK
  327. unsigned long chdkHigh = 0;
  328. boolean chdkActive = false;
  329. #endif
  330.  
  331. //===========================================================================
  332. //=============================Routines======================================
  333. //===========================================================================
  334.  
  335. void get_arc_coordinates();
  336. bool setTargetedHotend(int code);
  337.  
  338. void serial_echopair_P(const char *s_P, float v)
  339. { serialprintPGM(s_P); SERIAL_ECHO(v); }
  340. void serial_echopair_P(const char *s_P, double v)
  341. { serialprintPGM(s_P); SERIAL_ECHO(v); }
  342. void serial_echopair_P(const char *s_P, unsigned long v)
  343. { serialprintPGM(s_P); SERIAL_ECHO(v); }
  344.  
  345. extern "C"{
  346. extern unsigned int __bss_end;
  347. extern unsigned int __heap_start;
  348. extern void *__brkval;
  349.  
  350. int freeMemory() {
  351. int free_memory;
  352.  
  353. if((int)__brkval == 0)
  354. free_memory = ((int)&free_memory) - ((int)&__bss_end);
  355. else
  356. free_memory = ((int)&free_memory) - ((int)__brkval);
  357.  
  358. return free_memory;
  359. }
  360. }
  361.  
  362. //adds an command to the main command buffer
  363. //thats really done in a non-safe way.
  364. //needs overworking someday
  365. void enquecommand(const char *cmd)
  366. {
  367. if(buflen < BUFSIZE)
  368. {
  369. //this is dangerous if a mixing of serial and this happens
  370. strcpy(&(cmdbuffer[bufindw][0]),cmd);
  371. SERIAL_ECHO_START;
  372. SERIAL_ECHOPGM("enqueing \"");
  373. SERIAL_ECHO(cmdbuffer[bufindw]);
  374. SERIAL_ECHOLNPGM("\"");
  375. bufindw= (bufindw + 1)%BUFSIZE;
  376. buflen += 1;
  377. }
  378. }
  379.  
  380. void enquecommand_P(const char *cmd)
  381. {
  382. if(buflen < BUFSIZE)
  383. {
  384. //this is dangerous if a mixing of serial and this happens
  385. strcpy_P(&(cmdbuffer[bufindw][0]),cmd);
  386. SERIAL_ECHO_START;
  387. SERIAL_ECHOPGM("enqueing \"");
  388. SERIAL_ECHO(cmdbuffer[bufindw]);
  389. SERIAL_ECHOLNPGM("\"");
  390. bufindw= (bufindw + 1)%BUFSIZE;
  391. buflen += 1;
  392. }
  393. }
  394.  
  395. void setup_killpin()
  396. {
  397. #if defined(KILL_PIN) && KILL_PIN > -1
  398. pinMode(KILL_PIN,INPUT);
  399. WRITE(KILL_PIN,HIGH);
  400. #endif
  401. }
  402.  
  403. void setup_photpin()
  404. {
  405. #if defined(PHOTOGRAPH_PIN) && PHOTOGRAPH_PIN > -1
  406. SET_OUTPUT(PHOTOGRAPH_PIN);
  407. WRITE(PHOTOGRAPH_PIN, LOW);
  408. #endif
  409. }
  410.  
  411. void setup_powerhold()
  412. {
  413. #if defined(SUICIDE_PIN) && SUICIDE_PIN > -1
  414. SET_OUTPUT(SUICIDE_PIN);
  415. WRITE(SUICIDE_PIN, HIGH);
  416. #endif
  417. #if defined(PS_ON_PIN) && PS_ON_PIN > -1
  418. SET_OUTPUT(PS_ON_PIN);
  419. #if defined(PS_DEFAULT_OFF)
  420. WRITE(PS_ON_PIN, PS_ON_ASLEEP);
  421. #else
  422. WRITE(PS_ON_PIN, PS_ON_AWAKE);
  423. #endif
  424. #endif
  425. }
  426.  
  427. void suicide()
  428. {
  429. #if defined(SUICIDE_PIN) && SUICIDE_PIN > -1
  430. SET_OUTPUT(SUICIDE_PIN);
  431. WRITE(SUICIDE_PIN, LOW);
  432. #endif
  433. }
  434.  
  435. void servo_init()
  436. {
  437. #if (NUM_SERVOS >= 1) && defined(SERVO0_PIN) && (SERVO0_PIN > -1)
  438. servos[0].attach(SERVO0_PIN);
  439. #endif
  440. #if (NUM_SERVOS >= 2) && defined(SERVO1_PIN) && (SERVO1_PIN > -1)
  441. servos[1].attach(SERVO1_PIN);
  442. #endif
  443. #if (NUM_SERVOS >= 3) && defined(SERVO2_PIN) && (SERVO2_PIN > -1)
  444. servos[2].attach(SERVO2_PIN);
  445. #endif
  446. #if (NUM_SERVOS >= 4) && defined(SERVO3_PIN) && (SERVO3_PIN > -1)
  447. servos[3].attach(SERVO3_PIN);
  448. #endif
  449. #if (NUM_SERVOS >= 5)
  450. #error "TODO: enter initalisation code for more servos"
  451. #endif
  452.  
  453. // Set position of Servo Endstops that are defined
  454. #ifdef SERVO_ENDSTOPS
  455. for(int8_t i = 0; i < 3; i++)
  456. {
  457. if(servo_endstops[i] > -1) {
  458. servos[servo_endstops[i]].write(servo_endstop_angles[i * 2 + 1]);
  459. }
  460. }
  461. #endif
  462.  
  463. #if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
  464. delay(PROBE_SERVO_DEACTIVATION_DELAY);
  465. servos[servo_endstops[Z_AXIS]].detach();
  466. #endif
  467. }
  468.  
  469. void setup()
  470. {
  471. setup_killpin();
  472. setup_powerhold();
  473. MYSERIAL.begin(BAUDRATE);
  474. SERIAL_PROTOCOLLNPGM("start");
  475. SERIAL_ECHO_START;
  476.  
  477. // Check startup - does nothing if bootloader sets MCUSR to 0
  478. byte mcu = MCUSR;
  479. if(mcu & 1) SERIAL_ECHOLNPGM(MSG_POWERUP);
  480. if(mcu & 2) SERIAL_ECHOLNPGM(MSG_EXTERNAL_RESET);
  481. if(mcu & 4) SERIAL_ECHOLNPGM(MSG_BROWNOUT_RESET);
  482. if(mcu & 8) SERIAL_ECHOLNPGM(MSG_WATCHDOG_RESET);
  483. if(mcu & 32) SERIAL_ECHOLNPGM(MSG_SOFTWARE_RESET);
  484. MCUSR=0;
  485.  
  486. SERIAL_ECHOPGM(MSG_MARLIN);
  487. SERIAL_ECHOLNPGM(VERSION_STRING);
  488. #ifdef STRING_VERSION_CONFIG_H
  489. #ifdef STRING_CONFIG_H_AUTHOR
  490. SERIAL_ECHO_START;
  491. SERIAL_ECHOPGM(MSG_CONFIGURATION_VER);
  492. SERIAL_ECHOPGM(STRING_VERSION_CONFIG_H);
  493. SERIAL_ECHOPGM(MSG_AUTHOR);
  494. SERIAL_ECHOLNPGM(STRING_CONFIG_H_AUTHOR);
  495. SERIAL_ECHOPGM("Compiled: ");
  496. SERIAL_ECHOLNPGM(__DATE__);
  497. #endif
  498. #endif
  499. SERIAL_ECHO_START;
  500. SERIAL_ECHOPGM(MSG_FREE_MEMORY);
  501. SERIAL_ECHO(freeMemory());
  502. SERIAL_ECHOPGM(MSG_PLANNER_BUFFER_BYTES);
  503. SERIAL_ECHOLN((int)sizeof(block_t)*BLOCK_BUFFER_SIZE);
  504. for(int8_t i = 0; i < BUFSIZE; i++)
  505. {
  506. fromsd[i] = false;
  507. }
  508.  
  509. // loads data from EEPROM if available else uses defaults (and resets step acceleration rate)
  510. Config_RetrieveSettings();
  511.  
  512. tp_init(); // Initialize temperature loop
  513. plan_init(); // Initialize planner;
  514. watchdog_init();
  515. st_init(); // Initialize stepper, this enables interrupts!
  516. setup_photpin();
  517. servo_init();
  518.  
  519. lcd_init();
  520. _delay_ms(1000); // wait 1sec to display the splash screen
  521.  
  522. #if defined(CONTROLLERFAN_PIN) && CONTROLLERFAN_PIN > -1
  523. SET_OUTPUT(CONTROLLERFAN_PIN); //Set pin used for driver cooling fan
  524. #endif
  525.  
  526. #ifdef DIGIPOT_I2C
  527. digipot_i2c_init();
  528. #endif
  529. }
  530.  
  531.  
  532. void loop()
  533. {
  534. if(buflen < (BUFSIZE-1))
  535. get_command();
  536. #ifdef SDSUPPORT
  537. card.checkautostart(false);
  538. #endif
  539. if(buflen)
  540. {
  541. #ifdef SDSUPPORT
  542. if(card.saving)
  543. {
  544. if(strstr_P(cmdbuffer[bufindr], PSTR("M29")) == NULL)
  545. {
  546. card.write_command(cmdbuffer[bufindr]);
  547. if(card.logging)
  548. {
  549. process_commands();
  550. }
  551. else
  552. {
  553. SERIAL_PROTOCOLLNPGM(MSG_OK);
  554. }
  555. }
  556. else
  557. {
  558. card.closefile();
  559. SERIAL_PROTOCOLLNPGM(MSG_FILE_SAVED);
  560. }
  561. }
  562. else
  563. {
  564. process_commands();
  565. }
  566. #else
  567. process_commands();
  568. #endif //SDSUPPORT
  569. buflen = (buflen-1);
  570. bufindr = (bufindr + 1)%BUFSIZE;
  571. }
  572. //check heater every n milliseconds
  573. manage_heater();
  574. manage_inactivity();
  575. checkHitEndstops();
  576. lcd_update();
  577. }
  578.  
  579. void get_command()
  580. {
  581. while( MYSERIAL.available() > 0 && buflen < BUFSIZE) {
  582. serial_char = MYSERIAL.read();
  583. if(serial_char == '\n' ||
  584. serial_char == '\r' ||
  585. (serial_char == ':' && comment_mode == false) ||
  586. serial_count >= (MAX_CMD_SIZE - 1) )
  587. {
  588. if(!serial_count) { //if empty line
  589. comment_mode = false; //for new command
  590. return;
  591. }
  592. cmdbuffer[bufindw][serial_count] = 0; //terminate string
  593. if(!comment_mode){
  594. comment_mode = false; //for new command
  595. fromsd[bufindw] = false;
  596. if(strchr(cmdbuffer[bufindw], 'N') != NULL)
  597. {
  598. strchr_pointer = strchr(cmdbuffer[bufindw], 'N');
  599. gcode_N = (strtol(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL, 10));
  600. if(gcode_N != gcode_LastN+1 && (strstr_P(cmdbuffer[bufindw], PSTR("M110")) == NULL) ) {
  601. SERIAL_ERROR_START;
  602. SERIAL_ERRORPGM(MSG_ERR_LINE_NO);
  603. SERIAL_ERRORLN(gcode_LastN);
  604. //Serial.println(gcode_N);
  605. FlushSerialRequestResend();
  606. serial_count = 0;
  607. return;
  608. }
  609.  
  610. if(strchr(cmdbuffer[bufindw], '*') != NULL)
  611. {
  612. byte checksum = 0;
  613. byte count = 0;
  614. while(cmdbuffer[bufindw][count] != '*') checksum = checksum^cmdbuffer[bufindw][count++];
  615. strchr_pointer = strchr(cmdbuffer[bufindw], '*');
  616.  
  617. if( (int)(strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)) != checksum) {
  618. SERIAL_ERROR_START;
  619. SERIAL_ERRORPGM(MSG_ERR_CHECKSUM_MISMATCH);
  620. SERIAL_ERRORLN(gcode_LastN);
  621. FlushSerialRequestResend();
  622. serial_count = 0;
  623. return;
  624. }
  625. //if no errors, continue parsing
  626. }
  627. else
  628. {
  629. SERIAL_ERROR_START;
  630. SERIAL_ERRORPGM(MSG_ERR_NO_CHECKSUM);
  631. SERIAL_ERRORLN(gcode_LastN);
  632. FlushSerialRequestResend();
  633. serial_count = 0;
  634. return;
  635. }
  636.  
  637. gcode_LastN = gcode_N;
  638. //if no errors, continue parsing
  639. }
  640. else // if we don't receive 'N' but still see '*'
  641. {
  642. if((strchr(cmdbuffer[bufindw], '*') != NULL))
  643. {
  644. SERIAL_ERROR_START;
  645. SERIAL_ERRORPGM(MSG_ERR_NO_LINENUMBER_WITH_CHECKSUM);
  646. SERIAL_ERRORLN(gcode_LastN);
  647. serial_count = 0;
  648. return;
  649. }
  650. }
  651. if((strchr(cmdbuffer[bufindw], 'G') != NULL)){
  652. strchr_pointer = strchr(cmdbuffer[bufindw], 'G');
  653. switch((int)((strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)))){
  654. case 0:
  655. case 1:
  656. case 2:
  657. case 3:
  658. if(Stopped == false) { // If printer is stopped by an error the G[0-3] codes are ignored.
  659. #ifdef SDSUPPORT
  660. if(card.saving)
  661. break;
  662. #endif //SDSUPPORT
  663. SERIAL_PROTOCOLLNPGM(MSG_OK);
  664. }
  665. else {
  666. SERIAL_ERRORLNPGM(MSG_ERR_STOPPED);
  667. LCD_MESSAGEPGM(MSG_STOPPED);
  668. }
  669. break;
  670. default:
  671. break;
  672. }
  673.  
  674. }
  675. bufindw = (bufindw + 1)%BUFSIZE;
  676. buflen += 1;
  677. }
  678. serial_count = 0; //clear buffer
  679. }
  680. else
  681. {
  682. if(serial_char == ';') comment_mode = true;
  683. if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;
  684. }
  685. }
  686. #ifdef SDSUPPORT
  687. if(!card.sdprinting || serial_count!=0){
  688. return;
  689. }
  690.  
  691. //'#' stops reading from SD to the buffer prematurely, so procedural macro calls are possible
  692. // if it occurs, stop_buffering is triggered and the buffer is ran dry.
  693. // this character _can_ occur in serial com, due to checksums. however, no checksums are used in SD printing
  694.  
  695. static bool stop_buffering=false;
  696. if(buflen==0) stop_buffering=false;
  697.  
  698. while( !card.eof() && buflen < BUFSIZE && !stop_buffering) {
  699. int16_t n=card.get();
  700. serial_char = (char)n;
  701. if(serial_char == '\n' ||
  702. serial_char == '\r' ||
  703. (serial_char == '#' && comment_mode == false) ||
  704. (serial_char == ':' && comment_mode == false) ||
  705. serial_count >= (MAX_CMD_SIZE - 1)||n==-1)
  706. {
  707. if(card.eof()){
  708. SERIAL_PROTOCOLLNPGM(MSG_FILE_PRINTED);
  709. stoptime=millis();
  710. char time[30];
  711. unsigned long t=(stoptime-starttime)/1000;
  712. int hours, minutes;
  713. minutes=(t/60)%60;
  714. hours=t/60/60;
  715. sprintf_P(time, PSTR("%i hours %i minutes"),hours, minutes);
  716. SERIAL_ECHO_START;
  717. SERIAL_ECHOLN(time);
  718. lcd_setstatus(time);
  719. card.printingHasFinished();
  720. card.checkautostart(true);
  721.  
  722. }
  723. if(serial_char=='#')
  724. stop_buffering=true;
  725.  
  726. if(!serial_count)
  727. {
  728. comment_mode = false; //for new command
  729. return; //if empty line
  730. }
  731. cmdbuffer[bufindw][serial_count] = 0; //terminate string
  732. // if(!comment_mode){
  733. fromsd[bufindw] = true;
  734. buflen += 1;
  735. bufindw = (bufindw + 1)%BUFSIZE;
  736. // }
  737. comment_mode = false; //for new command
  738. serial_count = 0; //clear buffer
  739. }
  740. else
  741. {
  742. if(serial_char == ';') comment_mode = true;
  743. if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;
  744. }
  745. }
  746.  
  747. #endif //SDSUPPORT
  748.  
  749. }
  750.  
  751.  
  752. float code_value()
  753. {
  754. return (strtod(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL));
  755. }
  756.  
  757. long code_value_long()
  758. {
  759. return (strtol(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL, 10));
  760. }
  761.  
  762. bool code_seen(char code)
  763. {
  764. strchr_pointer = strchr(cmdbuffer[bufindr], code);
  765. return (strchr_pointer != NULL); //Return True if a character was found
  766. }
  767.  
  768. #define DEFINE_PGM_READ_ANY(type, reader) \
  769. static inline type pgm_read_any(const type *p) \
  770. { return pgm_read_##reader##_near(p); }
  771.  
  772. DEFINE_PGM_READ_ANY(float, float);
  773. DEFINE_PGM_READ_ANY(signed char, byte);
  774.  
  775. #define XYZ_CONSTS_FROM_CONFIG(type, array, CONFIG) \
  776. static const PROGMEM type array##_P[3] = \
  777. { X_##CONFIG, Y_##CONFIG, Z_##CONFIG }; \
  778. static inline type array(int axis) \
  779. { return pgm_read_any(&array##_P[axis]); }
  780.  
  781. XYZ_CONSTS_FROM_CONFIG(float, base_min_pos, MIN_POS);
  782. XYZ_CONSTS_FROM_CONFIG(float, base_max_pos, MAX_POS);
  783. XYZ_CONSTS_FROM_CONFIG(float, base_home_pos, HOME_POS);
  784. XYZ_CONSTS_FROM_CONFIG(float, max_length, MAX_LENGTH);
  785. XYZ_CONSTS_FROM_CONFIG(float, home_retract_mm, HOME_RETRACT_MM);
  786. XYZ_CONSTS_FROM_CONFIG(signed char, home_dir, HOME_DIR);
  787.  
  788. #ifdef DUAL_X_CARRIAGE
  789. #if EXTRUDERS == 1 || defined(COREXY) \
  790. || !defined(X2_ENABLE_PIN) || !defined(X2_STEP_PIN) || !defined(X2_DIR_PIN) \
  791. || !defined(X2_HOME_POS) || !defined(X2_MIN_POS) || !defined(X2_MAX_POS) \
  792. || !defined(X_MAX_PIN) || X_MAX_PIN < 0
  793. #error "Missing or invalid definitions for DUAL_X_CARRIAGE mode."
  794. #endif
  795. #if X_HOME_DIR != -1 || X2_HOME_DIR != 1
  796. #error "Please use canonical x-carriage assignment" // the x-carriages are defined by their homing directions
  797. #endif
  798.  
  799. #define DXC_FULL_CONTROL_MODE 0
  800. #define DXC_AUTO_PARK_MODE 1
  801. #define DXC_DUPLICATION_MODE 2
  802. static int dual_x_carriage_mode = DEFAULT_DUAL_X_CARRIAGE_MODE;
  803.  
  804. static float x_home_pos(int extruder) {
  805. if (extruder == 0)
  806. return base_home_pos(X_AXIS) + add_homeing[X_AXIS];
  807. else
  808. // In dual carriage mode the extruder offset provides an override of the
  809. // second X-carriage offset when homed - otherwise X2_HOME_POS is used.
  810. // This allow soft recalibration of the second extruder offset position without firmware reflash
  811. // (through the M218 command).
  812. return (extruder_offset[X_AXIS][1] > 0) ? extruder_offset[X_AXIS][1] : X2_HOME_POS;
  813. }
  814.  
  815. static int x_home_dir(int extruder) {
  816. return (extruder == 0) ? X_HOME_DIR : X2_HOME_DIR;
  817. }
  818.  
  819. static float inactive_extruder_x_pos = X2_MAX_POS; // used in mode 0 & 1
  820. static bool active_extruder_parked = false; // used in mode 1 & 2
  821. static float raised_parked_position[NUM_AXIS]; // used in mode 1
  822. static unsigned long delayed_move_time = 0; // used in mode 1
  823. static float duplicate_extruder_x_offset = DEFAULT_DUPLICATION_X_OFFSET; // used in mode 2
  824. static float duplicate_extruder_temp_offset = 0; // used in mode 2
  825. bool extruder_duplication_enabled = false; // used in mode 2
  826. #endif //DUAL_X_CARRIAGE
  827.  
  828. static void axis_is_at_home(int axis) {
  829. #ifdef DUAL_X_CARRIAGE
  830. if (axis == X_AXIS) {
  831. if (active_extruder != 0) {
  832. current_position[X_AXIS] = x_home_pos(active_extruder);
  833. min_pos[X_AXIS] = X2_MIN_POS;
  834. max_pos[X_AXIS] = max(extruder_offset[X_AXIS][1], X2_MAX_POS);
  835. return;
  836. }
  837. else if (dual_x_carriage_mode == DXC_DUPLICATION_MODE && active_extruder == 0) {
  838. current_position[X_AXIS] = base_home_pos(X_AXIS) + add_homeing[X_AXIS];
  839. min_pos[X_AXIS] = base_min_pos(X_AXIS) + add_homeing[X_AXIS];
  840. max_pos[X_AXIS] = min(base_max_pos(X_AXIS) + add_homeing[X_AXIS],
  841. max(extruder_offset[X_AXIS][1], X2_MAX_POS) - duplicate_extruder_x_offset);
  842. return;
  843. }
  844. }
  845. #endif
  846. current_position[axis] = base_home_pos(axis) + add_homeing[axis];
  847. min_pos[axis] = base_min_pos(axis) + add_homeing[axis];
  848. max_pos[axis] = base_max_pos(axis) + add_homeing[axis];
  849. }
  850.  
  851. #ifdef ENABLE_AUTO_BED_LEVELING
  852. #ifdef AUTO_BED_LEVELING_GRID
  853. static void set_bed_level_equation_lsq(double *plane_equation_coefficients)
  854. {
  855. vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1);
  856. planeNormal.debug("planeNormal");
  857. plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
  858. //bedLevel.debug("bedLevel");
  859.  
  860. //plan_bed_level_matrix.debug("bed level before");
  861. //vector_3 uncorrected_position = plan_get_position_mm();
  862. //uncorrected_position.debug("position before");
  863.  
  864. vector_3 corrected_position = plan_get_position();
  865. // corrected_position.debug("position after");
  866. current_position[X_AXIS] = corrected_position.x;
  867. current_position[Y_AXIS] = corrected_position.y;
  868. current_position[Z_AXIS] = corrected_position.z;
  869.  
  870. // but the bed at 0 so we don't go below it.
  871. current_position[Z_AXIS] = zprobe_zoffset; // in the lsq we reach here after raising the extruder due to the loop structure
  872.  
  873. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  874. }
  875.  
  876. #else // not AUTO_BED_LEVELING_GRID
  877.  
  878. static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float z_at_pt_3) {
  879.  
  880. plan_bed_level_matrix.set_to_identity();
  881.  
  882. vector_3 pt1 = vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, z_at_pt_1);
  883. vector_3 pt2 = vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, z_at_pt_2);
  884. vector_3 pt3 = vector_3(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, z_at_pt_3);
  885.  
  886. vector_3 from_2_to_1 = (pt1 - pt2).get_normal();
  887. vector_3 from_2_to_3 = (pt3 - pt2).get_normal();
  888. vector_3 planeNormal = vector_3::cross(from_2_to_1, from_2_to_3).get_normal();
  889. planeNormal = vector_3(planeNormal.x, planeNormal.y, abs(planeNormal.z));
  890.  
  891. plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
  892.  
  893. vector_3 corrected_position = plan_get_position();
  894. current_position[X_AXIS] = corrected_position.x;
  895. current_position[Y_AXIS] = corrected_position.y;
  896. current_position[Z_AXIS] = corrected_position.z;
  897.  
  898. // put the bed at 0 so we don't go below it.
  899. current_position[Z_AXIS] = zprobe_zoffset;
  900.  
  901. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  902.  
  903. }
  904.  
  905. #endif // AUTO_BED_LEVELING_GRID
  906.  
  907. static void run_z_probe() {
  908. plan_bed_level_matrix.set_to_identity();
  909. feedrate = homing_feedrate[Z_AXIS];
  910.  
  911. // move down until you find the bed
  912. float zPosition = -10;
  913. plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
  914. st_synchronize();
  915.  
  916. // we have to let the planner know where we are right now as it is not where we said to go.
  917. zPosition = st_get_position_mm(Z_AXIS);
  918. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS]);
  919.  
  920. // move up the retract distance
  921. zPosition += home_retract_mm(Z_AXIS);
  922. plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
  923. st_synchronize();
  924.  
  925. // move back down slowly to find bed
  926. feedrate = homing_feedrate[Z_AXIS]/4;
  927. zPosition -= home_retract_mm(Z_AXIS) * 2;
  928. plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
  929. st_synchronize();
  930.  
  931. current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
  932. // make sure the planner knows where we are as it may be a bit different than we last said to move to
  933. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  934. }
  935.  
  936. static void do_blocking_move_to(float x, float y, float z) {
  937. float oldFeedRate = feedrate;
  938.  
  939. feedrate = XY_TRAVEL_SPEED;
  940.  
  941. current_position[X_AXIS] = x;
  942. current_position[Y_AXIS] = y;
  943. current_position[Z_AXIS] = z;
  944. plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate/60, active_extruder);
  945. st_synchronize();
  946.  
  947. feedrate = oldFeedRate;
  948. }
  949.  
  950. static void do_blocking_move_relative(float offset_x, float offset_y, float offset_z) {
  951. do_blocking_move_to(current_position[X_AXIS] + offset_x, current_position[Y_AXIS] + offset_y, current_position[Z_AXIS] + offset_z);
  952. }
  953.  
  954. static void setup_for_endstop_move() {
  955. saved_feedrate = feedrate;
  956. saved_feedmultiply = feedmultiply;
  957. feedmultiply = 100;
  958. previous_millis_cmd = millis();
  959.  
  960. enable_endstops(true);
  961. }
  962.  
  963. static void clean_up_after_endstop_move() {
  964. #ifdef ENDSTOPS_ONLY_FOR_HOMING
  965. enable_endstops(false);
  966. #endif
  967.  
  968. feedrate = saved_feedrate;
  969. feedmultiply = saved_feedmultiply;
  970. previous_millis_cmd = millis();
  971. }
  972.  
  973. static void engage_z_probe() {
  974. // Engage Z Servo endstop if enabled
  975. #ifdef SERVO_ENDSTOPS
  976. if (servo_endstops[Z_AXIS] > -1) {
  977. #if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
  978. servos[servo_endstops[Z_AXIS]].attach(0);
  979. #endif
  980. servos[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2]);
  981. #if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
  982. delay(PROBE_SERVO_DEACTIVATION_DELAY);
  983. servos[servo_endstops[Z_AXIS]].detach();
  984. #endif
  985. }
  986. #endif
  987. }
  988.  
  989. static void retract_z_probe() {
  990. // Retract Z Servo endstop if enabled
  991. #ifdef SERVO_ENDSTOPS
  992. if (servo_endstops[Z_AXIS] > -1) {
  993. #if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
  994. servos[servo_endstops[Z_AXIS]].attach(0);
  995. #endif
  996. servos[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2 + 1]);
  997. #if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
  998. delay(PROBE_SERVO_DEACTIVATION_DELAY);
  999. servos[servo_endstops[Z_AXIS]].detach();
  1000. #endif
  1001. }
  1002. #endif
  1003. }
  1004.  
  1005. /// Probe bed height at position (x,y), returns the measured z value
  1006. static float probe_pt(float x, float y, float z_before) {
  1007. // move to right place
  1008. do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_before);
  1009. do_blocking_move_to(x - X_PROBE_OFFSET_FROM_EXTRUDER, y - Y_PROBE_OFFSET_FROM_EXTRUDER, current_position[Z_AXIS]);
  1010.  
  1011. // engage_z_probe(); // Engage Z Servo endstop if available
  1012. run_z_probe();
  1013. float measured_z = current_position[Z_AXIS];
  1014. // retract_z_probe();
  1015.  
  1016. SERIAL_PROTOCOLPGM(MSG_BED);
  1017. SERIAL_PROTOCOLPGM(" x: ");
  1018. SERIAL_PROTOCOL(x);
  1019. SERIAL_PROTOCOLPGM(" y: ");
  1020. SERIAL_PROTOCOL(y);
  1021. SERIAL_PROTOCOLPGM(" z: ");
  1022. SERIAL_PROTOCOL(measured_z);
  1023. SERIAL_PROTOCOLPGM("\n");
  1024. return measured_z;
  1025. }
  1026.  
  1027. #endif // #ifdef ENABLE_AUTO_BED_LEVELING
  1028.  
  1029. static void homeaxis(int axis) {
  1030. #define HOMEAXIS_DO(LETTER) \
  1031. ((LETTER##_MIN_PIN > -1 && LETTER##_HOME_DIR==-1) || (LETTER##_MAX_PIN > -1 && LETTER##_HOME_DIR==1))
  1032.  
  1033. if (axis==X_AXIS ? HOMEAXIS_DO(X) :
  1034. axis==Y_AXIS ? HOMEAXIS_DO(Y) :
  1035. axis==Z_AXIS ? HOMEAXIS_DO(Z) :
  1036. 0) {
  1037. int axis_home_dir = home_dir(axis);
  1038. #ifdef DUAL_X_CARRIAGE
  1039. if (axis == X_AXIS)
  1040. axis_home_dir = x_home_dir(active_extruder);
  1041. #endif
  1042.  
  1043. current_position[axis] = 0;
  1044. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  1045.  
  1046.  
  1047. // Engage Servo endstop if enabled
  1048. #ifdef SERVO_ENDSTOPS
  1049. #if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
  1050. if (axis==Z_AXIS) {
  1051. engage_z_probe();
  1052. }
  1053. else
  1054. #endif
  1055. if (servo_endstops[axis] > -1) {
  1056. servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2]);
  1057. }
  1058. #endif
  1059.  
  1060. destination[axis] = 1.5 * max_length(axis) * axis_home_dir;
  1061. feedrate = homing_feedrate[axis];
  1062. plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
  1063. st_synchronize();
  1064.  
  1065. current_position[axis] = 0;
  1066. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  1067. destination[axis] = -home_retract_mm(axis) * axis_home_dir;
  1068. plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
  1069. st_synchronize();
  1070.  
  1071. destination[axis] = 2*home_retract_mm(axis) * axis_home_dir;
  1072. #ifdef DELTA
  1073. feedrate = homing_feedrate[axis]/10;
  1074. #else
  1075. feedrate = homing_feedrate[axis]/2 ;
  1076. #endif
  1077. plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
  1078. st_synchronize();
  1079. #ifdef DELTA
  1080. // retrace by the amount specified in endstop_adj
  1081. if (endstop_adj[axis] * axis_home_dir < 0) {
  1082. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  1083. destination[axis] = endstop_adj[axis];
  1084. plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
  1085. st_synchronize();
  1086. }
  1087. #endif
  1088. axis_is_at_home(axis);
  1089. destination[axis] = current_position[axis];
  1090. feedrate = 0.0;
  1091. endstops_hit_on_purpose();
  1092. axis_known_position[axis] = true;
  1093.  
  1094. // Retract Servo endstop if enabled
  1095. #ifdef SERVO_ENDSTOPS
  1096. if (servo_endstops[axis] > -1) {
  1097. servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2 + 1]);
  1098. }
  1099. #endif
  1100. #if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
  1101.  
  1102. if (axis==Z_AXIS)
  1103. {
  1104. do_blocking_move_to(0,0, Z_RAISE_BEFORE_PROBING);
  1105. retract_z_probe();
  1106. }
  1107. #endif
  1108.  
  1109. }
  1110. }
  1111. #define HOMEAXIS(LETTER) homeaxis(LETTER##_AXIS)
  1112. void refresh_cmd_timeout(void)
  1113. {
  1114. previous_millis_cmd = millis();
  1115. }
  1116.  
  1117. #ifdef FWRETRACT
  1118. void retract(bool retracting) {
  1119. if(retracting && !retracted) {
  1120. destination[X_AXIS]=current_position[X_AXIS];
  1121. destination[Y_AXIS]=current_position[Y_AXIS];
  1122. destination[Z_AXIS]=current_position[Z_AXIS];
  1123. destination[E_AXIS]=current_position[E_AXIS];
  1124. current_position[E_AXIS]+=retract_length/volumetric_multiplier[active_extruder];
  1125. plan_set_e_position(current_position[E_AXIS]);
  1126. float oldFeedrate = feedrate;
  1127. feedrate=retract_feedrate;
  1128. retracted=true;
  1129. prepare_move();
  1130. current_position[Z_AXIS]-=retract_zlift;
  1131. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  1132. prepare_move();
  1133. feedrate = oldFeedrate;
  1134. } else if(!retracting && retracted) {
  1135. destination[X_AXIS]=current_position[X_AXIS];
  1136. destination[Y_AXIS]=current_position[Y_AXIS];
  1137. destination[Z_AXIS]=current_position[Z_AXIS];
  1138. destination[E_AXIS]=current_position[E_AXIS];
  1139. current_position[Z_AXIS]+=retract_zlift;
  1140. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  1141. //prepare_move();
  1142. current_position[E_AXIS]-=(retract_length+retract_recover_length)/volumetric_multiplier[active_extruder];
  1143. plan_set_e_position(current_position[E_AXIS]);
  1144. float oldFeedrate = feedrate;
  1145. feedrate=retract_recover_feedrate;
  1146. retracted=false;
  1147. prepare_move();
  1148. feedrate = oldFeedrate;
  1149. }
  1150. } //retract
  1151. #endif //FWRETRACT
  1152.  
  1153. void process_commands()
  1154. {
  1155. unsigned long codenum; //throw away variable
  1156. char *starpos = NULL;
  1157. #ifdef ENABLE_AUTO_BED_LEVELING
  1158. float x_tmp, y_tmp, z_tmp, real_z;
  1159. #endif
  1160. if(code_seen('G'))
  1161. {
  1162. switch((int)code_value())
  1163. {
  1164. case 0: // G0 -> G1
  1165. case 1: // G1
  1166. if(Stopped == false) {
  1167. get_coordinates(); // For X Y Z E F
  1168. #ifdef FWRETRACT
  1169. if(autoretract_enabled)
  1170. if( !(code_seen('X') || code_seen('Y') || code_seen('Z')) && code_seen('E')) {
  1171. float echange=destination[E_AXIS]-current_position[E_AXIS];
  1172. if((echange<-MIN_RETRACT && !retracted) || (echange>MIN_RETRACT && retracted)) { //move appears to be an attempt to retract or recover
  1173. current_position[E_AXIS] = destination[E_AXIS]; //hide the slicer-generated retract/recover from calculations
  1174. plan_set_e_position(current_position[E_AXIS]); //AND from the planner
  1175. retract(!retracted);
  1176. return;
  1177. }
  1178. }
  1179. #endif //FWRETRACT
  1180. prepare_move();
  1181. //ClearToSend();
  1182. return;
  1183. }
  1184. //break;
  1185. case 2: // G2 - CW ARC
  1186. if(Stopped == false) {
  1187. get_arc_coordinates();
  1188. prepare_arc_move(true);
  1189. return;
  1190. }
  1191. case 3: // G3 - CCW ARC
  1192. if(Stopped == false) {
  1193. get_arc_coordinates();
  1194. prepare_arc_move(false);
  1195. return;
  1196. }
  1197. case 4: // G4 dwell
  1198. LCD_MESSAGEPGM(MSG_DWELL);
  1199. codenum = 0;
  1200. if(code_seen('P')) codenum = code_value(); // milliseconds to wait
  1201. if(code_seen('S')) codenum = code_value() * 1000; // seconds to wait
  1202.  
  1203. st_synchronize();
  1204. codenum += millis(); // keep track of when we started waiting
  1205. previous_millis_cmd = millis();
  1206. while(millis() < codenum ){
  1207. manage_heater();
  1208. manage_inactivity();
  1209. lcd_update();
  1210. }
  1211. break;
  1212. #ifdef FWRETRACT
  1213. case 10: // G10 retract
  1214. retract(true);
  1215. break;
  1216. case 11: // G11 retract_recover
  1217. retract(false);
  1218. break;
  1219. #endif //FWRETRACT
  1220. case 28: //G28 Home all Axis one at a time
  1221. #ifdef ENABLE_AUTO_BED_LEVELING
  1222. plan_bed_level_matrix.set_to_identity(); //Reset the plane ("erase" all leveling data)
  1223. #endif //ENABLE_AUTO_BED_LEVELING
  1224.  
  1225.  
  1226. saved_feedrate = feedrate;
  1227. saved_feedmultiply = feedmultiply;
  1228. feedmultiply = 100;
  1229. previous_millis_cmd = millis();
  1230.  
  1231. enable_endstops(true);
  1232.  
  1233. for(int8_t i=0; i < NUM_AXIS; i++) {
  1234. destination[i] = current_position[i];
  1235. }
  1236. feedrate = 0.0;
  1237.  
  1238. #ifdef DELTA
  1239. // A delta can only safely home all axis at the same time
  1240. // all axis have to home at the same time
  1241.  
  1242. // Move all carriages up together until the first endstop is hit.
  1243. current_position[X_AXIS] = 0;
  1244. current_position[Y_AXIS] = 0;
  1245. current_position[Z_AXIS] = 0;
  1246. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  1247.  
  1248. destination[X_AXIS] = 3 * Z_MAX_LENGTH;
  1249. destination[Y_AXIS] = 3 * Z_MAX_LENGTH;
  1250. destination[Z_AXIS] = 3 * Z_MAX_LENGTH;
  1251. feedrate = 1.732 * homing_feedrate[X_AXIS];
  1252. plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
  1253. st_synchronize();
  1254. endstops_hit_on_purpose();
  1255.  
  1256. current_position[X_AXIS] = destination[X_AXIS];
  1257. current_position[Y_AXIS] = destination[Y_AXIS];
  1258. current_position[Z_AXIS] = destination[Z_AXIS];
  1259.  
  1260. // take care of back off and rehome now we are all at the top
  1261. HOMEAXIS(X);
  1262. HOMEAXIS(Y);
  1263. HOMEAXIS(Z);
  1264.  
  1265. calculate_delta(current_position);
  1266. plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
  1267.  
  1268. #else // NOT DELTA
  1269.  
  1270. home_all_axis = !((code_seen(axis_codes[X_AXIS])) || (code_seen(axis_codes[Y_AXIS])) || (code_seen(axis_codes[Z_AXIS])));
  1271.  
  1272. #if Z_HOME_DIR > 0 // If homing away from BED do Z first
  1273. if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) {
  1274. HOMEAXIS(Z);
  1275. }
  1276. #endif
  1277.  
  1278. #ifdef QUICK_HOME
  1279. if((home_all_axis)||( code_seen(axis_codes[X_AXIS]) && code_seen(axis_codes[Y_AXIS])) ) //first diagonal move
  1280. {
  1281. current_position[X_AXIS] = 0;current_position[Y_AXIS] = 0;
  1282.  
  1283. #ifndef DUAL_X_CARRIAGE
  1284. int x_axis_home_dir = home_dir(X_AXIS);
  1285. #else
  1286. int x_axis_home_dir = x_home_dir(active_extruder);
  1287. extruder_duplication_enabled = false;
  1288. #endif
  1289.  
  1290. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  1291. destination[X_AXIS] = 1.5 * max_length(X_AXIS) * x_axis_home_dir;destination[Y_AXIS] = 1.5 * max_length(Y_AXIS) * home_dir(Y_AXIS);
  1292. feedrate = homing_feedrate[X_AXIS];
  1293. if(homing_feedrate[Y_AXIS]<feedrate)
  1294. feedrate =homing_feedrate[Y_AXIS];
  1295. plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
  1296. st_synchronize();
  1297.  
  1298. axis_is_at_home(X_AXIS);
  1299. axis_is_at_home(Y_AXIS);
  1300. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  1301. destination[X_AXIS] = current_position[X_AXIS];
  1302. destination[Y_AXIS] = current_position[Y_AXIS];
  1303. plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
  1304. feedrate = 0.0;
  1305. st_synchronize();
  1306. endstops_hit_on_purpose();
  1307.  
  1308. current_position[X_AXIS] = destination[X_AXIS];
  1309. current_position[Y_AXIS] = destination[Y_AXIS];
  1310. current_position[Z_AXIS] = destination[Z_AXIS];
  1311. }
  1312. #endif
  1313.  
  1314. if((home_all_axis) || (code_seen(axis_codes[X_AXIS])))
  1315. {
  1316. #ifdef DUAL_X_CARRIAGE
  1317. int tmp_extruder = active_extruder;
  1318. extruder_duplication_enabled = false;
  1319. active_extruder = !active_extruder;
  1320. HOMEAXIS(X);
  1321. inactive_extruder_x_pos = current_position[X_AXIS];
  1322. active_extruder = tmp_extruder;
  1323. HOMEAXIS(X);
  1324. // reset state used by the different modes
  1325. memcpy(raised_parked_position, current_position, sizeof(raised_parked_position));
  1326. delayed_move_time = 0;
  1327. active_extruder_parked = true;
  1328. #else
  1329. HOMEAXIS(X);
  1330. #endif
  1331. }
  1332.  
  1333. if((home_all_axis) || (code_seen(axis_codes[Y_AXIS]))) {
  1334. HOMEAXIS(Y);
  1335. }
  1336.  
  1337. if(code_seen(axis_codes[X_AXIS]))
  1338. {
  1339. if(code_value_long() != 0) {
  1340. current_position[X_AXIS]=code_value()+add_homeing[0];
  1341. }
  1342. }
  1343.  
  1344. if(code_seen(axis_codes[Y_AXIS])) {
  1345. if(code_value_long() != 0) {
  1346. current_position[Y_AXIS]=code_value()+add_homeing[1];
  1347. }
  1348. }
  1349.  
  1350. #if Z_HOME_DIR < 0 // If homing towards BED do Z last
  1351. #ifndef Z_SAFE_HOMING
  1352. if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) {
  1353. #if defined (Z_RAISE_BEFORE_HOMING) && (Z_RAISE_BEFORE_HOMING > 0)
  1354. destination[Z_AXIS] = Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS) * (-1); // Set destination away from bed
  1355. feedrate = max_feedrate[Z_AXIS];
  1356. plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
  1357. st_synchronize();
  1358. #endif
  1359. HOMEAXIS(Z);
  1360. }
  1361. #else // Z Safe mode activated.
  1362. if(home_all_axis) {
  1363. destination[X_AXIS] = round(Z_SAFE_HOMING_X_POINT - X_PROBE_OFFSET_FROM_EXTRUDER);
  1364. destination[Y_AXIS] = round(Z_SAFE_HOMING_Y_POINT - Y_PROBE_OFFSET_FROM_EXTRUDER);
  1365. destination[Z_AXIS] = Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS) * (-1); // Set destination away from bed
  1366. feedrate = XY_TRAVEL_SPEED;
  1367. current_position[Z_AXIS] = 0;
  1368.  
  1369. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  1370. plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
  1371. st_synchronize();
  1372. current_position[X_AXIS] = destination[X_AXIS];
  1373. current_position[Y_AXIS] = destination[Y_AXIS];
  1374.  
  1375. HOMEAXIS(Z);
  1376. }
  1377. // Let's see if X and Y are homed and probe is inside bed area.
  1378. if(code_seen(axis_codes[Z_AXIS])) {
  1379. if ( (axis_known_position[X_AXIS]) && (axis_known_position[Y_AXIS]) \
  1380. && (current_position[X_AXIS]+X_PROBE_OFFSET_FROM_EXTRUDER >= X_MIN_POS) \
  1381. && (current_position[X_AXIS]+X_PROBE_OFFSET_FROM_EXTRUDER <= X_MAX_POS) \
  1382. && (current_position[Y_AXIS]+Y_PROBE_OFFSET_FROM_EXTRUDER >= Y_MIN_POS) \
  1383. && (current_position[Y_AXIS]+Y_PROBE_OFFSET_FROM_EXTRUDER <= Y_MAX_POS)) {
  1384.  
  1385. current_position[Z_AXIS] = 0;
  1386. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  1387. destination[Z_AXIS] = Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS) * (-1); // Set destination away from bed
  1388. feedrate = max_feedrate[Z_AXIS];
  1389. plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
  1390. st_synchronize();
  1391.  
  1392. HOMEAXIS(Z);
  1393. } else if (!((axis_known_position[X_AXIS]) && (axis_known_position[Y_AXIS]))) {
  1394. LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
  1395. SERIAL_ECHO_START;
  1396. SERIAL_ECHOLNPGM(MSG_POSITION_UNKNOWN);
  1397. } else {
  1398. LCD_MESSAGEPGM(MSG_ZPROBE_OUT);
  1399. SERIAL_ECHO_START;
  1400. SERIAL_ECHOLNPGM(MSG_ZPROBE_OUT);
  1401. }
  1402. }
  1403. #endif
  1404. #endif
  1405.  
  1406.  
  1407.  
  1408. if(code_seen(axis_codes[Z_AXIS])) {
  1409. if(code_value_long() != 0) {
  1410. current_position[Z_AXIS]=code_value()+add_homeing[2];
  1411. }
  1412. }
  1413. #ifdef ENABLE_AUTO_BED_LEVELING
  1414. if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) {
  1415. current_position[Z_AXIS] += zprobe_zoffset; //Add Z_Probe offset (the distance is negative)
  1416. }
  1417. #endif
  1418. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  1419. #endif // else DELTA
  1420.  
  1421. #ifdef ENDSTOPS_ONLY_FOR_HOMING
  1422. enable_endstops(false);
  1423. #endif
  1424.  
  1425. feedrate = saved_feedrate;
  1426. feedmultiply = saved_feedmultiply;
  1427. previous_millis_cmd = millis();
  1428. endstops_hit_on_purpose();
  1429. break;
  1430.  
  1431. #ifdef ENABLE_AUTO_BED_LEVELING
  1432. case 29: // G29 Detailed Z-Probe, probes the bed at 3 or more points.
  1433. {
  1434. #if Z_MIN_PIN == -1
  1435. #error "You must have a Z_MIN endstop in order to enable Auto Bed Leveling feature!!! Z_MIN_PIN must point to a valid hardware pin."
  1436. #endif
  1437.  
  1438. // Prevent user from running a G29 without first homing in X and Y
  1439. if (! (axis_known_position[X_AXIS] && axis_known_position[Y_AXIS]) )
  1440. {
  1441. LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
  1442. SERIAL_ECHO_START;
  1443. SERIAL_ECHOLNPGM(MSG_POSITION_UNKNOWN);
  1444. break; // abort G29, since we don't know where we are
  1445. }
  1446.  
  1447. st_synchronize();
  1448. // make sure the bed_level_rotation_matrix is identity or the planner will get it incorectly
  1449. //vector_3 corrected_position = plan_get_position_mm();
  1450. //corrected_position.debug("position before G29");
  1451. plan_bed_level_matrix.set_to_identity();
  1452. vector_3 uncorrected_position = plan_get_position();
  1453. //uncorrected_position.debug("position durring G29");
  1454. current_position[X_AXIS] = uncorrected_position.x;
  1455. current_position[Y_AXIS] = uncorrected_position.y;
  1456. current_position[Z_AXIS] = uncorrected_position.z;
  1457. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  1458. setup_for_endstop_move();
  1459.  
  1460. feedrate = homing_feedrate[Z_AXIS];
  1461. #ifdef AUTO_BED_LEVELING_GRID
  1462. // probe at the points of a lattice grid
  1463.  
  1464. int xGridSpacing = (RIGHT_PROBE_BED_POSITION - LEFT_PROBE_BED_POSITION) / (AUTO_BED_LEVELING_GRID_POINTS-1);
  1465. int yGridSpacing = (BACK_PROBE_BED_POSITION - FRONT_PROBE_BED_POSITION) / (AUTO_BED_LEVELING_GRID_POINTS-1);
  1466.  
  1467.  
  1468. // solve the plane equation ax + by + d = z
  1469. // A is the matrix with rows [x y 1] for all the probed points
  1470. // B is the vector of the Z positions
  1471. // the normal vector to the plane is formed by the coefficients of the plane equation in the standard form, which is Vx*x+Vy*y+Vz*z+d = 0
  1472. // so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z
  1473.  
  1474. // "A" matrix of the linear system of equations
  1475. double eqnAMatrix[AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS*3];
  1476. // "B" vector of Z points
  1477. double eqnBVector[AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS];
  1478.  
  1479.  
  1480. int probePointCounter = 0;
  1481. bool zig = true;
  1482. engage_z_probe(); // Engage Z Servo endstop if available
  1483.  
  1484. for (int yProbe=FRONT_PROBE_BED_POSITION; yProbe <= BACK_PROBE_BED_POSITION; yProbe += yGridSpacing)
  1485. {
  1486. int xProbe, xInc;
  1487. if (zig)
  1488. {
  1489. xProbe = LEFT_PROBE_BED_POSITION;
  1490. //xEnd = RIGHT_PROBE_BED_POSITION;
  1491. xInc = xGridSpacing;
  1492. zig = false;
  1493. } else // zag
  1494. {
  1495. xProbe = RIGHT_PROBE_BED_POSITION;
  1496. //xEnd = LEFT_PROBE_BED_POSITION;
  1497. xInc = -xGridSpacing;
  1498. zig = true;
  1499. }
  1500.  
  1501. for (int xCount=0; xCount < AUTO_BED_LEVELING_GRID_POINTS; xCount++)
  1502. {
  1503. float z_before;
  1504. if (probePointCounter == 0)
  1505. {
  1506. // raise before probing
  1507. z_before = Z_RAISE_BEFORE_PROBING;
  1508. } else
  1509. {
  1510. // raise extruder
  1511. z_before = current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS;
  1512. }
  1513.  
  1514. float measured_z = probe_pt(xProbe, yProbe, z_before);
  1515.  
  1516. eqnBVector[probePointCounter] = measured_z;
  1517.  
  1518. eqnAMatrix[probePointCounter + 0*AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS] = xProbe;
  1519. eqnAMatrix[probePointCounter + 1*AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS] = yProbe;
  1520. eqnAMatrix[probePointCounter + 2*AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS] = 1;
  1521. probePointCounter++;
  1522. xProbe += xInc;
  1523. }
  1524. }
  1525. do_blocking_move_relative(0, 0, Z_RAISE_BEFORE_PROBING);
  1526. retract_z_probe();
  1527. do_blocking_move_relative(0, 0, -Z_RAISE_BEFORE_PROBING);
  1528. clean_up_after_endstop_move();
  1529.  
  1530.  
  1531. // solve lsq problem
  1532. double *plane_equation_coefficients = qr_solve(AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS, 3, eqnAMatrix, eqnBVector);
  1533.  
  1534. SERIAL_PROTOCOLPGM("Eqn coefficients: a: ");
  1535. SERIAL_PROTOCOL(plane_equation_coefficients[0]);
  1536. SERIAL_PROTOCOLPGM(" b: ");
  1537. SERIAL_PROTOCOL(plane_equation_coefficients[1]);
  1538. SERIAL_PROTOCOLPGM(" d: ");
  1539. SERIAL_PROTOCOLLN(plane_equation_coefficients[2]);
  1540.  
  1541.  
  1542. set_bed_level_equation_lsq(plane_equation_coefficients);
  1543.  
  1544. free(plane_equation_coefficients);
  1545.  
  1546. #else // AUTO_BED_LEVELING_GRID not defined
  1547.  
  1548. // Probe at 3 arbitrary points
  1549. // probe 1
  1550. float z_at_pt_1 = probe_pt(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, Z_RAISE_BEFORE_PROBING);
  1551.  
  1552. // probe 2
  1553. float z_at_pt_2 = probe_pt(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
  1554.  
  1555. // probe 3
  1556. float z_at_pt_3 = probe_pt(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
  1557.  
  1558. clean_up_after_endstop_move();
  1559.  
  1560. set_bed_level_equation_3pts(z_at_pt_1, z_at_pt_2, z_at_pt_3);
  1561.  
  1562.  
  1563. #endif // AUTO_BED_LEVELING_GRID
  1564. st_synchronize();
  1565.  
  1566. // The following code correct the Z height difference from z-probe position and hotend tip position.
  1567. // The Z height on homing is measured by Z-Probe, but the probe is quite far from the hotend.
  1568. // When the bed is uneven, this height must be corrected.
  1569. real_z = float(st_get_position(Z_AXIS))/axis_steps_per_unit[Z_AXIS]; //get the real Z (since the auto bed leveling is already correcting the plane)
  1570. x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER;
  1571. y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER;
  1572. z_tmp = current_position[Z_AXIS];
  1573.  
  1574. apply_rotation_xyz(plan_bed_level_matrix, x_tmp, y_tmp, z_tmp); //Apply the correction sending the probe offset
  1575. current_position[Z_AXIS] = z_tmp - real_z + current_position[Z_AXIS]; //The difference is added to current position and sent to planner.
  1576. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  1577. }
  1578. break;
  1579.  
  1580. case 30: // G30 Single Z Probe
  1581. {
  1582. engage_z_probe(); // Engage Z Servo endstop if available
  1583.  
  1584. st_synchronize();
  1585. // TODO: make sure the bed_level_rotation_matrix is identity or the planner will get set incorectly
  1586. setup_for_endstop_move();
  1587.  
  1588. feedrate = homing_feedrate[Z_AXIS];
  1589.  
  1590. run_z_probe();
  1591. SERIAL_PROTOCOLPGM(MSG_BED);
  1592. SERIAL_PROTOCOLPGM(" X: ");
  1593. SERIAL_PROTOCOL(current_position[X_AXIS]);
  1594. SERIAL_PROTOCOLPGM(" Y: ");
  1595. SERIAL_PROTOCOL(current_position[Y_AXIS]);
  1596. SERIAL_PROTOCOLPGM(" Z: ");
  1597. SERIAL_PROTOCOL(current_position[Z_AXIS]);
  1598. SERIAL_PROTOCOLPGM("\n");
  1599.  
  1600. clean_up_after_endstop_move();
  1601.  
  1602. retract_z_probe(); // Retract Z Servo endstop if available
  1603. }
  1604. break;
  1605. #endif // ENABLE_AUTO_BED_LEVELING
  1606. case 90: // G90
  1607. relative_mode = false;
  1608. break;
  1609. case 91: // G91
  1610. relative_mode = true;
  1611. break;
  1612. case 92: // G92
  1613. if(!code_seen(axis_codes[E_AXIS]))
  1614. st_synchronize();
  1615. for(int8_t i=0; i < NUM_AXIS; i++) {
  1616. if(code_seen(axis_codes[i])) {
  1617. if(i == E_AXIS) {
  1618. current_position[i] = code_value();
  1619. plan_set_e_position(current_position[E_AXIS]);
  1620. }
  1621. else {
  1622. current_position[i] = code_value()+add_homeing[i];
  1623. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  1624. }
  1625. }
  1626. }
  1627. break;
  1628. }
  1629. }
  1630.  
  1631. else if(code_seen('M'))
  1632. {
  1633. switch( (int)code_value() )
  1634. {
  1635. #ifdef ULTIPANEL
  1636. case 0: // M0 - Unconditional stop - Wait for user button press on LCD
  1637. case 1: // M1 - Conditional stop - Wait for user button press on LCD
  1638. {
  1639. LCD_MESSAGEPGM(MSG_USERWAIT);
  1640. codenum = 0;
  1641. if(code_seen('P')) codenum = code_value(); // milliseconds to wait
  1642. if(code_seen('S')) codenum = code_value() * 1000; // seconds to wait
  1643.  
  1644. st_synchronize();
  1645. previous_millis_cmd = millis();
  1646. if (codenum > 0){
  1647. codenum += millis(); // keep track of when we started waiting
  1648. while(millis() < codenum && !lcd_clicked()){
  1649. manage_heater();
  1650. manage_inactivity();
  1651. lcd_update();
  1652. }
  1653. }else{
  1654. while(!lcd_clicked()){
  1655. manage_heater();
  1656. manage_inactivity();
  1657. lcd_update();
  1658. }
  1659. }
  1660. LCD_MESSAGEPGM(MSG_RESUMING);
  1661. }
  1662. break;
  1663. #endif
  1664. case 17:
  1665. LCD_MESSAGEPGM(MSG_NO_MOVE);
  1666. enable_x();
  1667. enable_y();
  1668. enable_z();
  1669. enable_e0();
  1670. enable_e1();
  1671. enable_e2();
  1672. break;
  1673.  
  1674. #ifdef SDSUPPORT
  1675. case 20: // M20 - list SD card
  1676. SERIAL_PROTOCOLLNPGM(MSG_BEGIN_FILE_LIST);
  1677. card.ls();
  1678. SERIAL_PROTOCOLLNPGM(MSG_END_FILE_LIST);
  1679. break;
  1680. case 21: // M21 - init SD card
  1681.  
  1682. card.initsd();
  1683.  
  1684. break;
  1685. case 22: //M22 - release SD card
  1686. card.release();
  1687.  
  1688. break;
  1689. case 23: //M23 - Select file
  1690. starpos = (strchr(strchr_pointer + 4,'*'));
  1691. if(starpos!=NULL)
  1692. *(starpos-1)='\0';
  1693. card.openFile(strchr_pointer + 4,true);
  1694. break;
  1695. case 24: //M24 - Start SD print
  1696. card.startFileprint();
  1697. starttime=millis();
  1698. break;
  1699. case 25: //M25 - Pause SD print
  1700. card.pauseSDPrint();
  1701. break;
  1702. case 26: //M26 - Set SD index
  1703. if(card.cardOK && code_seen('S')) {
  1704. card.setIndex(code_value_long());
  1705. }
  1706. break;
  1707. case 27: //M27 - Get SD status
  1708. card.getStatus();
  1709. break;
  1710. case 28: //M28 - Start SD write
  1711. starpos = (strchr(strchr_pointer + 4,'*'));
  1712. if(starpos != NULL){
  1713. char* npos = strchr(cmdbuffer[bufindr], 'N');
  1714. strchr_pointer = strchr(npos,' ') + 1;
  1715. *(starpos-1) = '\0';
  1716. }
  1717. card.openFile(strchr_pointer+4,false);
  1718. break;
  1719. case 29: //M29 - Stop SD write
  1720. //processed in write to file routine above
  1721. //card,saving = false;
  1722. break;
  1723. case 30: //M30 <filename> Delete File
  1724. if (card.cardOK){
  1725. card.closefile();
  1726. starpos = (strchr(strchr_pointer + 4,'*'));
  1727. if(starpos != NULL){
  1728. char* npos = strchr(cmdbuffer[bufindr], 'N');
  1729. strchr_pointer = strchr(npos,' ') + 1;
  1730. *(starpos-1) = '\0';
  1731. }
  1732. card.removeFile(strchr_pointer + 4);
  1733. }
  1734. break;
  1735. case 32: //M32 - Select file and start SD print
  1736. {
  1737. if(card.sdprinting) {
  1738. st_synchronize();
  1739.  
  1740. }
  1741. starpos = (strchr(strchr_pointer + 4,'*'));
  1742.  
  1743. char* namestartpos = (strchr(strchr_pointer + 4,'!')); //find ! to indicate filename string start.
  1744. if(namestartpos==NULL)
  1745. {
  1746. namestartpos=strchr_pointer + 4; //default name position, 4 letters after the M
  1747. }
  1748. else
  1749. namestartpos++; //to skip the '!'
  1750.  
  1751. if(starpos!=NULL)
  1752. *(starpos-1)='\0';
  1753.  
  1754. bool call_procedure=(code_seen('P'));
  1755.  
  1756. if(strchr_pointer>namestartpos)
  1757. call_procedure=false; //false alert, 'P' found within filename
  1758.  
  1759. if( card.cardOK )
  1760. {
  1761. card.openFile(namestartpos,true,!call_procedure);
  1762. if(code_seen('S'))
  1763. if(strchr_pointer<namestartpos) //only if "S" is occuring _before_ the filename
  1764. card.setIndex(code_value_long());
  1765. card.startFileprint();
  1766. if(!call_procedure)
  1767. starttime=millis(); //procedure calls count as normal print time.
  1768. }
  1769. } break;
  1770. case 928: //M928 - Start SD write
  1771. starpos = (strchr(strchr_pointer + 5,'*'));
  1772. if(starpos != NULL){
  1773. char* npos = strchr(cmdbuffer[bufindr], 'N');
  1774. strchr_pointer = strchr(npos,' ') + 1;
  1775. *(starpos-1) = '\0';
  1776. }
  1777. card.openLogFile(strchr_pointer+5);
  1778. break;
  1779.  
  1780. #endif //SDSUPPORT
  1781.  
  1782. case 31: //M31 take time since the start of the SD print or an M109 command
  1783. {
  1784. stoptime=millis();
  1785. char time[30];
  1786. unsigned long t=(stoptime-starttime)/1000;
  1787. int sec,min;
  1788. min=t/60;
  1789. sec=t%60;
  1790. sprintf_P(time, PSTR("%i min, %i sec"), min, sec);
  1791. SERIAL_ECHO_START;
  1792. SERIAL_ECHOLN(time);
  1793. lcd_setstatus(time);
  1794. autotempShutdown();
  1795. }
  1796. break;
  1797. case 42: //M42 -Change pin status via gcode
  1798. if (code_seen('S'))
  1799. {
  1800. int pin_status = code_value();
  1801. int pin_number = LED_PIN;
  1802. if (code_seen('P') && pin_status >= 0 && pin_status <= 255)
  1803. pin_number = code_value();
  1804. for(int8_t i = 0; i < (int8_t)sizeof(sensitive_pins); i++)
  1805. {
  1806. if (sensitive_pins[i] == pin_number)
  1807. {
  1808. pin_number = -1;
  1809. break;
  1810. }
  1811. }
  1812. #if defined(FAN_PIN) && FAN_PIN > -1
  1813. if (pin_number == FAN_PIN)
  1814. fanSpeed = pin_status;
  1815. #endif
  1816. if (pin_number > -1)
  1817. {
  1818. pinMode(pin_number, OUTPUT);
  1819. digitalWrite(pin_number, pin_status);
  1820. analogWrite(pin_number, pin_status);
  1821. }
  1822. }
  1823. break;
  1824. case 104: // M104
  1825. if(setTargetedHotend(104)){
  1826. break;
  1827. }
  1828. if (code_seen('S')) setTargetHotend(code_value(), tmp_extruder);
  1829. #ifdef DUAL_X_CARRIAGE
  1830. if (dual_x_carriage_mode == DXC_DUPLICATION_MODE && tmp_extruder == 0)
  1831. setTargetHotend1(code_value() == 0.0 ? 0.0 : code_value() + duplicate_extruder_temp_offset);
  1832. #endif
  1833. setWatch();
  1834. break;
  1835. case 140: // M140 set bed temp
  1836. if (code_seen('S')) setTargetBed(code_value());
  1837. break;
  1838. case 105 : // M105
  1839. if(setTargetedHotend(105)){
  1840. break;
  1841. }
  1842. #if defined(TEMP_0_PIN) && TEMP_0_PIN > -1
  1843. SERIAL_PROTOCOLPGM("ok T:");
  1844. SERIAL_PROTOCOL_F(degHotend(tmp_extruder),1);
  1845. SERIAL_PROTOCOLPGM(" /");
  1846. SERIAL_PROTOCOL_F(degTargetHotend(tmp_extruder),1);
  1847. #if defined(TEMP_BED_PIN) && TEMP_BED_PIN > -1
  1848. SERIAL_PROTOCOLPGM(" B:");
  1849. SERIAL_PROTOCOL_F(degBed(),1);
  1850. SERIAL_PROTOCOLPGM(" /");
  1851. SERIAL_PROTOCOL_F(degTargetBed(),1);
  1852. #endif //TEMP_BED_PIN
  1853. for (int8_t cur_extruder = 0; cur_extruder < EXTRUDERS; ++cur_extruder) {
  1854. SERIAL_PROTOCOLPGM(" T");
  1855. SERIAL_PROTOCOL(cur_extruder);
  1856. SERIAL_PROTOCOLPGM(":");
  1857. SERIAL_PROTOCOL_F(degHotend(cur_extruder),1);
  1858. SERIAL_PROTOCOLPGM(" /");
  1859. SERIAL_PROTOCOL_F(degTargetHotend(cur_extruder),1);
  1860. }
  1861. #else
  1862. SERIAL_ERROR_START;
  1863. SERIAL_ERRORLNPGM(MSG_ERR_NO_THERMISTORS);
  1864. #endif
  1865.  
  1866. SERIAL_PROTOCOLPGM(" @:");
  1867. #ifdef EXTRUDER_WATTS
  1868. SERIAL_PROTOCOL((EXTRUDER_WATTS * getHeaterPower(tmp_extruder))/127);
  1869. SERIAL_PROTOCOLPGM("W");
  1870. #else
  1871. SERIAL_PROTOCOL(getHeaterPower(tmp_extruder));
  1872. #endif
  1873.  
  1874. SERIAL_PROTOCOLPGM(" B@:");
  1875. #ifdef BED_WATTS
  1876. SERIAL_PROTOCOL((BED_WATTS * getHeaterPower(-1))/127);
  1877. SERIAL_PROTOCOLPGM("W");
  1878. #else
  1879. SERIAL_PROTOCOL(getHeaterPower(-1));
  1880. #endif
  1881.  
  1882. #ifdef SHOW_TEMP_ADC_VALUES
  1883. #if defined(TEMP_BED_PIN) && TEMP_BED_PIN > -1
  1884. SERIAL_PROTOCOLPGM(" ADC B:");
  1885. SERIAL_PROTOCOL_F(degBed(),1);
  1886. SERIAL_PROTOCOLPGM("C->");
  1887. SERIAL_PROTOCOL_F(rawBedTemp()/OVERSAMPLENR,0);
  1888. #endif
  1889. for (int8_t cur_extruder = 0; cur_extruder < EXTRUDERS; ++cur_extruder) {
  1890. SERIAL_PROTOCOLPGM(" T");
  1891. SERIAL_PROTOCOL(cur_extruder);
  1892. SERIAL_PROTOCOLPGM(":");
  1893. SERIAL_PROTOCOL_F(degHotend(cur_extruder),1);
  1894. SERIAL_PROTOCOLPGM("C->");
  1895. SERIAL_PROTOCOL_F(rawHotendTemp(cur_extruder)/OVERSAMPLENR,0);
  1896. }
  1897. #endif
  1898.  
  1899. SERIAL_PROTOCOLLN("");
  1900. return;
  1901. break;
  1902. case 109:
  1903. {// M109 - Wait for extruder heater to reach target.
  1904. if(setTargetedHotend(109)){
  1905. break;
  1906. }
  1907. LCD_MESSAGEPGM(MSG_HEATING);
  1908. #ifdef AUTOTEMP
  1909. autotemp_enabled=false;
  1910. #endif
  1911. if (code_seen('S')) {
  1912. setTargetHotend(code_value(), tmp_extruder);
  1913. #ifdef DUAL_X_CARRIAGE
  1914. if (dual_x_carriage_mode == DXC_DUPLICATION_MODE && tmp_extruder == 0)
  1915. setTargetHotend1(code_value() == 0.0 ? 0.0 : code_value() + duplicate_extruder_temp_offset);
  1916. #endif
  1917. CooldownNoWait = true;
  1918. } else if (code_seen('R')) {
  1919. setTargetHotend(code_value(), tmp_extruder);
  1920. #ifdef DUAL_X_CARRIAGE
  1921. if (dual_x_carriage_mode == DXC_DUPLICATION_MODE && tmp_extruder == 0)
  1922. setTargetHotend1(code_value() == 0.0 ? 0.0 : code_value() + duplicate_extruder_temp_offset);
  1923. #endif
  1924. CooldownNoWait = false;
  1925. }
  1926. #ifdef AUTOTEMP
  1927. if (code_seen('S')) autotemp_min=code_value();
  1928. if (code_seen('B')) autotemp_max=code_value();
  1929. if (code_seen('F'))
  1930. {
  1931. autotemp_factor=code_value();
  1932. autotemp_enabled=true;
  1933. }
  1934. #endif
  1935.  
  1936. setWatch();
  1937. codenum = millis();
  1938.  
  1939. /* See if we are heating up or cooling down */
  1940. target_direction = isHeatingHotend(tmp_extruder); // true if heating, false if cooling
  1941.  
  1942. #ifdef TEMP_RESIDENCY_TIME
  1943. long residencyStart;
  1944. residencyStart = -1;
  1945. /* continue to loop until we have reached the target temp
  1946. _and_ until TEMP_RESIDENCY_TIME hasn't passed since we reached it */
  1947. while((residencyStart == -1) ||
  1948. (residencyStart >= 0 && (((unsigned int) (millis() - residencyStart)) < (TEMP_RESIDENCY_TIME * 1000UL))) ) {
  1949. #else
  1950. while ( target_direction ? (isHeatingHotend(tmp_extruder)) : (isCoolingHotend(tmp_extruder)&&(CooldownNoWait==false)) ) {
  1951. #endif //TEMP_RESIDENCY_TIME
  1952. if( (millis() - codenum) > 1000UL )
  1953. { //Print Temp Reading and remaining time every 1 second while heating up/cooling down
  1954. SERIAL_PROTOCOLPGM("T:");
  1955. SERIAL_PROTOCOL_F(degHotend(tmp_extruder),1);
  1956. SERIAL_PROTOCOLPGM(" E:");
  1957. SERIAL_PROTOCOL((int)tmp_extruder);
  1958. #ifdef TEMP_RESIDENCY_TIME
  1959. SERIAL_PROTOCOLPGM(" W:");
  1960. if(residencyStart > -1)
  1961. {
  1962. codenum = ((TEMP_RESIDENCY_TIME * 1000UL) - (millis() - residencyStart)) / 1000UL;
  1963. SERIAL_PROTOCOLLN( codenum );
  1964. }
  1965. else
  1966. {
  1967. SERIAL_PROTOCOLLN( "?" );
  1968. }
  1969. #else
  1970. SERIAL_PROTOCOLLN("");
  1971. #endif
  1972. codenum = millis();
  1973. }
  1974. manage_heater();
  1975. manage_inactivity();
  1976. lcd_update();
  1977. #ifdef TEMP_RESIDENCY_TIME
  1978. /* start/restart the TEMP_RESIDENCY_TIME timer whenever we reach target temp for the first time
  1979. or when current temp falls outside the hysteresis after target temp was reached */
  1980. if ((residencyStart == -1 && target_direction && (degHotend(tmp_extruder) >= (degTargetHotend(tmp_extruder)-TEMP_WINDOW))) ||
  1981. (residencyStart == -1 && !target_direction && (degHotend(tmp_extruder) <= (degTargetHotend(tmp_extruder)+TEMP_WINDOW))) ||
  1982. (residencyStart > -1 && labs(degHotend(tmp_extruder) - degTargetHotend(tmp_extruder)) > TEMP_HYSTERESIS) )
  1983. {
  1984. residencyStart = millis();
  1985. }
  1986. #endif //TEMP_RESIDENCY_TIME
  1987. }
  1988. LCD_MESSAGEPGM(MSG_HEATING_COMPLETE);
  1989. starttime=millis();
  1990. previous_millis_cmd = millis();
  1991. }
  1992. break;
  1993. case 190: // M190 - Wait for bed heater to reach target.
  1994. #if defined(TEMP_BED_PIN) && TEMP_BED_PIN > -1
  1995. LCD_MESSAGEPGM(MSG_BED_HEATING);
  1996. if (code_seen('S')) {
  1997. setTargetBed(code_value());
  1998. CooldownNoWait = true;
  1999. } else if (code_seen('R')) {
  2000. setTargetBed(code_value());
  2001. CooldownNoWait = false;
  2002. }
  2003. codenum = millis();
  2004.  
  2005. target_direction = isHeatingBed(); // true if heating, false if cooling
  2006.  
  2007. while ( target_direction ? (isHeatingBed()) : (isCoolingBed()&&(CooldownNoWait==false)) )
  2008. {
  2009. if(( millis() - codenum) > 1000 ) //Print Temp Reading every 1 second while heating up.
  2010. {
  2011. float tt=degHotend(active_extruder);
  2012. SERIAL_PROTOCOLPGM("T:");
  2013. SERIAL_PROTOCOL(tt);
  2014. SERIAL_PROTOCOLPGM(" E:");
  2015. SERIAL_PROTOCOL((int)active_extruder);
  2016. SERIAL_PROTOCOLPGM(" B:");
  2017. SERIAL_PROTOCOL_F(degBed(),1);
  2018. SERIAL_PROTOCOLLN("");
  2019. codenum = millis();
  2020. }
  2021. manage_heater();
  2022. manage_inactivity();
  2023. lcd_update();
  2024. }
  2025. LCD_MESSAGEPGM(MSG_BED_DONE);
  2026. previous_millis_cmd = millis();
  2027. #endif
  2028. break;
  2029.  
  2030. #if defined(FAN_PIN) && FAN_PIN > -1
  2031. case 106: //M106 Fan On
  2032. if (code_seen('S')){
  2033. fanSpeed=constrain(code_value(),0,255);
  2034. }
  2035. else {
  2036. fanSpeed=255;
  2037. }
  2038. break;
  2039. case 107: //M107 Fan Off
  2040. fanSpeed = 0;
  2041. break;
  2042. #endif //FAN_PIN
  2043. #ifdef BARICUDA
  2044. // PWM for HEATER_1_PIN
  2045. #if defined(HEATER_1_PIN) && HEATER_1_PIN > -1
  2046. case 126: //M126 valve open
  2047. if (code_seen('S')){
  2048. ValvePressure=constrain(code_value(),0,255);
  2049. }
  2050. else {
  2051. ValvePressure=255;
  2052. }
  2053. break;
  2054. case 127: //M127 valve closed
  2055. ValvePressure = 0;
  2056. break;
  2057. #endif //HEATER_1_PIN
  2058.  
  2059. // PWM for HEATER_2_PIN
  2060. #if defined(HEATER_2_PIN) && HEATER_2_PIN > -1
  2061. case 128: //M128 valve open
  2062. if (code_seen('S')){
  2063. EtoPPressure=constrain(code_value(),0,255);
  2064. }
  2065. else {
  2066. EtoPPressure=255;
  2067. }
  2068. break;
  2069. case 129: //M129 valve closed
  2070. EtoPPressure = 0;
  2071. break;
  2072. #endif //HEATER_2_PIN
  2073. #endif
  2074.  
  2075. #if defined(PS_ON_PIN) && PS_ON_PIN > -1
  2076. case 80: // M80 - Turn on Power Supply
  2077. SET_OUTPUT(PS_ON_PIN); //GND
  2078. WRITE(PS_ON_PIN, PS_ON_AWAKE);
  2079.  
  2080. // If you have a switch on suicide pin, this is useful
  2081. // if you want to start another print with suicide feature after
  2082. // a print without suicide...
  2083. #if defined SUICIDE_PIN && SUICIDE_PIN > -1
  2084. SET_OUTPUT(SUICIDE_PIN);
  2085. WRITE(SUICIDE_PIN, HIGH);
  2086. #endif
  2087.  
  2088. #ifdef ULTIPANEL
  2089. powersupply = true;
  2090. LCD_MESSAGEPGM(WELCOME_MSG);
  2091. lcd_update();
  2092. #endif
  2093. break;
  2094. #endif
  2095.  
  2096. case 81: // M81 - Turn off Power Supply
  2097. disable_heater();
  2098. st_synchronize();
  2099. disable_e0();
  2100. disable_e1();
  2101. disable_e2();
  2102. finishAndDisableSteppers();
  2103. fanSpeed = 0;
  2104. delay(1000); // Wait a little before to switch off
  2105. #if defined(SUICIDE_PIN) && SUICIDE_PIN > -1
  2106. st_synchronize();
  2107. suicide();
  2108. #elif defined(PS_ON_PIN) && PS_ON_PIN > -1
  2109. SET_OUTPUT(PS_ON_PIN);
  2110. WRITE(PS_ON_PIN, PS_ON_ASLEEP);
  2111. #endif
  2112. #ifdef ULTIPANEL
  2113. powersupply = false;
  2114. LCD_MESSAGEPGM(MACHINE_NAME" "MSG_OFF".");
  2115. lcd_update();
  2116. #endif
  2117. break;
  2118.  
  2119. case 82:
  2120. axis_relative_modes[3] = false;
  2121. break;
  2122. case 83:
  2123. axis_relative_modes[3] = true;
  2124. break;
  2125. case 18: //compatibility
  2126. case 84: // M84
  2127. if(code_seen('S')){
  2128. stepper_inactive_time = code_value() * 1000;
  2129. }
  2130. else
  2131. {
  2132. bool all_axis = !((code_seen(axis_codes[X_AXIS])) || (code_seen(axis_codes[Y_AXIS])) || (code_seen(axis_codes[Z_AXIS]))|| (code_seen(axis_codes[E_AXIS])));
  2133. if(all_axis)
  2134. {
  2135. st_synchronize();
  2136. disable_e0();
  2137. disable_e1();
  2138. disable_e2();
  2139. finishAndDisableSteppers();
  2140. }
  2141. else
  2142. {
  2143. st_synchronize();
  2144. if(code_seen('X')) disable_x();
  2145. if(code_seen('Y')) disable_y();
  2146. if(code_seen('Z')) disable_z();
  2147. #if ((E0_ENABLE_PIN != X_ENABLE_PIN) && (E1_ENABLE_PIN != Y_ENABLE_PIN)) // Only enable on boards that have seperate ENABLE_PINS
  2148. if(code_seen('E')) {
  2149. disable_e0();
  2150. disable_e1();
  2151. disable_e2();
  2152. }
  2153. #endif
  2154. }
  2155. }
  2156. break;
  2157. case 85: // M85
  2158. code_seen('S');
  2159. max_inactive_time = code_value() * 1000;
  2160. break;
  2161. case 92: // M92
  2162. for(int8_t i=0; i < NUM_AXIS; i++)
  2163. {
  2164. if(code_seen(axis_codes[i]))
  2165. {
  2166. if(i == 3) { // E
  2167. float value = code_value();
  2168. if(value < 20.0) {
  2169. float factor = axis_steps_per_unit[i] / value; // increase e constants if M92 E14 is given for netfab.
  2170. max_e_jerk *= factor;
  2171. max_feedrate[i] *= factor;
  2172. axis_steps_per_sqr_second[i] *= factor;
  2173. }
  2174. axis_steps_per_unit[i] = value;
  2175. }
  2176. else {
  2177. axis_steps_per_unit[i] = code_value();
  2178. }
  2179. }
  2180. }
  2181. break;
  2182. case 115: // M115
  2183. SERIAL_PROTOCOLPGM(MSG_M115_REPORT);
  2184. break;
  2185. case 117: // M117 display message
  2186. starpos = (strchr(strchr_pointer + 5,'*'));
  2187. if(starpos!=NULL)
  2188. *(starpos-1)='\0';
  2189. lcd_setstatus(strchr_pointer + 5);
  2190. break;
  2191. case 114: // M114
  2192. SERIAL_PROTOCOLPGM("X:");
  2193. SERIAL_PROTOCOL(current_position[X_AXIS]);
  2194. SERIAL_PROTOCOLPGM(" Y:");
  2195. SERIAL_PROTOCOL(current_position[Y_AXIS]);
  2196. SERIAL_PROTOCOLPGM(" Z:");
  2197. SERIAL_PROTOCOL(current_position[Z_AXIS]);
  2198. SERIAL_PROTOCOLPGM(" E:");
  2199. SERIAL_PROTOCOL(current_position[E_AXIS]);
  2200.  
  2201. SERIAL_PROTOCOLPGM(MSG_COUNT_X);
  2202. SERIAL_PROTOCOL(float(st_get_position(X_AXIS))/axis_steps_per_unit[X_AXIS]);
  2203. SERIAL_PROTOCOLPGM(" Y:");
  2204. SERIAL_PROTOCOL(float(st_get_position(Y_AXIS))/axis_steps_per_unit[Y_AXIS]);
  2205. SERIAL_PROTOCOLPGM(" Z:");
  2206. SERIAL_PROTOCOL(float(st_get_position(Z_AXIS))/axis_steps_per_unit[Z_AXIS]);
  2207.  
  2208. SERIAL_PROTOCOLLN("");
  2209. break;
  2210. case 120: // M120
  2211. enable_endstops(false) ;
  2212. break;
  2213. case 121: // M121
  2214. enable_endstops(true) ;
  2215. break;
  2216. case 119: // M119
  2217. SERIAL_PROTOCOLLN(MSG_M119_REPORT);
  2218. #if defined(X_MIN_PIN) && X_MIN_PIN > -1
  2219. SERIAL_PROTOCOLPGM(MSG_X_MIN);
  2220. SERIAL_PROTOCOLLN(((READ(X_MIN_PIN)^X_MIN_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
  2221. #endif
  2222. #if defined(X_MAX_PIN) && X_MAX_PIN > -1
  2223. SERIAL_PROTOCOLPGM(MSG_X_MAX);
  2224. SERIAL_PROTOCOLLN(((READ(X_MAX_PIN)^X_MAX_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
  2225. #endif
  2226. #if defined(Y_MIN_PIN) && Y_MIN_PIN > -1
  2227. SERIAL_PROTOCOLPGM(MSG_Y_MIN);
  2228. SERIAL_PROTOCOLLN(((READ(Y_MIN_PIN)^Y_MIN_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
  2229. #endif
  2230. #if defined(Y_MAX_PIN) && Y_MAX_PIN > -1
  2231. SERIAL_PROTOCOLPGM(MSG_Y_MAX);
  2232. SERIAL_PROTOCOLLN(((READ(Y_MAX_PIN)^Y_MAX_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
  2233. #endif
  2234. #if defined(Z_MIN_PIN) && Z_MIN_PIN > -1
  2235. SERIAL_PROTOCOLPGM(MSG_Z_MIN);
  2236. SERIAL_PROTOCOLLN(((READ(Z_MIN_PIN)^Z_MIN_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
  2237. #endif
  2238. #if defined(Z_MAX_PIN) && Z_MAX_PIN > -1
  2239. SERIAL_PROTOCOLPGM(MSG_Z_MAX);
  2240. SERIAL_PROTOCOLLN(((READ(Z_MAX_PIN)^Z_MAX_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
  2241. #endif
  2242. break;
  2243. //TODO: update for all axis, use for loop
  2244. #ifdef BLINKM
  2245. case 150: // M150
  2246. {
  2247. byte red;
  2248. byte grn;
  2249. byte blu;
  2250.  
  2251. if(code_seen('R')) red = code_value();
  2252. if(code_seen('U')) grn = code_value();
  2253. if(code_seen('B')) blu = code_value();
  2254.  
  2255. SendColors(red,grn,blu);
  2256. }
  2257. break;
  2258. #endif //BLINKM
  2259. case 200: // M200 D<millimeters> set filament diameter and set E axis units to cubic millimeters (use S0 to set back to millimeters).
  2260. {
  2261. float area = .0;
  2262. float radius = .0;
  2263. if(code_seen('D')) {
  2264. radius = (float)code_value() * .5;
  2265. if(radius == 0) {
  2266. area = 1;
  2267. } else {
  2268. area = M_PI * pow(radius, 2);
  2269. }
  2270. } else {
  2271. //reserved for setting filament diameter via UFID or filament measuring device
  2272. break;
  2273. }
  2274. tmp_extruder = active_extruder;
  2275. if(code_seen('T')) {
  2276. tmp_extruder = code_value();
  2277. if(tmp_extruder >= EXTRUDERS) {
  2278. SERIAL_ECHO_START;
  2279. SERIAL_ECHO(MSG_M200_INVALID_EXTRUDER);
  2280. }
  2281. SERIAL_ECHOLN(tmp_extruder);
  2282. break;
  2283. }
  2284. volumetric_multiplier[tmp_extruder] = 1 / area;
  2285. }
  2286. break;
  2287. case 201: // M201
  2288. for(int8_t i=0; i < NUM_AXIS; i++)
  2289. {
  2290. if(code_seen(axis_codes[i]))
  2291. {
  2292. max_acceleration_units_per_sq_second[i] = code_value();
  2293. }
  2294. }
  2295. // steps per sq second need to be updated to agree with the units per sq second (as they are what is used in the planner)
  2296. reset_acceleration_rates();
  2297. break;
  2298. #if 0 // Not used for Sprinter/grbl gen6
  2299. case 202: // M202
  2300. for(int8_t i=0; i < NUM_AXIS; i++) {
  2301. if(code_seen(axis_codes[i])) axis_travel_steps_per_sqr_second[i] = code_value() * axis_steps_per_unit[i];
  2302. }
  2303. break;
  2304. #endif
  2305. case 203: // M203 max feedrate mm/sec
  2306. for(int8_t i=0; i < NUM_AXIS; i++) {
  2307. if(code_seen(axis_codes[i])) max_feedrate[i] = code_value();
  2308. }
  2309. break;
  2310. case 204: // M204 acclereration S normal moves T filmanent only moves
  2311. {
  2312. if(code_seen('S')) acceleration = code_value() ;
  2313. if(code_seen('T')) retract_acceleration = code_value() ;
  2314. }
  2315. break;
  2316. 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
  2317. {
  2318. if(code_seen('S')) minimumfeedrate = code_value();
  2319. if(code_seen('T')) mintravelfeedrate = code_value();
  2320. if(code_seen('B')) minsegmenttime = code_value() ;
  2321. if(code_seen('X')) max_xy_jerk = code_value() ;
  2322. if(code_seen('Z')) max_z_jerk = code_value() ;
  2323. if(code_seen('E')) max_e_jerk = code_value() ;
  2324. }
  2325. break;
  2326. case 206: // M206 additional homeing offset
  2327. for(int8_t i=0; i < 3; i++)
  2328. {
  2329. if(code_seen(axis_codes[i])) add_homeing[i] = code_value();
  2330. }
  2331. break;
  2332. #ifdef DELTA
  2333. case 665: // M665 set delta configurations L<diagonal_rod> R<delta_radius> S<segments_per_sec>
  2334. if(code_seen('L')) {
  2335. delta_diagonal_rod= code_value();
  2336. }
  2337. if(code_seen('R')) {
  2338. delta_radius= code_value();
  2339. }
  2340. if(code_seen('S')) {
  2341. delta_segments_per_second= code_value();
  2342. }
  2343.  
  2344. recalc_delta_settings(delta_radius, delta_diagonal_rod);
  2345. break;
  2346. case 666: // M666 set delta endstop adjustemnt
  2347. for(int8_t i=0; i < 3; i++)
  2348. {
  2349. if(code_seen(axis_codes[i])) endstop_adj[i] = code_value();
  2350. }
  2351. break;
  2352. #endif
  2353. #ifdef FWRETRACT
  2354. case 207: //M207 - set retract length S[positive mm] F[feedrate mm/sec] Z[additional zlift/hop]
  2355. {
  2356. if(code_seen('S'))
  2357. {
  2358. retract_length = code_value() ;
  2359. }
  2360. if(code_seen('F'))
  2361. {
  2362. retract_feedrate = code_value() ;
  2363. }
  2364. if(code_seen('Z'))
  2365. {
  2366. retract_zlift = code_value() ;
  2367. }
  2368. }break;
  2369. case 208: // M208 - set retract recover length S[positive mm surplus to the M207 S*] F[feedrate mm/sec]
  2370. {
  2371. if(code_seen('S'))
  2372. {
  2373. retract_recover_length = code_value() ;
  2374. }
  2375. if(code_seen('F'))
  2376. {
  2377. retract_recover_feedrate = code_value() ;
  2378. }
  2379. }break;
  2380. 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.
  2381. {
  2382. if(code_seen('S'))
  2383. {
  2384. int t= code_value() ;
  2385. switch(t)
  2386. {
  2387. case 0: autoretract_enabled=false;retracted=false;break;
  2388. case 1: autoretract_enabled=true;retracted=false;break;
  2389. default:
  2390. SERIAL_ECHO_START;
  2391. SERIAL_ECHOPGM(MSG_UNKNOWN_COMMAND);
  2392. SERIAL_ECHO(cmdbuffer[bufindr]);
  2393. SERIAL_ECHOLNPGM("\"");
  2394. }
  2395. }
  2396.  
  2397. }break;
  2398. #endif // FWRETRACT
  2399. #if EXTRUDERS > 1
  2400. case 218: // M218 - set hotend offset (in mm), T<extruder_number> X<offset_on_X> Y<offset_on_Y>
  2401. {
  2402. if(setTargetedHotend(218)){
  2403. break;
  2404. }
  2405. if(code_seen('X'))
  2406. {
  2407. extruder_offset[X_AXIS][tmp_extruder] = code_value();
  2408. }
  2409. if(code_seen('Y'))
  2410. {
  2411. extruder_offset[Y_AXIS][tmp_extruder] = code_value();
  2412. }
  2413. #ifdef DUAL_X_CARRIAGE
  2414. if(code_seen('Z'))
  2415. {
  2416. extruder_offset[Z_AXIS][tmp_extruder] = code_value();
  2417. }
  2418. #endif
  2419. SERIAL_ECHO_START;
  2420. SERIAL_ECHOPGM(MSG_HOTEND_OFFSET);
  2421. for(tmp_extruder = 0; tmp_extruder < EXTRUDERS; tmp_extruder++)
  2422. {
  2423. SERIAL_ECHO(" ");
  2424. SERIAL_ECHO(extruder_offset[X_AXIS][tmp_extruder]);
  2425. SERIAL_ECHO(",");
  2426. SERIAL_ECHO(extruder_offset[Y_AXIS][tmp_extruder]);
  2427. #ifdef DUAL_X_CARRIAGE
  2428. SERIAL_ECHO(",");
  2429. SERIAL_ECHO(extruder_offset[Z_AXIS][tmp_extruder]);
  2430. #endif
  2431. }
  2432. SERIAL_ECHOLN("");
  2433. }break;
  2434. #endif
  2435. case 220: // M220 S<factor in percent>- set speed factor override percentage
  2436. {
  2437. if(code_seen('S'))
  2438. {
  2439. feedmultiply = code_value() ;
  2440. }
  2441. }
  2442. break;
  2443. case 221: // M221 S<factor in percent>- set extrude factor override percentage
  2444. {
  2445. if(code_seen('S'))
  2446. {
  2447. int tmp_code = code_value();
  2448. if (code_seen('T'))
  2449. {
  2450. if(setTargetedHotend(221)){
  2451. break;
  2452. }
  2453. extruder_multiply[tmp_extruder] = tmp_code;
  2454. }
  2455. else
  2456. {
  2457. extrudemultiply = tmp_code ;
  2458. }
  2459. }
  2460. }
  2461. break;
  2462.  
  2463. case 226: // M226 P<pin number> S<pin state>- Wait until the specified pin reaches the state required
  2464. {
  2465. if(code_seen('P')){
  2466. int pin_number = code_value(); // pin number
  2467. int pin_state = -1; // required pin state - default is inverted
  2468.  
  2469. if(code_seen('S')) pin_state = code_value(); // required pin state
  2470.  
  2471. if(pin_state >= -1 && pin_state <= 1){
  2472.  
  2473. for(int8_t i = 0; i < (int8_t)sizeof(sensitive_pins); i++)
  2474. {
  2475. if (sensitive_pins[i] == pin_number)
  2476. {
  2477. pin_number = -1;
  2478. break;
  2479. }
  2480. }
  2481.  
  2482. if (pin_number > -1)
  2483. {
  2484. st_synchronize();
  2485.  
  2486. pinMode(pin_number, INPUT);
  2487.  
  2488. int target;
  2489. switch(pin_state){
  2490. case 1:
  2491. target = HIGH;
  2492. break;
  2493.  
  2494. case 0:
  2495. target = LOW;
  2496. break;
  2497.  
  2498. case -1:
  2499. target = !digitalRead(pin_number);
  2500. break;
  2501. }
  2502.  
  2503. while(digitalRead(pin_number) != target){
  2504. manage_heater();
  2505. manage_inactivity();
  2506. lcd_update();
  2507. }
  2508. }
  2509. }
  2510. }
  2511. }
  2512. break;
  2513.  
  2514. #if NUM_SERVOS > 0
  2515. case 280: // M280 - set servo position absolute. P: servo index, S: angle or microseconds
  2516. {
  2517. int servo_index = -1;
  2518. int servo_position = 0;
  2519. if (code_seen('P'))
  2520. servo_index = code_value();
  2521. if (code_seen('S')) {
  2522. servo_position = code_value();
  2523. if ((servo_index >= 0) && (servo_index < NUM_SERVOS)) {
  2524. #if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
  2525. servos[servo_index].attach(0);
  2526. #endif
  2527. servos[servo_index].write(servo_position);
  2528. #if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
  2529. delay(PROBE_SERVO_DEACTIVATION_DELAY);
  2530. servos[servo_index].detach();
  2531. #endif
  2532. }
  2533. else {
  2534. SERIAL_ECHO_START;
  2535. SERIAL_ECHO("Servo ");
  2536. SERIAL_ECHO(servo_index);
  2537. SERIAL_ECHOLN(" out of range");
  2538. }
  2539. }
  2540. else if (servo_index >= 0) {
  2541. SERIAL_PROTOCOL(MSG_OK);
  2542. SERIAL_PROTOCOL(" Servo ");
  2543. SERIAL_PROTOCOL(servo_index);
  2544. SERIAL_PROTOCOL(": ");
  2545. SERIAL_PROTOCOL(servos[servo_index].read());
  2546. SERIAL_PROTOCOLLN("");
  2547. }
  2548. }
  2549. break;
  2550. #endif // NUM_SERVOS > 0
  2551.  
  2552. #if (LARGE_FLASH == true && ( BEEPER > 0 || defined(ULTRALCD) || defined(LCD_USE_I2C_BUZZER)))
  2553. case 300: // M300
  2554. {
  2555. int beepS = code_seen('S') ? code_value() : 110;
  2556. int beepP = code_seen('P') ? code_value() : 1000;
  2557. if (beepS > 0)
  2558. {
  2559. #if BEEPER > 0
  2560. tone(BEEPER, beepS);
  2561. delay(beepP);
  2562. noTone(BEEPER);
  2563. #elif defined(ULTRALCD)
  2564. lcd_buzz(beepS, beepP);
  2565. #elif defined(LCD_USE_I2C_BUZZER)
  2566. lcd_buzz(beepP, beepS);
  2567. #endif
  2568. }
  2569. else
  2570. {
  2571. delay(beepP);
  2572. }
  2573. }
  2574. break;
  2575. #endif // M300
  2576.  
  2577. #ifdef PIDTEMP
  2578. case 301: // M301
  2579. {
  2580. if(code_seen('P')) Kp = code_value();
  2581. if(code_seen('I')) Ki = scalePID_i(code_value());
  2582. if(code_seen('D')) Kd = scalePID_d(code_value());
  2583.  
  2584. #ifdef PID_ADD_EXTRUSION_RATE
  2585. if(code_seen('C')) Kc = code_value();
  2586. #endif
  2587.  
  2588. updatePID();
  2589. SERIAL_PROTOCOL(MSG_OK);
  2590. SERIAL_PROTOCOL(" p:");
  2591. SERIAL_PROTOCOL(Kp);
  2592. SERIAL_PROTOCOL(" i:");
  2593. SERIAL_PROTOCOL(unscalePID_i(Ki));
  2594. SERIAL_PROTOCOL(" d:");
  2595. SERIAL_PROTOCOL(unscalePID_d(Kd));
  2596. #ifdef PID_ADD_EXTRUSION_RATE
  2597. SERIAL_PROTOCOL(" c:");
  2598. //Kc does not have scaling applied above, or in resetting defaults
  2599. SERIAL_PROTOCOL(Kc);
  2600. #endif
  2601. SERIAL_PROTOCOLLN("");
  2602. }
  2603. break;
  2604. #endif //PIDTEMP
  2605. #ifdef PIDTEMPBED
  2606. case 304: // M304
  2607. {
  2608. if(code_seen('P')) bedKp = code_value();
  2609. if(code_seen('I')) bedKi = scalePID_i(code_value());
  2610. if(code_seen('D')) bedKd = scalePID_d(code_value());
  2611.  
  2612. updatePID();
  2613. SERIAL_PROTOCOL(MSG_OK);
  2614. SERIAL_PROTOCOL(" p:");
  2615. SERIAL_PROTOCOL(bedKp);
  2616. SERIAL_PROTOCOL(" i:");
  2617. SERIAL_PROTOCOL(unscalePID_i(bedKi));
  2618. SERIAL_PROTOCOL(" d:");
  2619. SERIAL_PROTOCOL(unscalePID_d(bedKd));
  2620. SERIAL_PROTOCOLLN("");
  2621. }
  2622. break;
  2623. #endif //PIDTEMP
  2624. case 240: // M240 Triggers a camera by emulating a Canon RC-1 : http://www.doc-diy.net/photo/rc-1_hacked/
  2625. {
  2626. #ifdef CHDK
  2627.  
  2628. SET_OUTPUT(CHDK);
  2629. WRITE(CHDK, HIGH);
  2630. chdkHigh = millis();
  2631. chdkActive = true;
  2632.  
  2633. #else
  2634.  
  2635. #if defined(PHOTOGRAPH_PIN) && PHOTOGRAPH_PIN > -1
  2636. const uint8_t NUM_PULSES=16;
  2637. const float PULSE_LENGTH=0.01524;
  2638. for(int i=0; i < NUM_PULSES; i++) {
  2639. WRITE(PHOTOGRAPH_PIN, HIGH);
  2640. _delay_ms(PULSE_LENGTH);
  2641. WRITE(PHOTOGRAPH_PIN, LOW);
  2642. _delay_ms(PULSE_LENGTH);
  2643. }
  2644. delay(7.33);
  2645. for(int i=0; i < NUM_PULSES; i++) {
  2646. WRITE(PHOTOGRAPH_PIN, HIGH);
  2647. _delay_ms(PULSE_LENGTH);
  2648. WRITE(PHOTOGRAPH_PIN, LOW);
  2649. _delay_ms(PULSE_LENGTH);
  2650. }
  2651. #endif
  2652. #endif //chdk end if
  2653. }
  2654. break;
  2655. #ifdef DOGLCD
  2656. case 250: // M250 Set LCD contrast value: C<value> (value 0..63)
  2657. {
  2658. if (code_seen('C')) {
  2659. lcd_setcontrast( ((int)code_value())&63 );
  2660. }
  2661. SERIAL_PROTOCOLPGM("lcd contrast value: ");
  2662. SERIAL_PROTOCOL(lcd_contrast);
  2663. SERIAL_PROTOCOLLN("");
  2664. }
  2665. break;
  2666. #endif
  2667. #ifdef PREVENT_DANGEROUS_EXTRUDE
  2668. case 302: // allow cold extrudes, or set the minimum extrude temperature
  2669. {
  2670. float temp = .0;
  2671. if (code_seen('S')) temp=code_value();
  2672. set_extrude_min_temp(temp);
  2673. }
  2674. break;
  2675. #endif
  2676. case 303: // M303 PID autotune
  2677. {
  2678. float temp = 150.0;
  2679. int e=0;
  2680. int c=5;
  2681. if (code_seen('E')) e=code_value();
  2682. if (e<0)
  2683. temp=70;
  2684. if (code_seen('S')) temp=code_value();
  2685. if (code_seen('C')) c=code_value();
  2686. PID_autotune(temp, e, c);
  2687. }
  2688. break;
  2689. case 400: // M400 finish all moves
  2690. {
  2691. st_synchronize();
  2692. }
  2693. break;
  2694. #if defined(ENABLE_AUTO_BED_LEVELING) && defined(SERVO_ENDSTOPS)
  2695. case 401:
  2696. {
  2697. engage_z_probe(); // Engage Z Servo endstop if available
  2698. }
  2699. break;
  2700.  
  2701. case 402:
  2702. {
  2703. retract_z_probe(); // Retract Z Servo endstop if enabled
  2704. }
  2705. break;
  2706. #endif
  2707. case 500: // M500 Store settings in EEPROM
  2708. {
  2709. Config_StoreSettings();
  2710. }
  2711. break;
  2712. case 501: // M501 Read settings from EEPROM
  2713. {
  2714. Config_RetrieveSettings();
  2715. }
  2716. break;
  2717. case 502: // M502 Revert to default settings
  2718. {
  2719. Config_ResetDefault();
  2720. }
  2721. break;
  2722. case 503: // M503 print settings currently in memory
  2723. {
  2724. Config_PrintSettings();
  2725. }
  2726. break;
  2727. #ifdef ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED
  2728. case 540:
  2729. {
  2730. if(code_seen('S')) abort_on_endstop_hit = code_value() > 0;
  2731. }
  2732. break;
  2733. #endif
  2734. #ifdef FILAMENTCHANGEENABLE
  2735. case 600: //Pause for filament change X[pos] Y[pos] Z[relative lift] E[initial retract] L[later retract distance for removal]
  2736. {
  2737. float target[4];
  2738. float lastpos[4];
  2739. target[X_AXIS]=current_position[X_AXIS];
  2740. target[Y_AXIS]=current_position[Y_AXIS];
  2741. target[Z_AXIS]=current_position[Z_AXIS];
  2742. target[E_AXIS]=current_position[E_AXIS];
  2743. lastpos[X_AXIS]=current_position[X_AXIS];
  2744. lastpos[Y_AXIS]=current_position[Y_AXIS];
  2745. lastpos[Z_AXIS]=current_position[Z_AXIS];
  2746. lastpos[E_AXIS]=current_position[E_AXIS];
  2747. //retract by E
  2748. if(code_seen('E'))
  2749. {
  2750. target[E_AXIS]+= code_value();
  2751. }
  2752. else
  2753. {
  2754. #ifdef FILAMENTCHANGE_FIRSTRETRACT
  2755. target[E_AXIS]+= FILAMENTCHANGE_FIRSTRETRACT ;
  2756. #endif
  2757. }
  2758. plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], feedrate/60, active_extruder);
  2759.  
  2760. //lift Z
  2761. if(code_seen('Z'))
  2762. {
  2763. target[Z_AXIS]+= code_value();
  2764. }
  2765. else
  2766. {
  2767. #ifdef FILAMENTCHANGE_ZADD
  2768. target[Z_AXIS]+= FILAMENTCHANGE_ZADD ;
  2769. #endif
  2770. }
  2771. plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], feedrate/60, active_extruder);
  2772.  
  2773. //move xy
  2774. if(code_seen('X'))
  2775. {
  2776. target[X_AXIS]+= code_value();
  2777. }
  2778. else
  2779. {
  2780. #ifdef FILAMENTCHANGE_XPOS
  2781. target[X_AXIS]= FILAMENTCHANGE_XPOS ;
  2782. #endif
  2783. }
  2784. if(code_seen('Y'))
  2785. {
  2786. target[Y_AXIS]= code_value();
  2787. }
  2788. else
  2789. {
  2790. #ifdef FILAMENTCHANGE_YPOS
  2791. target[Y_AXIS]= FILAMENTCHANGE_YPOS ;
  2792. #endif
  2793. }
  2794.  
  2795. plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], feedrate/60, active_extruder);
  2796.  
  2797. if(code_seen('L'))
  2798. {
  2799. target[E_AXIS]+= code_value();
  2800. }
  2801. else
  2802. {
  2803. #ifdef FILAMENTCHANGE_FINALRETRACT
  2804. target[E_AXIS]+= FILAMENTCHANGE_FINALRETRACT ;
  2805. #endif
  2806. }
  2807.  
  2808. plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], feedrate/60, active_extruder);
  2809.  
  2810. //finish moves
  2811. st_synchronize();
  2812. //disable extruder steppers so filament can be removed
  2813. disable_e0();
  2814. disable_e1();
  2815. disable_e2();
  2816. delay(100);
  2817. LCD_ALERTMESSAGEPGM(MSG_FILAMENTCHANGE);
  2818. uint8_t cnt=0;
  2819. while(!lcd_clicked()){
  2820. cnt++;
  2821. manage_heater();
  2822. manage_inactivity();
  2823. lcd_update();
  2824. if(cnt==0)
  2825. {
  2826. #if BEEPER > 0
  2827. SET_OUTPUT(BEEPER);
  2828.  
  2829. WRITE(BEEPER,HIGH);
  2830. delay(3);
  2831. WRITE(BEEPER,LOW);
  2832. delay(3);
  2833. #else
  2834. #if !defined(LCD_FEEDBACK_FREQUENCY_HZ) || !defined(LCD_FEEDBACK_FREQUENCY_DURATION_MS)
  2835. lcd_buzz(1000/6,100);
  2836. #else
  2837. lcd_buzz(LCD_FEEDBACK_FREQUENCY_DURATION_MS,LCD_FEEDBACK_FREQUENCY_HZ);
  2838. #endif
  2839. #endif
  2840. }
  2841. }
  2842.  
  2843. //return to normal
  2844. if(code_seen('L'))
  2845. {
  2846. target[E_AXIS]+= -code_value();
  2847. }
  2848. else
  2849. {
  2850. #ifdef FILAMENTCHANGE_FINALRETRACT
  2851. target[E_AXIS]+=(-1)*FILAMENTCHANGE_FINALRETRACT ;
  2852. #endif
  2853. }
  2854. current_position[E_AXIS]=target[E_AXIS]; //the long retract of L is compensated by manual filament feeding
  2855. plan_set_e_position(current_position[E_AXIS]);
  2856. plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], feedrate/60, active_extruder); //should do nothing
  2857. plan_buffer_line(lastpos[X_AXIS], lastpos[Y_AXIS], target[Z_AXIS], target[E_AXIS], feedrate/60, active_extruder); //move xy back
  2858. plan_buffer_line(lastpos[X_AXIS], lastpos[Y_AXIS], lastpos[Z_AXIS], target[E_AXIS], feedrate/60, active_extruder); //move z back
  2859. plan_buffer_line(lastpos[X_AXIS], lastpos[Y_AXIS], lastpos[Z_AXIS], lastpos[E_AXIS], feedrate/60, active_extruder); //final untretract
  2860. }
  2861. break;
  2862. #endif //FILAMENTCHANGEENABLE
  2863. #ifdef DUAL_X_CARRIAGE
  2864. case 605: // Set dual x-carriage movement mode:
  2865. // M605 S0: Full control mode. The slicer has full control over x-carriage movement
  2866. // M605 S1: Auto-park mode. The inactive head will auto park/unpark without slicer involvement
  2867. // M605 S2 [Xnnn] [Rmmm]: Duplication mode. The second extruder will duplicate the first with nnn
  2868. // millimeters x-offset and an optional differential hotend temperature of
  2869. // mmm degrees. E.g., with "M605 S2 X100 R2" the second extruder will duplicate
  2870. // the first with a spacing of 100mm in the x direction and 2 degrees hotter.
  2871. //
  2872. // Note: the X axis should be homed after changing dual x-carriage mode.
  2873. {
  2874. st_synchronize();
  2875.  
  2876. if (code_seen('S'))
  2877. dual_x_carriage_mode = code_value();
  2878.  
  2879. if (dual_x_carriage_mode == DXC_DUPLICATION_MODE)
  2880. {
  2881. if (code_seen('X'))
  2882. duplicate_extruder_x_offset = max(code_value(),X2_MIN_POS - x_home_pos(0));
  2883.  
  2884. if (code_seen('R'))
  2885. duplicate_extruder_temp_offset = code_value();
  2886.  
  2887. SERIAL_ECHO_START;
  2888. SERIAL_ECHOPGM(MSG_HOTEND_OFFSET);
  2889. SERIAL_ECHO(" ");
  2890. SERIAL_ECHO(extruder_offset[X_AXIS][0]);
  2891. SERIAL_ECHO(",");
  2892. SERIAL_ECHO(extruder_offset[Y_AXIS][0]);
  2893. SERIAL_ECHO(" ");
  2894. SERIAL_ECHO(duplicate_extruder_x_offset);
  2895. SERIAL_ECHO(",");
  2896. SERIAL_ECHOLN(extruder_offset[Y_AXIS][1]);
  2897. }
  2898. else if (dual_x_carriage_mode != DXC_FULL_CONTROL_MODE && dual_x_carriage_mode != DXC_AUTO_PARK_MODE)
  2899. {
  2900. dual_x_carriage_mode = DEFAULT_DUAL_X_CARRIAGE_MODE;
  2901. }
  2902.  
  2903. active_extruder_parked = false;
  2904. extruder_duplication_enabled = false;
  2905. delayed_move_time = 0;
  2906. }
  2907. break;
  2908. #endif //DUAL_X_CARRIAGE
  2909.  
  2910. case 907: // M907 Set digital trimpot motor current using axis codes.
  2911. {
  2912. #if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1
  2913. for(int i=0;i<NUM_AXIS;i++) if(code_seen(axis_codes[i])) digipot_current(i,code_value());
  2914. if(code_seen('B')) digipot_current(4,code_value());
  2915. if(code_seen('S')) for(int i=0;i<=4;i++) digipot_current(i,code_value());
  2916. #endif
  2917. #ifdef MOTOR_CURRENT_PWM_XY_PIN
  2918. if(code_seen('X')) digipot_current(0, code_value());
  2919. #endif
  2920. #ifdef MOTOR_CURRENT_PWM_Z_PIN
  2921. if(code_seen('Z')) digipot_current(1, code_value());
  2922. #endif
  2923. #ifdef MOTOR_CURRENT_PWM_E_PIN
  2924. if(code_seen('E')) digipot_current(2, code_value());
  2925. #endif
  2926. #ifdef DIGIPOT_I2C
  2927. // this one uses actual amps in floating point
  2928. for(int i=0;i<NUM_AXIS;i++) if(code_seen(axis_codes[i])) digipot_i2c_set_current(i, code_value());
  2929. // for each additional extruder (named B,C,D,E..., channels 4,5,6,7...)
  2930. for(int i=NUM_AXIS;i<DIGIPOT_I2C_NUM_CHANNELS;i++) if(code_seen('B'+i-NUM_AXIS)) digipot_i2c_set_current(i, code_value());
  2931. #endif
  2932. }
  2933. break;
  2934. case 908: // M908 Control digital trimpot directly.
  2935. {
  2936. #if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1
  2937. uint8_t channel,current;
  2938. if(code_seen('P')) channel=code_value();
  2939. if(code_seen('S')) current=code_value();
  2940. digitalPotWrite(channel, current);
  2941. #endif
  2942. }
  2943. break;
  2944. case 350: // M350 Set microstepping mode. Warning: Steps per unit remains unchanged. S code sets stepping mode for all drivers.
  2945. {
  2946. #if defined(X_MS1_PIN) && X_MS1_PIN > -1
  2947. if(code_seen('S')) for(int i=0;i<=4;i++) microstep_mode(i,code_value());
  2948. for(int i=0;i<NUM_AXIS;i++) if(code_seen(axis_codes[i])) microstep_mode(i,(uint8_t)code_value());
  2949. if(code_seen('B')) microstep_mode(4,code_value());
  2950. microstep_readings();
  2951. #endif
  2952. }
  2953. break;
  2954. case 351: // M351 Toggle MS1 MS2 pins directly, S# determines MS1 or MS2, X# sets the pin high/low.
  2955. {
  2956. #if defined(X_MS1_PIN) && X_MS1_PIN > -1
  2957. if(code_seen('S')) switch((int)code_value())
  2958. {
  2959. case 1:
  2960. for(int i=0;i<NUM_AXIS;i++) if(code_seen(axis_codes[i])) microstep_ms(i,code_value(),-1);
  2961. if(code_seen('B')) microstep_ms(4,code_value(),-1);
  2962. break;
  2963. case 2:
  2964. for(int i=0;i<NUM_AXIS;i++) if(code_seen(axis_codes[i])) microstep_ms(i,-1,code_value());
  2965. if(code_seen('B')) microstep_ms(4,-1,code_value());
  2966. break;
  2967. }
  2968. microstep_readings();
  2969. #endif
  2970. }
  2971. break;
  2972. case 999: // M999: Restart after being stopped
  2973. Stopped = false;
  2974. lcd_reset_alert_level();
  2975. gcode_LastN = Stopped_gcode_LastN;
  2976. FlushSerialRequestResend();
  2977. break;
  2978. }
  2979. }
  2980.  
  2981. else if(code_seen('T'))
  2982. {
  2983. tmp_extruder = code_value();
  2984. if(tmp_extruder >= EXTRUDERS) {
  2985. SERIAL_ECHO_START;
  2986. SERIAL_ECHO("T");
  2987. SERIAL_ECHO(tmp_extruder);
  2988. SERIAL_ECHOLN(MSG_INVALID_EXTRUDER);
  2989. }
  2990. else {
  2991. boolean make_move = false;
  2992. if(code_seen('F')) {
  2993. make_move = true;
  2994. next_feedrate = code_value();
  2995. if(next_feedrate > 0.0) {
  2996. feedrate = next_feedrate;
  2997. }
  2998. }
  2999. #if EXTRUDERS > 1
  3000. if(tmp_extruder != active_extruder) {
  3001. // Save current position to return to after applying extruder offset
  3002. memcpy(destination, current_position, sizeof(destination));
  3003. #ifdef DUAL_X_CARRIAGE
  3004. if (dual_x_carriage_mode == DXC_AUTO_PARK_MODE && Stopped == false &&
  3005. (delayed_move_time != 0 || current_position[X_AXIS] != x_home_pos(active_extruder)))
  3006. {
  3007. // Park old head: 1) raise 2) move to park position 3) lower
  3008. plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + TOOLCHANGE_PARK_ZLIFT,
  3009. current_position[E_AXIS], max_feedrate[Z_AXIS], active_extruder);
  3010. plan_buffer_line(x_home_pos(active_extruder), current_position[Y_AXIS], current_position[Z_AXIS] + TOOLCHANGE_PARK_ZLIFT,
  3011. current_position[E_AXIS], max_feedrate[X_AXIS], active_extruder);
  3012. plan_buffer_line(x_home_pos(active_extruder), current_position[Y_AXIS], current_position[Z_AXIS],
  3013. current_position[E_AXIS], max_feedrate[Z_AXIS], active_extruder);
  3014. st_synchronize();
  3015. }
  3016.  
  3017. // apply Y & Z extruder offset (x offset is already used in determining home pos)
  3018. current_position[Y_AXIS] = current_position[Y_AXIS] -
  3019. extruder_offset[Y_AXIS][active_extruder] +
  3020. extruder_offset[Y_AXIS][tmp_extruder];
  3021. current_position[Z_AXIS] = current_position[Z_AXIS] -
  3022. extruder_offset[Z_AXIS][active_extruder] +
  3023. extruder_offset[Z_AXIS][tmp_extruder];
  3024.  
  3025. active_extruder = tmp_extruder;
  3026.  
  3027. // This function resets the max/min values - the current position may be overwritten below.
  3028. axis_is_at_home(X_AXIS);
  3029.  
  3030. if (dual_x_carriage_mode == DXC_FULL_CONTROL_MODE)
  3031. {
  3032. current_position[X_AXIS] = inactive_extruder_x_pos;
  3033. inactive_extruder_x_pos = destination[X_AXIS];
  3034. }
  3035. else if (dual_x_carriage_mode == DXC_DUPLICATION_MODE)
  3036. {
  3037. active_extruder_parked = (active_extruder == 0); // this triggers the second extruder to move into the duplication position
  3038. if (active_extruder == 0 || active_extruder_parked)
  3039. current_position[X_AXIS] = inactive_extruder_x_pos;
  3040. else
  3041. current_position[X_AXIS] = destination[X_AXIS] + duplicate_extruder_x_offset;
  3042. inactive_extruder_x_pos = destination[X_AXIS];
  3043. extruder_duplication_enabled = false;
  3044. }
  3045. else
  3046. {
  3047. // record raised toolhead position for use by unpark
  3048. memcpy(raised_parked_position, current_position, sizeof(raised_parked_position));
  3049. raised_parked_position[Z_AXIS] += TOOLCHANGE_UNPARK_ZLIFT;
  3050. active_extruder_parked = true;
  3051. delayed_move_time = 0;
  3052. }
  3053. #else
  3054. // Offset extruder (only by XY)
  3055. int i;
  3056. for(i = 0; i < 2; i++) {
  3057. current_position[i] = current_position[i] -
  3058. extruder_offset[i][active_extruder] +
  3059. extruder_offset[i][tmp_extruder];
  3060. }
  3061. // Set the new active extruder and position
  3062. active_extruder = tmp_extruder;
  3063. #endif //else DUAL_X_CARRIAGE
  3064. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  3065. // Move to the old position if 'F' was in the parameters
  3066. if(make_move && Stopped == false) {
  3067. prepare_move();
  3068. }
  3069. }
  3070. #endif
  3071. SERIAL_ECHO_START;
  3072. SERIAL_ECHO(MSG_ACTIVE_EXTRUDER);
  3073. SERIAL_PROTOCOLLN((int)active_extruder);
  3074. }
  3075. }
  3076.  
  3077. else
  3078. {
  3079. SERIAL_ECHO_START;
  3080. SERIAL_ECHOPGM(MSG_UNKNOWN_COMMAND);
  3081. SERIAL_ECHO(cmdbuffer[bufindr]);
  3082. SERIAL_ECHOLNPGM("\"");
  3083. }
  3084.  
  3085. ClearToSend();
  3086. }
  3087.  
  3088. void FlushSerialRequestResend()
  3089. {
  3090. //char cmdbuffer[bufindr][100]="Resend:";
  3091. MYSERIAL.flush();
  3092. SERIAL_PROTOCOLPGM(MSG_RESEND);
  3093. SERIAL_PROTOCOLLN(gcode_LastN + 1);
  3094. ClearToSend();
  3095. }
  3096.  
  3097. void ClearToSend()
  3098. {
  3099. previous_millis_cmd = millis();
  3100. #ifdef SDSUPPORT
  3101. if(fromsd[bufindr])
  3102. return;
  3103. #endif //SDSUPPORT
  3104. SERIAL_PROTOCOLLNPGM(MSG_OK);
  3105. }
  3106.  
  3107. void get_coordinates()
  3108. {
  3109. bool seen[4]={false,false,false,false};
  3110. for(int8_t i=0; i < NUM_AXIS; i++) {
  3111. if(code_seen(axis_codes[i]))
  3112. {
  3113. destination[i] = (float)code_value() + (axis_relative_modes[i] || relative_mode)*current_position[i];
  3114. seen[i]=true;
  3115. }
  3116. else destination[i] = current_position[i]; //Are these else lines really needed?
  3117. }
  3118. if(code_seen('F')) {
  3119. next_feedrate = code_value();
  3120. if(next_feedrate > 0.0) feedrate = next_feedrate;
  3121. }
  3122. }
  3123.  
  3124. void get_arc_coordinates()
  3125. {
  3126. #ifdef SF_ARC_FIX
  3127. bool relative_mode_backup = relative_mode;
  3128. relative_mode = true;
  3129. #endif
  3130. get_coordinates();
  3131. #ifdef SF_ARC_FIX
  3132. relative_mode=relative_mode_backup;
  3133. #endif
  3134.  
  3135. if(code_seen('I')) {
  3136. offset[0] = code_value();
  3137. }
  3138. else {
  3139. offset[0] = 0.0;
  3140. }
  3141. if(code_seen('J')) {
  3142. offset[1] = code_value();
  3143. }
  3144. else {
  3145. offset[1] = 0.0;
  3146. }
  3147. }
  3148.  
  3149. void clamp_to_software_endstops(float target[3])
  3150. {
  3151. if (min_software_endstops) {
  3152. if (target[X_AXIS] < min_pos[X_AXIS]) target[X_AXIS] = min_pos[X_AXIS];
  3153. if (target[Y_AXIS] < min_pos[Y_AXIS]) target[Y_AXIS] = min_pos[Y_AXIS];
  3154. if (target[Z_AXIS] < min_pos[Z_AXIS]) target[Z_AXIS] = min_pos[Z_AXIS];
  3155. }
  3156.  
  3157. if (max_software_endstops) {
  3158. if (target[X_AXIS] > max_pos[X_AXIS]) target[X_AXIS] = max_pos[X_AXIS];
  3159. if (target[Y_AXIS] > max_pos[Y_AXIS]) target[Y_AXIS] = max_pos[Y_AXIS];
  3160. if (target[Z_AXIS] > max_pos[Z_AXIS]) target[Z_AXIS] = max_pos[Z_AXIS];
  3161. }
  3162. }
  3163.  
  3164. #ifdef DELTA
  3165. void recalc_delta_settings(float radius, float diagonal_rod)
  3166. {
  3167. delta_tower1_x= -SIN_60*radius; // front left tower
  3168. delta_tower1_y= -COS_60*radius;
  3169. delta_tower2_x= SIN_60*radius; // front right tower
  3170. delta_tower2_y= -COS_60*radius;
  3171. delta_tower3_x= 0.0; // back middle tower
  3172. delta_tower3_y= radius;
  3173. delta_diagonal_rod_2= sq(diagonal_rod);
  3174. }
  3175.  
  3176. void calculate_delta(float cartesian[3])
  3177. {
  3178. delta[X_AXIS] = sqrt(delta_diagonal_rod_2
  3179. - sq(delta_tower1_x-cartesian[X_AXIS])
  3180. - sq(delta_tower1_y-cartesian[Y_AXIS])
  3181. ) + cartesian[Z_AXIS];
  3182. delta[Y_AXIS] = sqrt(delta_diagonal_rod_2
  3183. - sq(delta_tower2_x-cartesian[X_AXIS])
  3184. - sq(delta_tower2_y-cartesian[Y_AXIS])
  3185. ) + cartesian[Z_AXIS];
  3186. delta[Z_AXIS] = sqrt(delta_diagonal_rod_2
  3187. - sq(delta_tower3_x-cartesian[X_AXIS])
  3188. - sq(delta_tower3_y-cartesian[Y_AXIS])
  3189. ) + cartesian[Z_AXIS];
  3190. /*
  3191. SERIAL_ECHOPGM("cartesian x="); SERIAL_ECHO(cartesian[X_AXIS]);
  3192. SERIAL_ECHOPGM(" y="); SERIAL_ECHO(cartesian[Y_AXIS]);
  3193. SERIAL_ECHOPGM(" z="); SERIAL_ECHOLN(cartesian[Z_AXIS]);
  3194.  
  3195. SERIAL_ECHOPGM("delta x="); SERIAL_ECHO(delta[X_AXIS]);
  3196. SERIAL_ECHOPGM(" y="); SERIAL_ECHO(delta[Y_AXIS]);
  3197. SERIAL_ECHOPGM(" z="); SERIAL_ECHOLN(delta[Z_AXIS]);
  3198. */
  3199. }
  3200. #endif
  3201.  
  3202. void prepare_move()
  3203. {
  3204. clamp_to_software_endstops(destination);
  3205.  
  3206. previous_millis_cmd = millis();
  3207. #ifdef DELTA
  3208. float difference[NUM_AXIS];
  3209. for (int8_t i=0; i < NUM_AXIS; i++) {
  3210. difference[i] = destination[i] - current_position[i];
  3211. }
  3212. float cartesian_mm = sqrt(sq(difference[X_AXIS]) +
  3213. sq(difference[Y_AXIS]) +
  3214. sq(difference[Z_AXIS]));
  3215. if (cartesian_mm < 0.000001) { cartesian_mm = abs(difference[E_AXIS]); }
  3216. if (cartesian_mm < 0.000001) { return; }
  3217. float seconds = 6000 * cartesian_mm / feedrate / feedmultiply;
  3218. int steps = max(1, int(delta_segments_per_second * seconds));
  3219. // SERIAL_ECHOPGM("mm="); SERIAL_ECHO(cartesian_mm);
  3220. // SERIAL_ECHOPGM(" seconds="); SERIAL_ECHO(seconds);
  3221. // SERIAL_ECHOPGM(" steps="); SERIAL_ECHOLN(steps);
  3222. for (int s = 1; s <= steps; s++) {
  3223. float fraction = float(s) / float(steps);
  3224. for(int8_t i=0; i < NUM_AXIS; i++) {
  3225. destination[i] = current_position[i] + difference[i] * fraction;
  3226. }
  3227. calculate_delta(destination);
  3228. plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],
  3229. destination[E_AXIS], feedrate*feedmultiply/60/100.0,
  3230. active_extruder);
  3231. }
  3232. #else
  3233.  
  3234. #ifdef DUAL_X_CARRIAGE
  3235. if (active_extruder_parked)
  3236. {
  3237. if (dual_x_carriage_mode == DXC_DUPLICATION_MODE && active_extruder == 0)
  3238. {
  3239. // move duplicate extruder into correct duplication position.
  3240. plan_set_position(inactive_extruder_x_pos, current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  3241. plan_buffer_line(current_position[X_AXIS] + duplicate_extruder_x_offset, current_position[Y_AXIS], current_position[Z_AXIS],
  3242. current_position[E_AXIS], max_feedrate[X_AXIS], 1);
  3243. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  3244. st_synchronize();
  3245. extruder_duplication_enabled = true;
  3246. active_extruder_parked = false;
  3247. }
  3248. else if (dual_x_carriage_mode == DXC_AUTO_PARK_MODE) // handle unparking of head
  3249. {
  3250. if (current_position[E_AXIS] == destination[E_AXIS])
  3251. {
  3252. // this is a travel move - skit it but keep track of current position (so that it can later
  3253. // be used as start of first non-travel move)
  3254. if (delayed_move_time != 0xFFFFFFFFUL)
  3255. {
  3256. memcpy(current_position, destination, sizeof(current_position));
  3257. if (destination[Z_AXIS] > raised_parked_position[Z_AXIS])
  3258. raised_parked_position[Z_AXIS] = destination[Z_AXIS];
  3259. delayed_move_time = millis();
  3260. return;
  3261. }
  3262. }
  3263. delayed_move_time = 0;
  3264. // unpark extruder: 1) raise, 2) move into starting XY position, 3) lower
  3265. plan_buffer_line(raised_parked_position[X_AXIS], raised_parked_position[Y_AXIS], raised_parked_position[Z_AXIS], current_position[E_AXIS], max_feedrate[Z_AXIS], active_extruder);
  3266. plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], raised_parked_position[Z_AXIS],
  3267. current_position[E_AXIS], min(max_feedrate[X_AXIS],max_feedrate[Y_AXIS]), active_extruder);
  3268. plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS],
  3269. current_position[E_AXIS], max_feedrate[Z_AXIS], active_extruder);
  3270. active_extruder_parked = false;
  3271. }
  3272. }
  3273. #endif //DUAL_X_CARRIAGE
  3274.  
  3275. // Do not use feedmultiply for E or Z only moves
  3276. if( (current_position[X_AXIS] == destination [X_AXIS]) && (current_position[Y_AXIS] == destination [Y_AXIS])) {
  3277. plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
  3278. }
  3279. else {
  3280. plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder);
  3281. }
  3282. #endif //else DELTA
  3283. for(int8_t i=0; i < NUM_AXIS; i++) {
  3284. current_position[i] = destination[i];
  3285. }
  3286. }
  3287.  
  3288. void prepare_arc_move(char isclockwise) {
  3289. float r = hypot(offset[X_AXIS], offset[Y_AXIS]); // Compute arc radius for mc_arc
  3290.  
  3291. // Trace the arc
  3292. mc_arc(current_position, destination, offset, X_AXIS, Y_AXIS, Z_AXIS, feedrate*feedmultiply/60/100.0, r, isclockwise, active_extruder);
  3293.  
  3294. // As far as the parser is concerned, the position is now == target. In reality the
  3295. // motion control system might still be processing the action and the real tool position
  3296. // in any intermediate location.
  3297. for(int8_t i=0; i < NUM_AXIS; i++) {
  3298. current_position[i] = destination[i];
  3299. }
  3300. previous_millis_cmd = millis();
  3301. }
  3302.  
  3303. #if defined(CONTROLLERFAN_PIN) && CONTROLLERFAN_PIN > -1
  3304.  
  3305. #if defined(FAN_PIN)
  3306. #if CONTROLLERFAN_PIN == FAN_PIN
  3307. #error "You cannot set CONTROLLERFAN_PIN equal to FAN_PIN"
  3308. #endif
  3309. #endif
  3310.  
  3311. unsigned long lastMotor = 0; //Save the time for when a motor was turned on last
  3312. unsigned long lastMotorCheck = 0;
  3313.  
  3314. void controllerFan()
  3315. {
  3316. if ((millis() - lastMotorCheck) >= 2500) //Not a time critical function, so we only check every 2500ms
  3317. {
  3318. lastMotorCheck = millis();
  3319.  
  3320. if(!READ(X_ENABLE_PIN) || !READ(Y_ENABLE_PIN) || !READ(Z_ENABLE_PIN) || (soft_pwm_bed > 0)
  3321. #if EXTRUDERS > 2
  3322. || !READ(E2_ENABLE_PIN)
  3323. #endif
  3324. #if EXTRUDER > 1
  3325. #if defined(X2_ENABLE_PIN) && X2_ENABLE_PIN > -1
  3326. || !READ(X2_ENABLE_PIN)
  3327. #endif
  3328. || !READ(E1_ENABLE_PIN)
  3329. #endif
  3330. || !READ(E0_ENABLE_PIN)) //If any of the drivers are enabled...
  3331. {
  3332. lastMotor = millis(); //... set time to NOW so the fan will turn on
  3333. }
  3334.  
  3335. if ((millis() - lastMotor) >= (CONTROLLERFAN_SECS*1000UL) || lastMotor == 0) //If the last time any driver was enabled, is longer since than CONTROLLERSEC...
  3336. {
  3337. digitalWrite(CONTROLLERFAN_PIN, 0);
  3338. analogWrite(CONTROLLERFAN_PIN, 0);
  3339. }
  3340. else
  3341. {
  3342. // allows digital or PWM fan output to be used (see M42 handling)
  3343. digitalWrite(CONTROLLERFAN_PIN, CONTROLLERFAN_SPEED);
  3344. analogWrite(CONTROLLERFAN_PIN, CONTROLLERFAN_SPEED);
  3345. }
  3346. }
  3347. }
  3348. #endif
  3349.  
  3350. #ifdef TEMP_STAT_LEDS
  3351. static bool blue_led = false;
  3352. static bool red_led = false;
  3353. static uint32_t stat_update = 0;
  3354.  
  3355. void handle_status_leds(void) {
  3356. float max_temp = 0.0;
  3357. if(millis() > stat_update) {
  3358. stat_update += 500; // Update every 0.5s
  3359. for (int8_t cur_extruder = 0; cur_extruder < EXTRUDERS; ++cur_extruder) {
  3360. max_temp = max(max_temp, degHotend(cur_extruder));
  3361. max_temp = max(max_temp, degTargetHotend(cur_extruder));
  3362. }
  3363. #if defined(TEMP_BED_PIN) && TEMP_BED_PIN > -1
  3364. max_temp = max(max_temp, degTargetBed());
  3365. max_temp = max(max_temp, degBed());
  3366. #endif
  3367. if((max_temp > 55.0) && (red_led == false)) {
  3368. digitalWrite(STAT_LED_RED, 1);
  3369. digitalWrite(STAT_LED_BLUE, 0);
  3370. red_led = true;
  3371. blue_led = false;
  3372. }
  3373. if((max_temp < 54.0) && (blue_led == false)) {
  3374. digitalWrite(STAT_LED_RED, 0);
  3375. digitalWrite(STAT_LED_BLUE, 1);
  3376. red_led = false;
  3377. blue_led = true;
  3378. }
  3379. }
  3380. }
  3381. #endif
  3382.  
  3383. void manage_inactivity()
  3384. {
  3385. if( (millis() - previous_millis_cmd) > max_inactive_time )
  3386. if(max_inactive_time)
  3387. kill();
  3388. if(stepper_inactive_time) {
  3389. if( (millis() - previous_millis_cmd) > stepper_inactive_time )
  3390. {
  3391. if(blocks_queued() == false) {
  3392. disable_x();
  3393. disable_y();
  3394. disable_z();
  3395. disable_e0();
  3396. disable_e1();
  3397. disable_e2();
  3398. }
  3399. }
  3400. }
  3401.  
  3402. #ifdef CHDK //Check if pin should be set to LOW after M240 set it to HIGH
  3403. if (chdkActive)
  3404. {
  3405. chdkActive = false;
  3406. if (millis()-chdkHigh < CHDK_DELAY) return;
  3407. WRITE(CHDK, LOW);
  3408. }
  3409. #endif
  3410.  
  3411. #if defined(KILL_PIN) && KILL_PIN > -1
  3412. if( 0 == READ(KILL_PIN) )
  3413. kill();
  3414. #endif
  3415. #if defined(CONTROLLERFAN_PIN) && CONTROLLERFAN_PIN > -1
  3416. controllerFan(); //Check if fan should be turned on to cool stepper drivers down
  3417. #endif
  3418. #ifdef EXTRUDER_RUNOUT_PREVENT
  3419. if( (millis() - previous_millis_cmd) > EXTRUDER_RUNOUT_SECONDS*1000 )
  3420. if(degHotend(active_extruder)>EXTRUDER_RUNOUT_MINTEMP)
  3421. {
  3422. bool oldstatus=READ(E0_ENABLE_PIN);
  3423. enable_e0();
  3424. float oldepos=current_position[E_AXIS];
  3425. float oldedes=destination[E_AXIS];
  3426. plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS],
  3427. destination[E_AXIS]+EXTRUDER_RUNOUT_EXTRUDE*EXTRUDER_RUNOUT_ESTEPS/axis_steps_per_unit[E_AXIS],
  3428. EXTRUDER_RUNOUT_SPEED/60.*EXTRUDER_RUNOUT_ESTEPS/axis_steps_per_unit[E_AXIS], active_extruder);
  3429. current_position[E_AXIS]=oldepos;
  3430. destination[E_AXIS]=oldedes;
  3431. plan_set_e_position(oldepos);
  3432. previous_millis_cmd=millis();
  3433. st_synchronize();
  3434. WRITE(E0_ENABLE_PIN,oldstatus);
  3435. }
  3436. #endif
  3437. #if defined(DUAL_X_CARRIAGE)
  3438. // handle delayed move timeout
  3439. if (delayed_move_time != 0 && (millis() - delayed_move_time) > 1000 && Stopped == false)
  3440. {
  3441. // travel moves have been received so enact them
  3442. delayed_move_time = 0xFFFFFFFFUL; // force moves to be done
  3443. memcpy(destination,current_position,sizeof(destination));
  3444. prepare_move();
  3445. }
  3446. #endif
  3447. #ifdef TEMP_STAT_LEDS
  3448. handle_status_leds();
  3449. #endif
  3450. check_axes_activity();
  3451. }
  3452.  
  3453. void kill()
  3454. {
  3455. cli(); // Stop interrupts
  3456. disable_heater();
  3457.  
  3458. disable_x();
  3459. disable_y();
  3460. disable_z();
  3461. disable_e0();
  3462. disable_e1();
  3463. disable_e2();
  3464.  
  3465. #if defined(PS_ON_PIN) && PS_ON_PIN > -1
  3466. pinMode(PS_ON_PIN,INPUT);
  3467. #endif
  3468. SERIAL_ERROR_START;
  3469. SERIAL_ERRORLNPGM(MSG_ERR_KILLED);
  3470. LCD_ALERTMESSAGEPGM(MSG_KILLED);
  3471. suicide();
  3472. while(1) { /* Intentionally left empty */ } // Wait for reset
  3473. }
  3474.  
  3475. void Stop()
  3476. {
  3477. disable_heater();
  3478. if(Stopped == false) {
  3479. Stopped = true;
  3480. Stopped_gcode_LastN = gcode_LastN; // Save last g_code for restart
  3481. SERIAL_ERROR_START;
  3482. SERIAL_ERRORLNPGM(MSG_ERR_STOPPED);
  3483. LCD_MESSAGEPGM(MSG_STOPPED);
  3484. }
  3485. }
  3486.  
  3487. bool IsStopped() { return Stopped; };
  3488.  
  3489. #ifdef FAST_PWM_FAN
  3490. void setPwmFrequency(uint8_t pin, int val)
  3491. {
  3492. val &= 0x07;
  3493. switch(digitalPinToTimer(pin))
  3494. {
  3495.  
  3496. #if defined(TCCR0A)
  3497. case TIMER0A:
  3498. case TIMER0B:
  3499. // TCCR0B &= ~(_BV(CS00) | _BV(CS01) | _BV(CS02));
  3500. // TCCR0B |= val;
  3501. break;
  3502. #endif
  3503.  
  3504. #if defined(TCCR1A)
  3505. case TIMER1A:
  3506. case TIMER1B:
  3507. // TCCR1B &= ~(_BV(CS10) | _BV(CS11) | _BV(CS12));
  3508. // TCCR1B |= val;
  3509. break;
  3510. #endif
  3511.  
  3512. #if defined(TCCR2)
  3513. case TIMER2:
  3514. case TIMER2:
  3515. TCCR2 &= ~(_BV(CS10) | _BV(CS11) | _BV(CS12));
  3516. TCCR2 |= val;
  3517. break;
  3518. #endif
  3519.  
  3520. #if defined(TCCR2A)
  3521. case TIMER2A:
  3522. case TIMER2B:
  3523. TCCR2B &= ~(_BV(CS20) | _BV(CS21) | _BV(CS22));
  3524. TCCR2B |= val;
  3525. break;
  3526. #endif
  3527.  
  3528. #if defined(TCCR3A)
  3529. case TIMER3A:
  3530. case TIMER3B:
  3531. case TIMER3C:
  3532. TCCR3B &= ~(_BV(CS30) | _BV(CS31) | _BV(CS32));
  3533. TCCR3B |= val;
  3534. break;
  3535. #endif
  3536.  
  3537. #if defined(TCCR4A)
  3538. case TIMER4A:
  3539. case TIMER4B:
  3540. case TIMER4C:
  3541. TCCR4B &= ~(_BV(CS40) | _BV(CS41) | _BV(CS42));
  3542. TCCR4B |= val;
  3543. break;
  3544. #endif
  3545.  
  3546. #if defined(TCCR5A)
  3547. case TIMER5A:
  3548. case TIMER5B:
  3549. case TIMER5C:
  3550. TCCR5B &= ~(_BV(CS50) | _BV(CS51) | _BV(CS52));
  3551. TCCR5B |= val;
  3552. break;
  3553. #endif
  3554.  
  3555. }
  3556. }
  3557. #endif //FAST_PWM_FAN
  3558.  
  3559. bool setTargetedHotend(int code){
  3560. tmp_extruder = active_extruder;
  3561. if(code_seen('T')) {
  3562. tmp_extruder = code_value();
  3563. if(tmp_extruder >= EXTRUDERS) {
  3564. SERIAL_ECHO_START;
  3565. switch(code){
  3566. case 104:
  3567. SERIAL_ECHO(MSG_M104_INVALID_EXTRUDER);
  3568. break;
  3569. case 105:
  3570. SERIAL_ECHO(MSG_M105_INVALID_EXTRUDER);
  3571. break;
  3572. case 109:
  3573. SERIAL_ECHO(MSG_M109_INVALID_EXTRUDER);
  3574. break;
  3575. case 218:
  3576. SERIAL_ECHO(MSG_M218_INVALID_EXTRUDER);
  3577. break;
  3578. case 221:
  3579. SERIAL_ECHO(MSG_M221_INVALID_EXTRUDER);
  3580. break;
  3581. }
  3582. SERIAL_ECHOLN(tmp_extruder);
  3583. return true;
  3584. }
  3585. }
  3586. return false;
  3587. }
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!