My Marlin configs for Fabrikator Mini and CTC i3 Pro B
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

Marlin.pde 62KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353135413551356135713581359136013611362136313641365136613671368136913701371137213731374137513761377137813791380138113821383138413851386138713881389139013911392139313941395139613971398139914001401140214031404140514061407140814091410141114121413141414151416141714181419142014211422142314241425142614271428142914301431143214331434143514361437143814391440144114421443144414451446144714481449145014511452145314541455145614571458145914601461146214631464146514661467146814691470147114721473147414751476147714781479148014811482148314841485148614871488148914901491149214931494149514961497149814991500150115021503150415051506150715081509151015111512151315141515151615171518151915201521152215231524152515261527152815291530153115321533153415351536153715381539154015411542154315441545154615471548154915501551155215531554155515561557155815591560156115621563156415651566156715681569157015711572157315741575157615771578157915801581158215831584158515861587158815891590159115921593159415951596159715981599160016011602160316041605160616071608160916101611161216131614161516161617161816191620162116221623162416251626162716281629163016311632163316341635163616371638163916401641164216431644164516461647164816491650165116521653165416551656165716581659166016611662166316641665166616671668166916701671167216731674167516761677167816791680168116821683168416851686168716881689169016911692169316941695169616971698169917001701170217031704170517061707170817091710171117121713171417151716171717181719172017211722172317241725172617271728172917301731173217331734173517361737173817391740174117421743174417451746174717481749175017511752175317541755175617571758175917601761176217631764176517661767176817691770177117721773177417751776177717781779178017811782178317841785178617871788178917901791179217931794179517961797179817991800180118021803180418051806180718081809181018111812181318141815181618171818181918201821182218231824182518261827182818291830183118321833183418351836183718381839184018411842184318441845184618471848184918501851185218531854185518561857185818591860186118621863186418651866186718681869187018711872187318741875187618771878187918801881188218831884188518861887188818891890189118921893189418951896189718981899190019011902190319041905190619071908190919101911191219131914191519161917191819191920192119221923192419251926192719281929193019311932193319341935193619371938193919401941194219431944194519461947194819491950195119521953195419551956195719581959196019611962196319641965196619671968196919701971197219731974197519761977197819791980198119821983198419851986198719881989199019911992199319941995199619971998199920002001200220032004200520062007200820092010201120122013201420152016201720182019202020212022202320242025202620272028202920302031203220332034203520362037203820392040204120422043204420452046
  1. /*
  2. Reprap firmware based on Sprinter and grbl.
  3. Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
  4. This program is free software: you can redistribute it and/or modify
  5. it under the terms of the GNU General Public License as published by
  6. the Free Software Foundation, either version 3 of the License, or
  7. (at your option) any later version.
  8. This program is distributed in the hope that it will be useful,
  9. but WITHOUT ANY WARRANTY; without even the implied warranty of
  10. MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  11. GNU General Public License for more details.
  12. You should have received a copy of the GNU General Public License
  13. along with this program. If not, see <http://www.gnu.org/licenses/>.
  14. */
  15. /*
  16. This firmware is a mashup between Sprinter and grbl.
  17. (https://github.com/kliment/Sprinter)
  18. (https://github.com/simen/grbl/tree)
  19. It has preliminary support for Matthew Roberts advance algorithm
  20. http://reprap.org/pipermail/reprap-dev/2011-May/003323.html
  21. This firmware is optimized for gen6 electronics.
  22. */
  23. #include "fastio.h"
  24. #include "Configuration.h"
  25. #include "pins.h"
  26. #include "Marlin.h"
  27. #include "speed_lookuptable.h"
  28. char version_string[] = "0.9.8";
  29. #ifdef SDSUPPORT
  30. #include "SdFat.h"
  31. #endif //SDSUPPORT
  32. #ifndef CRITICAL_SECTION_START
  33. #define CRITICAL_SECTION_START unsigned char _sreg = SREG; cli()
  34. #define CRITICAL_SECTION_END SREG = _sreg
  35. #endif //CRITICAL_SECTION_START
  36. // look here for descriptions of gcodes: http://linuxcnc.org/handbook/gcode/g-code.html
  37. // http://objects.reprap.org/wiki/Mendel_User_Manual:_RepRapGCodes
  38. //Implemented Codes
  39. //-------------------
  40. // G0 -> G1
  41. // G1 - Coordinated Movement X Y Z E
  42. // G4 - Dwell S<seconds> or P<milliseconds>
  43. // G28 - Home all Axis
  44. // G90 - Use Absolute Coordinates
  45. // G91 - Use Relative Coordinates
  46. // G92 - Set current position to cordinates given
  47. //RepRap M Codes
  48. // M104 - Set extruder target temp
  49. // M105 - Read current temp
  50. // M106 - Fan on
  51. // M107 - Fan off
  52. // M109 - Wait for extruder current temp to reach target temp.
  53. // M114 - Display current position
  54. //Custom M Codes
  55. // M80 - Turn on Power Supply
  56. // M20 - List SD card
  57. // M21 - Init SD card
  58. // M22 - Release SD card
  59. // M23 - Select SD file (M23 filename.g)
  60. // M24 - Start/resume SD print
  61. // M25 - Pause SD print
  62. // M26 - Set SD position in bytes (M26 S12345)
  63. // M27 - Report SD print status
  64. // M28 - Start SD write (M28 filename.g)
  65. // M29 - Stop SD write
  66. // M81 - Turn off Power Supply
  67. // M82 - Set E codes absolute (default)
  68. // M83 - Set E codes relative while in Absolute Coordinates (G90) mode
  69. // M84 - Disable steppers until next move,
  70. // or use S<seconds> to specify an inactivity timeout, after which the steppers will be disabled. S0 to disable the timeout.
  71. // M85 - Set inactivity shutdown timer with parameter S<seconds>. To disable set zero (default)
  72. // M92 - Set axis_steps_per_unit - same syntax as G92
  73. // M115 - Capabilities string
  74. // M140 - Set bed target temp
  75. // M190 - Wait for bed current temp to reach target temp.
  76. // M201 - Set max acceleration in units/s^2 for print moves (M201 X1000 Y1000)
  77. // M202 - Set max acceleration in units/s^2 for travel moves (M202 X1000 Y1000)
  78. // M301 - Set PID parameters P I and D
  79. //Stepper Movement Variables
  80. char axis_codes[NUM_AXIS] = {
  81. 'X', 'Y', 'Z', 'E'};
  82. float destination[NUM_AXIS] = {
  83. 0.0, 0.0, 0.0, 0.0};
  84. float current_position[NUM_AXIS] = {
  85. 0.0, 0.0, 0.0, 0.0};
  86. bool home_all_axis = true;
  87. long feedrate = 1500, next_feedrate, saved_feedrate;
  88. long gcode_N, gcode_LastN;
  89. bool relative_mode = false; //Determines Absolute or Relative Coordinates
  90. bool relative_mode_e = false; //Determines Absolute or Relative E Codes while in Absolute Coordinates mode. E is always relative in Relative Coordinates mode.
  91. unsigned long axis_steps_per_sqr_second[NUM_AXIS];
  92. // comm variables
  93. #define MAX_CMD_SIZE 96
  94. #define BUFSIZE 8
  95. char cmdbuffer[BUFSIZE][MAX_CMD_SIZE];
  96. bool fromsd[BUFSIZE];
  97. int bufindr = 0;
  98. int bufindw = 0;
  99. int buflen = 0;
  100. int i = 0;
  101. char serial_char;
  102. int serial_count = 0;
  103. boolean comment_mode = false;
  104. char *strchr_pointer; // just a pointer to find chars in the cmd string like X, Y, Z, E, etc
  105. // Manage heater variables.
  106. int target_raw = 0;
  107. int current_raw = 0;
  108. unsigned char temp_meas_ready = false;
  109. #ifdef PIDTEMP
  110. double temp_iState = 0;
  111. double temp_dState = 0;
  112. double pTerm;
  113. double iTerm;
  114. double dTerm;
  115. //int output;
  116. double pid_error;
  117. double temp_iState_min;
  118. double temp_iState_max;
  119. double pid_setpoint = 0.0;
  120. double pid_input;
  121. double pid_output;
  122. bool pid_reset;
  123. #endif //PIDTEMP
  124. #ifdef WATCHPERIOD
  125. int watch_raw = -1000;
  126. unsigned long watchmillis = 0;
  127. #endif //WATCHPERIOD
  128. #ifdef MINTEMP
  129. int minttemp = temp2analogh(MINTEMP);
  130. #endif //MINTEMP
  131. #ifdef MAXTEMP
  132. int maxttemp = temp2analogh(MAXTEMP);
  133. #endif //MAXTEMP
  134. //Inactivity shutdown variables
  135. unsigned long previous_millis_cmd = 0;
  136. unsigned long max_inactive_time = 0;
  137. unsigned long stepper_inactive_time = 0;
  138. #ifdef SDSUPPORT
  139. Sd2Card card;
  140. SdVolume volume;
  141. SdFile root;
  142. SdFile file;
  143. uint32_t filesize = 0;
  144. uint32_t sdpos = 0;
  145. bool sdmode = false;
  146. bool sdactive = false;
  147. bool savetosd = false;
  148. int16_t n;
  149. void initsd(){
  150. sdactive = false;
  151. #if SDSS >- 1
  152. if(root.isOpen())
  153. root.close();
  154. if (!card.init(SPI_FULL_SPEED,SDSS)){
  155. //if (!card.init(SPI_HALF_SPEED,SDSS))
  156. Serial.println("SD init fail");
  157. }
  158. else if (!volume.init(&card))
  159. Serial.println("volume.init failed");
  160. else if (!root.openRoot(&volume))
  161. Serial.println("openRoot failed");
  162. else
  163. sdactive = true;
  164. #endif //SDSS
  165. }
  166. inline void write_command(char *buf){
  167. char* begin = buf;
  168. char* npos = 0;
  169. char* end = buf + strlen(buf) - 1;
  170. file.writeError = false;
  171. if((npos = strchr(buf, 'N')) != NULL){
  172. begin = strchr(npos, ' ') + 1;
  173. end = strchr(npos, '*') - 1;
  174. }
  175. end[1] = '\r';
  176. end[2] = '\n';
  177. end[3] = '\0';
  178. //Serial.println(begin);
  179. file.write(begin);
  180. if (file.writeError){
  181. Serial.println("error writing to file");
  182. }
  183. }
  184. #endif //SDSUPPORT
  185. void setup()
  186. {
  187. Serial.begin(BAUDRATE);
  188. Serial.print("Marlin ");
  189. Serial.println(version_string);
  190. Serial.println("start");
  191. for(int i = 0; i < BUFSIZE; i++){
  192. fromsd[i] = false;
  193. }
  194. //Initialize Dir Pins
  195. #if X_DIR_PIN > -1
  196. SET_OUTPUT(X_DIR_PIN);
  197. #endif
  198. #if Y_DIR_PIN > -1
  199. SET_OUTPUT(Y_DIR_PIN);
  200. #endif
  201. #if Z_DIR_PIN > -1
  202. SET_OUTPUT(Z_DIR_PIN);
  203. #endif
  204. #if E_DIR_PIN > -1
  205. SET_OUTPUT(E_DIR_PIN);
  206. #endif
  207. //Initialize Enable Pins - steppers default to disabled.
  208. #if (X_ENABLE_PIN > -1)
  209. SET_OUTPUT(X_ENABLE_PIN);
  210. if(!X_ENABLE_ON) WRITE(X_ENABLE_PIN,HIGH);
  211. #endif
  212. #if (Y_ENABLE_PIN > -1)
  213. SET_OUTPUT(Y_ENABLE_PIN);
  214. if(!Y_ENABLE_ON) WRITE(Y_ENABLE_PIN,HIGH);
  215. #endif
  216. #if (Z_ENABLE_PIN > -1)
  217. SET_OUTPUT(Z_ENABLE_PIN);
  218. if(!Z_ENABLE_ON) WRITE(Z_ENABLE_PIN,HIGH);
  219. #endif
  220. #if (E_ENABLE_PIN > -1)
  221. SET_OUTPUT(E_ENABLE_PIN);
  222. if(!E_ENABLE_ON) WRITE(E_ENABLE_PIN,HIGH);
  223. #endif
  224. //endstops and pullups
  225. #ifdef ENDSTOPPULLUPS
  226. #if X_MIN_PIN > -1
  227. SET_INPUT(X_MIN_PIN);
  228. WRITE(X_MIN_PIN,HIGH);
  229. #endif
  230. #if X_MAX_PIN > -1
  231. SET_INPUT(X_MAX_PIN);
  232. WRITE(X_MAX_PIN,HIGH);
  233. #endif
  234. #if Y_MIN_PIN > -1
  235. SET_INPUT(Y_MIN_PIN);
  236. WRITE(Y_MIN_PIN,HIGH);
  237. #endif
  238. #if Y_MAX_PIN > -1
  239. SET_INPUT(Y_MAX_PIN);
  240. WRITE(Y_MAX_PIN,HIGH);
  241. #endif
  242. #if Z_MIN_PIN > -1
  243. SET_INPUT(Z_MIN_PIN);
  244. WRITE(Z_MIN_PIN,HIGH);
  245. #endif
  246. #if Z_MAX_PIN > -1
  247. SET_INPUT(Z_MAX_PIN);
  248. WRITE(Z_MAX_PIN,HIGH);
  249. #endif
  250. #else //ENDSTOPPULLUPS
  251. #if X_MIN_PIN > -1
  252. SET_INPUT(X_MIN_PIN);
  253. #endif
  254. #if X_MAX_PIN > -1
  255. SET_INPUT(X_MAX_PIN);
  256. #endif
  257. #if Y_MIN_PIN > -1
  258. SET_INPUT(Y_MIN_PIN);
  259. #endif
  260. #if Y_MAX_PIN > -1
  261. SET_INPUT(Y_MAX_PIN);
  262. #endif
  263. #if Z_MIN_PIN > -1
  264. SET_INPUT(Z_MIN_PIN);
  265. #endif
  266. #if Z_MAX_PIN > -1
  267. SET_INPUT(Z_MAX_PIN);
  268. #endif
  269. #endif //ENDSTOPPULLUPS
  270. #if (HEATER_0_PIN > -1)
  271. SET_OUTPUT(HEATER_0_PIN);
  272. #endif
  273. #if (HEATER_1_PIN > -1)
  274. SET_OUTPUT(HEATER_1_PIN);
  275. #endif
  276. //Initialize Step Pins
  277. #if (X_STEP_PIN > -1)
  278. SET_OUTPUT(X_STEP_PIN);
  279. #endif
  280. #if (Y_STEP_PIN > -1)
  281. SET_OUTPUT(Y_STEP_PIN);
  282. #endif
  283. #if (Z_STEP_PIN > -1)
  284. SET_OUTPUT(Z_STEP_PIN);
  285. #endif
  286. #if (E_STEP_PIN > -1)
  287. SET_OUTPUT(E_STEP_PIN);
  288. #endif
  289. for(int i=0; i < NUM_AXIS; i++){
  290. axis_steps_per_sqr_second[i] = max_acceleration_units_per_sq_second[i] * axis_steps_per_unit[i];
  291. }
  292. #ifdef PIDTEMP
  293. temp_iState_min = 0.0;
  294. temp_iState_max = PID_INTEGRAL_DRIVE_MAX / Ki;
  295. #endif //PIDTEMP
  296. #ifdef SDSUPPORT
  297. //power to SD reader
  298. #if SDPOWER > -1
  299. SET_OUTPUT(SDPOWER);
  300. WRITE(SDPOWER,HIGH);
  301. #endif //SDPOWER
  302. initsd();
  303. #endif //SDSUPPORT
  304. plan_init(); // Initialize planner;
  305. st_init(); // Initialize stepper;
  306. tp_init(); // Initialize temperature loop
  307. }
  308. void loop()
  309. {
  310. if(buflen<3)
  311. get_command();
  312. if(buflen){
  313. #ifdef SDSUPPORT
  314. if(savetosd){
  315. if(strstr(cmdbuffer[bufindr],"M29") == NULL){
  316. write_command(cmdbuffer[bufindr]);
  317. Serial.println("ok");
  318. }
  319. else{
  320. file.sync();
  321. file.close();
  322. savetosd = false;
  323. Serial.println("Done saving file.");
  324. }
  325. }
  326. else{
  327. process_commands();
  328. }
  329. #else
  330. process_commands();
  331. #endif //SDSUPPORT
  332. buflen = (buflen-1);
  333. bufindr = (bufindr + 1)%BUFSIZE;
  334. }
  335. //check heater every n milliseconds
  336. manage_heater();
  337. manage_inactivity(1);
  338. }
  339. inline void get_command()
  340. {
  341. while( Serial.available() > 0 && buflen < BUFSIZE) {
  342. serial_char = Serial.read();
  343. if(serial_char == '\n' || serial_char == '\r' || serial_char == ':' || serial_count >= (MAX_CMD_SIZE - 1) )
  344. {
  345. if(!serial_count) return; //if empty line
  346. cmdbuffer[bufindw][serial_count] = 0; //terminate string
  347. if(!comment_mode){
  348. fromsd[bufindw] = false;
  349. if(strstr(cmdbuffer[bufindw], "N") != NULL)
  350. {
  351. strchr_pointer = strchr(cmdbuffer[bufindw], 'N');
  352. gcode_N = (strtol(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL, 10));
  353. if(gcode_N != gcode_LastN+1 && (strstr(cmdbuffer[bufindw], "M110") == NULL) ) {
  354. Serial.print("Serial Error: Line Number is not Last Line Number+1, Last Line:");
  355. Serial.println(gcode_LastN);
  356. //Serial.println(gcode_N);
  357. FlushSerialRequestResend();
  358. serial_count = 0;
  359. return;
  360. }
  361. if(strstr(cmdbuffer[bufindw], "*") != NULL)
  362. {
  363. byte checksum = 0;
  364. byte count = 0;
  365. while(cmdbuffer[bufindw][count] != '*') checksum = checksum^cmdbuffer[bufindw][count++];
  366. strchr_pointer = strchr(cmdbuffer[bufindw], '*');
  367. if( (int)(strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)) != checksum) {
  368. Serial.print("Error: checksum mismatch, Last Line:");
  369. Serial.println(gcode_LastN);
  370. FlushSerialRequestResend();
  371. serial_count = 0;
  372. return;
  373. }
  374. //if no errors, continue parsing
  375. }
  376. else
  377. {
  378. Serial.print("Error: No Checksum with line number, Last Line:");
  379. Serial.println(gcode_LastN);
  380. FlushSerialRequestResend();
  381. serial_count = 0;
  382. return;
  383. }
  384. gcode_LastN = gcode_N;
  385. //if no errors, continue parsing
  386. }
  387. else // if we don't receive 'N' but still see '*'
  388. {
  389. if((strstr(cmdbuffer[bufindw], "*") != NULL))
  390. {
  391. Serial.print("Error: No Line Number with checksum, Last Line:");
  392. Serial.println(gcode_LastN);
  393. serial_count = 0;
  394. return;
  395. }
  396. }
  397. if((strstr(cmdbuffer[bufindw], "G") != NULL)){
  398. strchr_pointer = strchr(cmdbuffer[bufindw], 'G');
  399. switch((int)((strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)))){
  400. case 0:
  401. case 1:
  402. #ifdef SDSUPPORT
  403. if(savetosd)
  404. break;
  405. #endif //SDSUPPORT
  406. Serial.println("ok");
  407. break;
  408. default:
  409. break;
  410. }
  411. }
  412. bufindw = (bufindw + 1)%BUFSIZE;
  413. buflen += 1;
  414. }
  415. comment_mode = false; //for new command
  416. serial_count = 0; //clear buffer
  417. }
  418. else
  419. {
  420. if(serial_char == ';') comment_mode = true;
  421. if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;
  422. }
  423. }
  424. #ifdef SDSUPPORT
  425. if(!sdmode || serial_count!=0){
  426. return;
  427. }
  428. while( filesize > sdpos && buflen < BUFSIZE) {
  429. n = file.read();
  430. serial_char = (char)n;
  431. if(serial_char == '\n' || serial_char == '\r' || serial_char == ':' || serial_count >= (MAX_CMD_SIZE - 1) || n == -1)
  432. {
  433. sdpos = file.curPosition();
  434. if(sdpos >= filesize){
  435. sdmode = false;
  436. Serial.println("Done printing file");
  437. }
  438. if(!serial_count) return; //if empty line
  439. cmdbuffer[bufindw][serial_count] = 0; //terminate string
  440. if(!comment_mode){
  441. fromsd[bufindw] = true;
  442. buflen += 1;
  443. bufindw = (bufindw + 1)%BUFSIZE;
  444. }
  445. comment_mode = false; //for new command
  446. serial_count = 0; //clear buffer
  447. }
  448. else
  449. {
  450. if(serial_char == ';') comment_mode = true;
  451. if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;
  452. }
  453. }
  454. #endif //SDSUPPORT
  455. }
  456. inline float code_value() {
  457. return (strtod(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL));
  458. }
  459. inline long code_value_long() {
  460. return (strtol(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL, 10));
  461. }
  462. inline bool code_seen(char code_string[]) {
  463. return (strstr(cmdbuffer[bufindr], code_string) != NULL);
  464. } //Return True if the string was found
  465. inline bool code_seen(char code)
  466. {
  467. strchr_pointer = strchr(cmdbuffer[bufindr], code);
  468. return (strchr_pointer != NULL); //Return True if a character was found
  469. }
  470. inline void process_commands()
  471. {
  472. unsigned long codenum; //throw away variable
  473. char *starpos = NULL;
  474. if(code_seen('G'))
  475. {
  476. switch((int)code_value())
  477. {
  478. case 0: // G0 -> G1
  479. case 1: // G1
  480. get_coordinates(); // For X Y Z E F
  481. prepare_move();
  482. previous_millis_cmd = millis();
  483. //ClearToSend();
  484. return;
  485. //break;
  486. case 4: // G4 dwell
  487. codenum = 0;
  488. if(code_seen('P')) codenum = code_value(); // milliseconds to wait
  489. if(code_seen('S')) codenum = code_value() * 1000; // seconds to wait
  490. codenum += millis(); // keep track of when we started waiting
  491. while(millis() < codenum ){
  492. manage_heater();
  493. }
  494. break;
  495. case 28: //G28 Home all Axis one at a time
  496. saved_feedrate = feedrate;
  497. for(int i=0; i < NUM_AXIS; i++) {
  498. destination[i] = current_position[i];
  499. }
  500. feedrate = 0;
  501. home_all_axis = !((code_seen(axis_codes[0])) || (code_seen(axis_codes[1])) || (code_seen(axis_codes[2])));
  502. if((home_all_axis) || (code_seen(axis_codes[X_AXIS]))) {
  503. if ((X_MIN_PIN > -1 && X_HOME_DIR==-1) || (X_MAX_PIN > -1 && X_HOME_DIR==1)){
  504. st_synchronize();
  505. current_position[X_AXIS] = 0;
  506. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  507. destination[X_AXIS] = 1.5 * X_MAX_LENGTH * X_HOME_DIR;
  508. feedrate = homing_feedrate[X_AXIS];
  509. prepare_move();
  510. st_synchronize();
  511. current_position[X_AXIS] = 0;
  512. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  513. destination[X_AXIS] = -5 * X_HOME_DIR;
  514. prepare_move();
  515. st_synchronize();
  516. destination[X_AXIS] = 10 * X_HOME_DIR;
  517. feedrate = homing_feedrate[X_AXIS]/2 ;
  518. prepare_move();
  519. st_synchronize();
  520. current_position[X_AXIS] = (X_HOME_DIR == -1) ? 0 : X_MAX_LENGTH;
  521. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  522. destination[X_AXIS] = current_position[X_AXIS];
  523. feedrate = 0;
  524. }
  525. }
  526. if((home_all_axis) || (code_seen(axis_codes[Y_AXIS]))) {
  527. if ((Y_MIN_PIN > -1 && Y_HOME_DIR==-1) || (Y_MAX_PIN > -1 && Y_HOME_DIR==1)){
  528. current_position[Y_AXIS] = 0;
  529. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  530. destination[Y_AXIS] = 1.5 * Y_MAX_LENGTH * Y_HOME_DIR;
  531. feedrate = homing_feedrate[Y_AXIS];
  532. prepare_move();
  533. st_synchronize();
  534. current_position[Y_AXIS] = 0;
  535. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  536. destination[Y_AXIS] = -5 * Y_HOME_DIR;
  537. prepare_move();
  538. st_synchronize();
  539. destination[Y_AXIS] = 10 * Y_HOME_DIR;
  540. feedrate = homing_feedrate[Y_AXIS]/2;
  541. prepare_move();
  542. st_synchronize();
  543. current_position[Y_AXIS] = (Y_HOME_DIR == -1) ? 0 : Y_MAX_LENGTH;
  544. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  545. destination[Y_AXIS] = current_position[Y_AXIS];
  546. feedrate = 0;
  547. }
  548. }
  549. if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) {
  550. if ((Z_MIN_PIN > -1 && Z_HOME_DIR==-1) || (Z_MAX_PIN > -1 && Z_HOME_DIR==1)){
  551. current_position[Z_AXIS] = 0;
  552. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  553. destination[Z_AXIS] = 1.5 * Z_MAX_LENGTH * Z_HOME_DIR;
  554. feedrate = homing_feedrate[Z_AXIS];
  555. prepare_move();
  556. st_synchronize();
  557. current_position[Z_AXIS] = 0;
  558. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  559. destination[Z_AXIS] = -2 * Z_HOME_DIR;
  560. prepare_move();
  561. st_synchronize();
  562. destination[Z_AXIS] = 3 * Z_HOME_DIR;
  563. feedrate = homing_feedrate[Z_AXIS]/2;
  564. prepare_move();
  565. st_synchronize();
  566. current_position[Z_AXIS] = (Z_HOME_DIR == -1) ? 0 : Z_MAX_LENGTH;
  567. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  568. destination[Z_AXIS] = current_position[Z_AXIS];
  569. feedrate = 0;
  570. }
  571. }
  572. feedrate = saved_feedrate;
  573. previous_millis_cmd = millis();
  574. break;
  575. case 90: // G90
  576. relative_mode = false;
  577. break;
  578. case 91: // G91
  579. relative_mode = true;
  580. break;
  581. case 92: // G92
  582. if(!code_seen(axis_codes[E_AXIS]))
  583. st_synchronize();
  584. for(int i=0; i < NUM_AXIS; i++) {
  585. if(code_seen(axis_codes[i])) current_position[i] = code_value();
  586. }
  587. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  588. break;
  589. }
  590. }
  591. else if(code_seen('M'))
  592. {
  593. switch( (int)code_value() )
  594. {
  595. #ifdef SDSUPPORT
  596. case 20: // M20 - list SD card
  597. Serial.println("Begin file list");
  598. root.ls();
  599. Serial.println("End file list");
  600. break;
  601. case 21: // M21 - init SD card
  602. sdmode = false;
  603. initsd();
  604. break;
  605. case 22: //M22 - release SD card
  606. sdmode = false;
  607. sdactive = false;
  608. break;
  609. case 23: //M23 - Select file
  610. if(sdactive){
  611. sdmode = false;
  612. file.close();
  613. starpos = (strchr(strchr_pointer + 4,'*'));
  614. if(starpos!=NULL)
  615. *(starpos-1)='\0';
  616. if (file.open(&root, strchr_pointer + 4, O_READ)) {
  617. Serial.print("File opened:");
  618. Serial.print(strchr_pointer + 4);
  619. Serial.print(" Size:");
  620. Serial.println(file.fileSize());
  621. sdpos = 0;
  622. filesize = file.fileSize();
  623. Serial.println("File selected");
  624. }
  625. else{
  626. Serial.println("file.open failed");
  627. }
  628. }
  629. break;
  630. case 24: //M24 - Start SD print
  631. if(sdactive){
  632. sdmode = true;
  633. }
  634. break;
  635. case 25: //M25 - Pause SD print
  636. if(sdmode){
  637. sdmode = false;
  638. }
  639. break;
  640. case 26: //M26 - Set SD index
  641. if(sdactive && code_seen('S')){
  642. sdpos = code_value_long();
  643. file.seekSet(sdpos);
  644. }
  645. break;
  646. case 27: //M27 - Get SD status
  647. if(sdactive){
  648. Serial.print("SD printing byte ");
  649. Serial.print(sdpos);
  650. Serial.print("/");
  651. Serial.println(filesize);
  652. }
  653. else{
  654. Serial.println("Not SD printing");
  655. }
  656. break;
  657. case 28: //M28 - Start SD write
  658. if(sdactive){
  659. char* npos = 0;
  660. file.close();
  661. sdmode = false;
  662. starpos = (strchr(strchr_pointer + 4,'*'));
  663. if(starpos != NULL){
  664. npos = strchr(cmdbuffer[bufindr], 'N');
  665. strchr_pointer = strchr(npos,' ') + 1;
  666. *(starpos-1) = '\0';
  667. }
  668. if (!file.open(&root, strchr_pointer+4, O_CREAT | O_APPEND | O_WRITE | O_TRUNC))
  669. {
  670. Serial.print("open failed, File: ");
  671. Serial.print(strchr_pointer + 4);
  672. Serial.print(".");
  673. }
  674. else{
  675. savetosd = true;
  676. Serial.print("Writing to file: ");
  677. Serial.println(strchr_pointer + 4);
  678. }
  679. }
  680. break;
  681. case 29: //M29 - Stop SD write
  682. //processed in write to file routine above
  683. //savetosd = false;
  684. break;
  685. #endif //SDSUPPORT
  686. case 104: // M104
  687. #ifdef PID_OPENLOOP
  688. if (code_seen('S')) PidTemp_Output = code_value() * (PID_MAX/100.0);
  689. if(pid_output > PID_MAX) pid_output = PID_MAX;
  690. if(pid_output < 0) pid_output = 0;
  691. #else //PID_OPENLOOP
  692. if (code_seen('S')) {
  693. target_raw = temp2analogh(code_value());
  694. #ifdef PIDTEMP
  695. pid_setpoint = code_value();
  696. #endif //PIDTEMP
  697. }
  698. #ifdef WATCHPERIOD
  699. if(target_raw > current_raw){
  700. watchmillis = max(1,millis());
  701. watch_raw = current_raw;
  702. }
  703. else{
  704. watchmillis = 0;
  705. }
  706. #endif //WATCHPERIOD
  707. #endif //PID_OPENLOOP
  708. break;
  709. case 105: // M105
  710. Serial.print("ok T:");
  711. Serial.println(analog2temp(current_raw));
  712. return;
  713. //break;
  714. case 109: // M109 - Wait for extruder heater to reach target.
  715. if (code_seen('S')) {
  716. target_raw = temp2analogh(code_value());
  717. #ifdef PIDTEMP
  718. pid_setpoint = code_value();
  719. #endif //PIDTEMP
  720. }
  721. #ifdef WATCHPERIOD
  722. if(target_raw>current_raw){
  723. watchmillis = max(1,millis());
  724. watch_raw = current_raw;
  725. }
  726. else{
  727. watchmillis = 0;
  728. }
  729. #endif //WATCHERPERIOD
  730. codenum = millis();
  731. while(current_raw < target_raw) {
  732. if( (millis() - codenum) > 1000 ) //Print Temp Reading every 1 second while heating up.
  733. {
  734. Serial.print("T:");
  735. Serial.println( analog2temp(current_raw));
  736. codenum = millis();
  737. }
  738. manage_heater();
  739. }
  740. break;
  741. case 190:
  742. break;
  743. case 82:
  744. axis_relative_modes[3] = false;
  745. break;
  746. case 83:
  747. axis_relative_modes[3] = true;
  748. break;
  749. case 84:
  750. if(code_seen('S')){
  751. stepper_inactive_time = code_value() * 1000;
  752. }
  753. else{
  754. st_synchronize();
  755. disable_x();
  756. disable_y();
  757. disable_z();
  758. disable_e();
  759. }
  760. break;
  761. case 85: // M85
  762. code_seen('S');
  763. max_inactive_time = code_value() * 1000;
  764. break;
  765. case 92: // M92
  766. for(int i=0; i < NUM_AXIS; i++) {
  767. if(code_seen(axis_codes[i])) axis_steps_per_unit[i] = code_value();
  768. }
  769. break;
  770. case 115: // M115
  771. Serial.println("FIRMWARE_NAME:Sprinter/grbl mashup for gen6 FIRMWARE_URL:http://www.mendel-parts.com PROTOCOL_VERSION:1.0 MACHINE_TYPE:Mendel EXTRUDER_COUNT:1");
  772. break;
  773. case 114: // M114
  774. Serial.print("X:");
  775. Serial.print(current_position[X_AXIS]);
  776. Serial.print("Y:");
  777. Serial.print(current_position[Y_AXIS]);
  778. Serial.print("Z:");
  779. Serial.print(current_position[Z_AXIS]);
  780. Serial.print("E:");
  781. Serial.println(current_position[E_AXIS]);
  782. break;
  783. case 119: // M119
  784. #if (X_MIN_PIN > -1)
  785. Serial.print("x_min:");
  786. Serial.print((READ(X_MIN_PIN)^ENDSTOPS_INVERTING)?"H ":"L ");
  787. #endif
  788. #if (X_MAX_PIN > -1)
  789. Serial.print("x_max:");
  790. Serial.print((READ(X_MAX_PIN)^ENDSTOPS_INVERTING)?"H ":"L ");
  791. #endif
  792. #if (Y_MIN_PIN > -1)
  793. Serial.print("y_min:");
  794. Serial.print((READ(Y_MIN_PIN)^ENDSTOPS_INVERTING)?"H ":"L ");
  795. #endif
  796. #if (Y_MAX_PIN > -1)
  797. Serial.print("y_max:");
  798. Serial.print((READ(Y_MAX_PIN)^ENDSTOPS_INVERTING)?"H ":"L ");
  799. #endif
  800. #if (Z_MIN_PIN > -1)
  801. Serial.print("z_min:");
  802. Serial.print((READ(Z_MIN_PIN)^ENDSTOPS_INVERTING)?"H ":"L ");
  803. #endif
  804. #if (Z_MAX_PIN > -1)
  805. Serial.print("z_max:");
  806. Serial.print((READ(Z_MAX_PIN)^ENDSTOPS_INVERTING)?"H ":"L ");
  807. #endif
  808. Serial.println("");
  809. break;
  810. //TODO: update for all axis, use for loop
  811. case 201: // M201
  812. for(int i=0; i < NUM_AXIS; i++) {
  813. if(code_seen(axis_codes[i])) axis_steps_per_sqr_second[i] = code_value() * axis_steps_per_unit[i];
  814. }
  815. break;
  816. #if 0 // Not used for Sprinter/grbl gen6
  817. case 202: // M202
  818. for(int i=0; i < NUM_AXIS; i++) {
  819. if(code_seen(axis_codes[i])) axis_travel_steps_per_sqr_second[i] = code_value() * axis_steps_per_unit[i];
  820. }
  821. break;
  822. #endif
  823. #ifdef PIDTEMP
  824. case 301: // M301
  825. if(code_seen('P')) Kp = code_value();
  826. if(code_seen('I')) Ki = code_value()*PID_dT;
  827. if(code_seen('D')) Kd = code_value()/PID_dT;
  828. Serial.print("Kp ");Serial.println(Kp);
  829. Serial.print("Ki ");Serial.println(Ki/PID_dT);
  830. Serial.print("Kd ");Serial.println(Kd*PID_dT);
  831. temp_iState_min = 0.0;
  832. temp_iState_max = PID_INTEGRAL_DRIVE_MAX / Ki;
  833. break;
  834. #endif //PIDTEMP
  835. }
  836. }
  837. else{
  838. Serial.println("Unknown command:");
  839. Serial.println(cmdbuffer[bufindr]);
  840. }
  841. ClearToSend();
  842. }
  843. void FlushSerialRequestResend()
  844. {
  845. //char cmdbuffer[bufindr][100]="Resend:";
  846. Serial.flush();
  847. Serial.print("Resend:");
  848. Serial.println(gcode_LastN + 1);
  849. ClearToSend();
  850. }
  851. void ClearToSend()
  852. {
  853. previous_millis_cmd = millis();
  854. #ifdef SDSUPPORT
  855. if(fromsd[bufindr])
  856. return;
  857. #endif //SDSUPPORT
  858. Serial.println("ok");
  859. }
  860. inline void get_coordinates()
  861. {
  862. for(int i=0; i < NUM_AXIS; i++) {
  863. if(code_seen(axis_codes[i])) destination[i] = (float)code_value() + (axis_relative_modes[i] || relative_mode)*current_position[i];
  864. else destination[i] = current_position[i]; //Are these else lines really needed?
  865. }
  866. if(code_seen('F')) {
  867. next_feedrate = code_value();
  868. if(next_feedrate > 0.0) feedrate = next_feedrate;
  869. }
  870. }
  871. void prepare_move()
  872. {
  873. plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60.0);
  874. for(int i=0; i < NUM_AXIS; i++) {
  875. current_position[i] = destination[i];
  876. }
  877. }
  878. void manage_heater()
  879. {
  880. float pid_input;
  881. float pid_output;
  882. if(temp_meas_ready != true)
  883. return;
  884. CRITICAL_SECTION_START;
  885. temp_meas_ready = false;
  886. CRITICAL_SECTION_END;
  887. #ifdef PIDTEMP
  888. pid_input = analog2temp(current_raw);
  889. #ifndef PID_OPENLOOP
  890. pid_error = pid_setpoint - pid_input;
  891. if(pid_error > 10){
  892. pid_output = PID_MAX;
  893. pid_reset = true;
  894. }
  895. else if(pid_error < -10) {
  896. pid_output = 0;
  897. pid_reset = true;
  898. }
  899. else {
  900. if(pid_reset == true) {
  901. temp_iState = 0.0;
  902. pid_reset = false;
  903. }
  904. pTerm = Kp * pid_error;
  905. temp_iState += pid_error;
  906. temp_iState = constrain(temp_iState, temp_iState_min, temp_iState_max);
  907. iTerm = Ki * temp_iState;
  908. #define K1 0.8
  909. #define K2 (1.0-K1)
  910. dTerm = (Kd * (pid_input - temp_dState))*K2 + (K1 * dTerm);
  911. temp_dState = pid_input;
  912. pid_output = constrain(pTerm + iTerm - dTerm, 0, PID_MAX);
  913. }
  914. #endif //PID_OPENLOOP
  915. #ifdef PID_DEBUG
  916. Serial.print(" Input ");
  917. Serial.print(pid_input);
  918. Serial.print(" Output ");
  919. Serial.print(pid_output);
  920. Serial.print(" pTerm ");
  921. Serial.print(pTerm);
  922. Serial.print(" iTerm ");
  923. Serial.print(iTerm);
  924. Serial.print(" dTerm ");
  925. Serial.print(dTerm);
  926. Serial.println();
  927. #endif //PID_DEBUG
  928. OCR2B = pid_output;
  929. #endif //PIDTEMP
  930. }
  931. int temp2analogu(int celsius, const short table[][2], int numtemps) {
  932. int raw = 0;
  933. byte i;
  934. for (i=1; i<numtemps; i++) {
  935. if (table[i][1] < celsius) {
  936. raw = table[i-1][0] +
  937. (celsius - table[i-1][1]) *
  938. (table[i][0] - table[i-1][0]) /
  939. (table[i][1] - table[i-1][1]);
  940. break;
  941. }
  942. }
  943. // Overflow: Set to last value in the table
  944. if (i == numtemps) raw = table[i-1][0];
  945. return 16383 - raw;
  946. }
  947. float analog2tempu(int raw,const short table[][2], int numtemps) {
  948. float celsius = 0.0;
  949. byte i;
  950. raw = 16383 - raw;
  951. for (i=1; i<numtemps; i++) {
  952. if (table[i][0] > raw) {
  953. celsius = (float)table[i-1][1] +
  954. (float)(raw - table[i-1][0]) *
  955. (float)(table[i][1] - table[i-1][1]) /
  956. (float)(table[i][0] - table[i-1][0]);
  957. break;
  958. }
  959. }
  960. // Overflow: Set to last value in the table
  961. if (i == numtemps) celsius = table[i-1][1];
  962. return celsius;
  963. }
  964. inline void kill()
  965. {
  966. target_raw=0;
  967. #ifdef PIDTEMP
  968. pid_setpoint = 0.0;
  969. #endif //PIDTEMP
  970. OCR2B = 0;
  971. WRITE(HEATER_0_PIN,LOW);
  972. disable_x();
  973. disable_y();
  974. disable_z();
  975. disable_e();
  976. }
  977. inline void manage_inactivity(byte debug) {
  978. if( (millis()-previous_millis_cmd) > max_inactive_time ) if(max_inactive_time) kill();
  979. if( (millis()-previous_millis_cmd) > stepper_inactive_time ) if(stepper_inactive_time) {
  980. disable_x();
  981. disable_y();
  982. disable_z();
  983. disable_e();
  984. }
  985. check_axes_activity();
  986. }
  987. // Planner
  988. /*
  989. Reasoning behind the mathematics in this module (in the key of 'Mathematica'):
  990. s == speed, a == acceleration, t == time, d == distance
  991. Basic definitions:
  992. Speed[s_, a_, t_] := s + (a*t)
  993. Travel[s_, a_, t_] := Integrate[Speed[s, a, t], t]
  994. Distance to reach a specific speed with a constant acceleration:
  995. Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, d, t]
  996. d -> (m^2 - s^2)/(2 a) --> estimate_acceleration_distance()
  997. Speed after a given distance of travel with constant acceleration:
  998. Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, m, t]
  999. m -> Sqrt[2 a d + s^2]
  1000. DestinationSpeed[s_, a_, d_] := Sqrt[2 a d + s^2]
  1001. When to start braking (di) to reach a specified destionation speed (s2) after accelerating
  1002. from initial speed s1 without ever stopping at a plateau:
  1003. Solve[{DestinationSpeed[s1, a, di] == DestinationSpeed[s2, a, d - di]}, di]
  1004. di -> (2 a d - s1^2 + s2^2)/(4 a) --> intersection_distance()
  1005. IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a)
  1006. */
  1007. // The number of linear motions that can be in the plan at any give time
  1008. #define BLOCK_BUFFER_SIZE 16
  1009. #define BLOCK_BUFFER_MASK 0x0f
  1010. static block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions
  1011. static volatile unsigned char block_buffer_head; // Index of the next block to be pushed
  1012. static volatile unsigned char block_buffer_tail; // Index of the block to process now
  1013. // The current position of the tool in absolute steps
  1014. static long position[4];
  1015. #define ONE_MINUTE_OF_MICROSECONDS 60000000.0
  1016. // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
  1017. // given acceleration:
  1018. inline long estimate_acceleration_distance(long initial_rate, long target_rate, long acceleration) {
  1019. return(
  1020. (target_rate*target_rate-initial_rate*initial_rate)/
  1021. (2L*acceleration)
  1022. );
  1023. }
  1024. // This function gives you the point at which you must start braking (at the rate of -acceleration) if
  1025. // you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
  1026. // a total travel of distance. This can be used to compute the intersection point between acceleration and
  1027. // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
  1028. inline long intersection_distance(long initial_rate, long final_rate, long acceleration, long distance) {
  1029. return(
  1030. (2*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/
  1031. (4*acceleration)
  1032. );
  1033. }
  1034. // Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
  1035. void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit_speed) {
  1036. if(block->busy == true) return; // If block is busy then bail out.
  1037. float entry_factor = entry_speed / block->nominal_speed;
  1038. float exit_factor = exit_speed / block->nominal_speed;
  1039. long initial_rate = ceil(block->nominal_rate*entry_factor);
  1040. long final_rate = ceil(block->nominal_rate*exit_factor);
  1041. #ifdef ADVANCE
  1042. long initial_advance = block->advance*entry_factor*entry_factor;
  1043. long final_advance = block->advance*exit_factor*exit_factor;
  1044. #endif // ADVANCE
  1045. // Limit minimal step rate (Otherwise the timer will overflow.)
  1046. if(initial_rate <120) initial_rate=120;
  1047. if(final_rate < 120) final_rate=120;
  1048. // Calculate the acceleration steps
  1049. long acceleration = block->acceleration_st;
  1050. long accelerate_steps = estimate_acceleration_distance(initial_rate, block->nominal_rate, acceleration);
  1051. long decelerate_steps = estimate_acceleration_distance(final_rate, block->nominal_rate, acceleration);
  1052. // Calculate the size of Plateau of Nominal Rate.
  1053. long plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps;
  1054. // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
  1055. // have to use intersection_distance() to calculate when to abort acceleration and start braking
  1056. // in order to reach the final_rate exactly at the end of this block.
  1057. if (plateau_steps < 0) {
  1058. accelerate_steps = intersection_distance(initial_rate, final_rate, acceleration, block->step_event_count);
  1059. plateau_steps = 0;
  1060. }
  1061. long decelerate_after = accelerate_steps+plateau_steps;
  1062. long acceleration_rate = (long)((float)acceleration * 8.388608);
  1063. CRITICAL_SECTION_START; // Fill variables used by the stepper in a critical section
  1064. if(block->busy == false) { // Don't update variables if block is busy.
  1065. block->accelerate_until = accelerate_steps;
  1066. block->decelerate_after = decelerate_after;
  1067. block->acceleration_rate = acceleration_rate;
  1068. block->initial_rate = initial_rate;
  1069. block->final_rate = final_rate;
  1070. #ifdef ADVANCE
  1071. block->initial_advance = initial_advance;
  1072. block->final_advance = final_advance;
  1073. #endif ADVANCE
  1074. }
  1075. CRITICAL_SECTION_END;
  1076. }
  1077. // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
  1078. // acceleration within the allotted distance.
  1079. inline float max_allowable_speed(float acceleration, float target_velocity, float distance) {
  1080. return(
  1081. sqrt(target_velocity*target_velocity-2*acceleration*60*60*distance)
  1082. );
  1083. }
  1084. // "Junction jerk" in this context is the immediate change in speed at the junction of two blocks.
  1085. // This method will calculate the junction jerk as the euclidean distance between the nominal
  1086. // velocities of the respective blocks.
  1087. inline float junction_jerk(block_t *before, block_t *after) {
  1088. return(sqrt(
  1089. pow((before->speed_x-after->speed_x), 2)+
  1090. pow((before->speed_y-after->speed_y), 2)));
  1091. }
  1092. // Return the safe speed which is max_jerk/2, e.g. the
  1093. // speed under which you cannot exceed max_jerk no matter what you do.
  1094. float safe_speed(block_t *block) {
  1095. float safe_speed;
  1096. safe_speed = max_xy_jerk/2;
  1097. if(abs(block->speed_z) > max_z_jerk/2) safe_speed = max_z_jerk/2;
  1098. if (safe_speed > block->nominal_speed) safe_speed = block->nominal_speed;
  1099. return safe_speed;
  1100. }
  1101. // The kernel called by planner_recalculate() when scanning the plan from last to first entry.
  1102. void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) {
  1103. if(!current) {
  1104. return;
  1105. }
  1106. float entry_speed = current->nominal_speed;
  1107. float exit_factor;
  1108. float exit_speed;
  1109. if (next) {
  1110. exit_speed = next->entry_speed;
  1111. }
  1112. else {
  1113. exit_speed = safe_speed(current);
  1114. }
  1115. // Calculate the entry_factor for the current block.
  1116. if (previous) {
  1117. // Reduce speed so that junction_jerk is within the maximum allowed
  1118. float jerk = junction_jerk(previous, current);
  1119. if((previous->steps_x == 0) && (previous->steps_y == 0)) {
  1120. entry_speed = safe_speed(current);
  1121. }
  1122. else if (jerk > max_xy_jerk) {
  1123. entry_speed = (max_xy_jerk/jerk) * entry_speed;
  1124. }
  1125. if(abs(previous->speed_z - current->speed_z) > max_z_jerk) {
  1126. entry_speed = (max_z_jerk/abs(previous->speed_z - current->speed_z)) * entry_speed;
  1127. }
  1128. // If the required deceleration across the block is too rapid, reduce the entry_factor accordingly.
  1129. if (entry_speed > exit_speed) {
  1130. float max_entry_speed = max_allowable_speed(-current->acceleration,exit_speed, current->millimeters);
  1131. if (max_entry_speed < entry_speed) {
  1132. entry_speed = max_entry_speed;
  1133. }
  1134. }
  1135. }
  1136. else {
  1137. entry_speed = safe_speed(current);
  1138. }
  1139. // Store result
  1140. current->entry_speed = entry_speed;
  1141. }
  1142. // planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
  1143. // implements the reverse pass.
  1144. void planner_reverse_pass() {
  1145. char block_index = block_buffer_head;
  1146. block_t *block[3] = {
  1147. NULL, NULL, NULL };
  1148. while(block_index != block_buffer_tail) {
  1149. block[2]= block[1];
  1150. block[1]= block[0];
  1151. block[0] = &block_buffer[block_index];
  1152. planner_reverse_pass_kernel(block[0], block[1], block[2]);
  1153. block_index--;
  1154. if(block_index < 0) {
  1155. block_index = BLOCK_BUFFER_SIZE-1;
  1156. }
  1157. }
  1158. // planner_reverse_pass_kernel(NULL, block[0], block[1]);
  1159. }
  1160. // The kernel called by planner_recalculate() when scanning the plan from first to last entry.
  1161. void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) {
  1162. if(!current) {
  1163. return;
  1164. }
  1165. if(previous) {
  1166. // If the previous block is an acceleration block, but it is not long enough to
  1167. // complete the full speed change within the block, we need to adjust out entry
  1168. // speed accordingly. Remember current->entry_factor equals the exit factor of
  1169. // the previous block.
  1170. if(previous->entry_speed < current->entry_speed) {
  1171. float max_entry_speed = max_allowable_speed(-previous->acceleration, previous->entry_speed, previous->millimeters);
  1172. if (max_entry_speed < current->entry_speed) {
  1173. current->entry_speed = max_entry_speed;
  1174. }
  1175. }
  1176. }
  1177. }
  1178. // planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
  1179. // implements the forward pass.
  1180. void planner_forward_pass() {
  1181. char block_index = block_buffer_tail;
  1182. block_t *block[3] = {
  1183. NULL, NULL, NULL };
  1184. while(block_index != block_buffer_head) {
  1185. block[0] = block[1];
  1186. block[1] = block[2];
  1187. block[2] = &block_buffer[block_index];
  1188. planner_forward_pass_kernel(block[0],block[1],block[2]);
  1189. block_index = (block_index+1) & BLOCK_BUFFER_MASK;
  1190. }
  1191. planner_forward_pass_kernel(block[1], block[2], NULL);
  1192. }
  1193. // Recalculates the trapezoid speed profiles for all blocks in the plan according to the
  1194. // entry_factor for each junction. Must be called by planner_recalculate() after
  1195. // updating the blocks.
  1196. void planner_recalculate_trapezoids() {
  1197. char block_index = block_buffer_tail;
  1198. block_t *current;
  1199. block_t *next = NULL;
  1200. while(block_index != block_buffer_head) {
  1201. current = next;
  1202. next = &block_buffer[block_index];
  1203. if (current) {
  1204. calculate_trapezoid_for_block(current, current->entry_speed, next->entry_speed);
  1205. }
  1206. block_index = (block_index+1) & BLOCK_BUFFER_MASK;
  1207. }
  1208. calculate_trapezoid_for_block(next, next->entry_speed, safe_speed(next));
  1209. }
  1210. // Recalculates the motion plan according to the following algorithm:
  1211. //
  1212. // 1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_factor)
  1213. // so that:
  1214. // a. The junction jerk is within the set limit
  1215. // b. No speed reduction within one block requires faster deceleration than the one, true constant
  1216. // acceleration.
  1217. // 2. Go over every block in chronological order and dial down junction speed reduction values if
  1218. // a. The speed increase within one block would require faster accelleration than the one, true
  1219. // constant acceleration.
  1220. //
  1221. // When these stages are complete all blocks have an entry_factor that will allow all speed changes to
  1222. // be performed using only the one, true constant acceleration, and where no junction jerk is jerkier than
  1223. // the set limit. Finally it will:
  1224. //
  1225. // 3. Recalculate trapezoids for all blocks.
  1226. void planner_recalculate() {
  1227. planner_reverse_pass();
  1228. planner_forward_pass();
  1229. planner_recalculate_trapezoids();
  1230. }
  1231. void plan_init() {
  1232. block_buffer_head = 0;
  1233. block_buffer_tail = 0;
  1234. memset(position, 0, sizeof(position)); // clear position
  1235. }
  1236. inline void plan_discard_current_block() {
  1237. if (block_buffer_head != block_buffer_tail) {
  1238. block_buffer_tail = (block_buffer_tail + 1) & BLOCK_BUFFER_MASK;
  1239. }
  1240. }
  1241. inline block_t *plan_get_current_block() {
  1242. if (block_buffer_head == block_buffer_tail) {
  1243. return(NULL);
  1244. }
  1245. block_t *block = &block_buffer[block_buffer_tail];
  1246. block->busy = true;
  1247. return(block);
  1248. }
  1249. void check_axes_activity() {
  1250. unsigned char x_active = 0;
  1251. unsigned char y_active = 0;
  1252. unsigned char z_active = 0;
  1253. unsigned char e_active = 0;
  1254. block_t *block;
  1255. if(block_buffer_tail != block_buffer_head) {
  1256. char block_index = block_buffer_tail;
  1257. while(block_index != block_buffer_head) {
  1258. block = &block_buffer[block_index];
  1259. if(block->steps_x != 0) x_active++;
  1260. if(block->steps_y != 0) y_active++;
  1261. if(block->steps_z != 0) z_active++;
  1262. if(block->steps_e != 0) e_active++;
  1263. block_index = (block_index+1) & BLOCK_BUFFER_MASK;
  1264. }
  1265. }
  1266. if((DISABLE_X) && (x_active == 0)) disable_x();
  1267. if((DISABLE_Y) && (y_active == 0)) disable_y();
  1268. if((DISABLE_Z) && (z_active == 0)) disable_z();
  1269. if((DISABLE_E) && (e_active == 0)) disable_e();
  1270. }
  1271. // Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in
  1272. // mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
  1273. // calculation the caller must also provide the physical length of the line in millimeters.
  1274. void plan_buffer_line(float x, float y, float z, float e, float feed_rate) {
  1275. // The target position of the tool in absolute steps
  1276. // Calculate target position in absolute steps
  1277. long target[4];
  1278. target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
  1279. target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
  1280. target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
  1281. target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
  1282. // Calculate the buffer head after we push this byte
  1283. int next_buffer_head = (block_buffer_head + 1) & BLOCK_BUFFER_MASK;
  1284. // If the buffer is full: good! That means we are well ahead of the robot.
  1285. // Rest here until there is room in the buffer.
  1286. while(block_buffer_tail == next_buffer_head) {
  1287. manage_heater();
  1288. manage_inactivity(1);
  1289. }
  1290. // Prepare to set up new block
  1291. block_t *block = &block_buffer[block_buffer_head];
  1292. // Mark block as not busy (Not executed by the stepper interrupt)
  1293. block->busy = false;
  1294. // Number of steps for each axis
  1295. block->steps_x = labs(target[X_AXIS]-position[X_AXIS]);
  1296. block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);
  1297. block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]);
  1298. block->steps_e = labs(target[E_AXIS]-position[E_AXIS]);
  1299. block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e)));
  1300. // Bail if this is a zero-length block
  1301. if (block->step_event_count == 0) {
  1302. return;
  1303. };
  1304. //enable active axes
  1305. if(block->steps_x != 0) enable_x();
  1306. if(block->steps_y != 0) enable_y();
  1307. if(block->steps_z != 0) enable_z();
  1308. if(block->steps_e != 0) enable_e();
  1309. float delta_x_mm = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
  1310. float delta_y_mm = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
  1311. float delta_z_mm = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS];
  1312. float delta_e_mm = (target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS];
  1313. block->millimeters = sqrt(square(delta_x_mm) + square(delta_y_mm) + square(delta_z_mm) + square(delta_e_mm));
  1314. unsigned long microseconds;
  1315. microseconds = lround((block->millimeters/feed_rate)*1000000);
  1316. // Calculate speed in mm/minute for each axis
  1317. float multiplier = 60.0*1000000.0/microseconds;
  1318. block->speed_z = delta_z_mm * multiplier;
  1319. block->speed_x = delta_x_mm * multiplier;
  1320. block->speed_y = delta_y_mm * multiplier;
  1321. block->speed_e = delta_e_mm * multiplier;
  1322. // Limit speed per axis
  1323. float speed_factor = 1;
  1324. float tmp_speed_factor;
  1325. if(abs(block->speed_x) > max_feedrate[X_AXIS]) {
  1326. speed_factor = max_feedrate[X_AXIS] / abs(block->speed_x);
  1327. }
  1328. if(abs(block->speed_y) > max_feedrate[Y_AXIS]){
  1329. tmp_speed_factor = max_feedrate[Y_AXIS] / abs(block->speed_y);
  1330. if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor;
  1331. }
  1332. if(abs(block->speed_z) > max_feedrate[Z_AXIS]){
  1333. tmp_speed_factor = max_feedrate[Z_AXIS] / abs(block->speed_z);
  1334. if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor;
  1335. }
  1336. if(abs(block->speed_e) > max_feedrate[E_AXIS]){
  1337. tmp_speed_factor = max_feedrate[E_AXIS] / abs(block->speed_e);
  1338. if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor;
  1339. }
  1340. multiplier = multiplier * speed_factor;
  1341. block->speed_z = delta_z_mm * multiplier;
  1342. block->speed_x = delta_x_mm * multiplier;
  1343. block->speed_y = delta_y_mm * multiplier;
  1344. block->speed_e = delta_e_mm * multiplier;
  1345. block->nominal_speed = block->millimeters * multiplier;
  1346. block->nominal_rate = ceil(block->step_event_count * multiplier / 60);
  1347. if(block->nominal_rate < 120) block->nominal_rate = 120;
  1348. block->entry_speed = safe_speed(block);
  1349. // Compute the acceleration rate for the trapezoid generator.
  1350. float travel_per_step = block->millimeters/block->step_event_count;
  1351. if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0) {
  1352. block->acceleration = ceil( (retract_acceleration)/travel_per_step); // convert to: acceleration steps/sec^2
  1353. }
  1354. else {
  1355. block->acceleration_st = ceil( (acceleration)/travel_per_step); // convert to: acceleration steps/sec^2
  1356. // Limit acceleration per axis
  1357. if((block->acceleration_st * block->steps_x / block->step_event_count) > axis_steps_per_sqr_second[X_AXIS])
  1358. block->acceleration_st = axis_steps_per_sqr_second[X_AXIS];
  1359. if((block->acceleration_st * block->steps_y / block->step_event_count) > axis_steps_per_sqr_second[Y_AXIS])
  1360. block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS];
  1361. if((block->acceleration_st * block->steps_e / block->step_event_count) > axis_steps_per_sqr_second[E_AXIS])
  1362. block->acceleration_st = axis_steps_per_sqr_second[E_AXIS];
  1363. if(((block->acceleration_st / block->step_event_count) * block->steps_z ) > axis_steps_per_sqr_second[Z_AXIS])
  1364. block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS];
  1365. }
  1366. block->acceleration = block->acceleration_st * travel_per_step;
  1367. #ifdef ADVANCE
  1368. // Calculate advance rate
  1369. if((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) {
  1370. block->advance_rate = 0;
  1371. block->advance = 0;
  1372. }
  1373. else {
  1374. long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);
  1375. float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) *
  1376. (block->speed_e * block->speed_e * EXTRUTION_AREA * EXTRUTION_AREA / 3600.0)*65536;
  1377. block->advance = advance;
  1378. if(acc_dist == 0) {
  1379. block->advance_rate = 0;
  1380. }
  1381. else {
  1382. block->advance_rate = advance / (float)acc_dist;
  1383. }
  1384. }
  1385. #endif // ADVANCE
  1386. // compute a preliminary conservative acceleration trapezoid
  1387. float safespeed = safe_speed(block);
  1388. calculate_trapezoid_for_block(block, safespeed, safespeed);
  1389. // Compute direction bits for this block
  1390. block->direction_bits = 0;
  1391. if (target[X_AXIS] < position[X_AXIS]) {
  1392. block->direction_bits |= (1<<X_AXIS);
  1393. }
  1394. if (target[Y_AXIS] < position[Y_AXIS]) {
  1395. block->direction_bits |= (1<<Y_AXIS);
  1396. }
  1397. if (target[Z_AXIS] < position[Z_AXIS]) {
  1398. block->direction_bits |= (1<<Z_AXIS);
  1399. }
  1400. if (target[E_AXIS] < position[E_AXIS]) {
  1401. block->direction_bits |= (1<<E_AXIS);
  1402. }
  1403. // Move buffer head
  1404. block_buffer_head = next_buffer_head;
  1405. // Update position
  1406. memcpy(position, target, sizeof(target)); // position[] = target[]
  1407. planner_recalculate();
  1408. st_wake_up();
  1409. }
  1410. void plan_set_position(float x, float y, float z, float e)
  1411. {
  1412. position[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
  1413. position[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
  1414. position[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
  1415. position[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
  1416. }
  1417. // Stepper
  1418. // intRes = intIn1 * intIn2 >> 16
  1419. // uses:
  1420. // r26 to store 0
  1421. // r27 to store the byte 1 of the 24 bit result
  1422. #define MultiU16X8toH16(intRes, charIn1, intIn2) \
  1423. asm volatile ( \
  1424. "clr r26 \n\t" \
  1425. "mul %A1, %B2 \n\t" \
  1426. "movw %A0, r0 \n\t" \
  1427. "mul %A1, %A2 \n\t" \
  1428. "add %A0, r1 \n\t" \
  1429. "adc %B0, r26 \n\t" \
  1430. "lsr r0 \n\t" \
  1431. "adc %A0, r26 \n\t" \
  1432. "adc %B0, r26 \n\t" \
  1433. "clr r1 \n\t" \
  1434. : \
  1435. "=&r" (intRes) \
  1436. : \
  1437. "d" (charIn1), \
  1438. "d" (intIn2) \
  1439. : \
  1440. "r26" \
  1441. )
  1442. // intRes = longIn1 * longIn2 >> 24
  1443. // uses:
  1444. // r26 to store 0
  1445. // r27 to store the byte 1 of the 48bit result
  1446. #define MultiU24X24toH16(intRes, longIn1, longIn2) \
  1447. asm volatile ( \
  1448. "clr r26 \n\t" \
  1449. "mul %A1, %B2 \n\t" \
  1450. "mov r27, r1 \n\t" \
  1451. "mul %B1, %C2 \n\t" \
  1452. "movw %A0, r0 \n\t" \
  1453. "mul %C1, %C2 \n\t" \
  1454. "add %B0, r0 \n\t" \
  1455. "mul %C1, %B2 \n\t" \
  1456. "add %A0, r0 \n\t" \
  1457. "adc %B0, r1 \n\t" \
  1458. "mul %A1, %C2 \n\t" \
  1459. "add r27, r0 \n\t" \
  1460. "adc %A0, r1 \n\t" \
  1461. "adc %B0, r26 \n\t" \
  1462. "mul %B1, %B2 \n\t" \
  1463. "add r27, r0 \n\t" \
  1464. "adc %A0, r1 \n\t" \
  1465. "adc %B0, r26 \n\t" \
  1466. "mul %C1, %A2 \n\t" \
  1467. "add r27, r0 \n\t" \
  1468. "adc %A0, r1 \n\t" \
  1469. "adc %B0, r26 \n\t" \
  1470. "mul %B1, %A2 \n\t" \
  1471. "add r27, r1 \n\t" \
  1472. "adc %A0, r26 \n\t" \
  1473. "adc %B0, r26 \n\t" \
  1474. "lsr r27 \n\t" \
  1475. "adc %A0, r26 \n\t" \
  1476. "adc %B0, r26 \n\t" \
  1477. "clr r1 \n\t" \
  1478. : \
  1479. "=&r" (intRes) \
  1480. : \
  1481. "d" (longIn1), \
  1482. "d" (longIn2) \
  1483. : \
  1484. "r26" , "r27" \
  1485. )
  1486. // Some useful constants
  1487. #define ENABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 |= (1<<OCIE1A)
  1488. #define DISABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 &= ~(1<<OCIE1A)
  1489. static block_t *current_block; // A pointer to the block currently being traced
  1490. // Variables used by The Stepper Driver Interrupt
  1491. static unsigned char out_bits; // The next stepping-bits to be output
  1492. static long counter_x, // Counter variables for the bresenham line tracer
  1493. counter_y,
  1494. counter_z,
  1495. counter_e;
  1496. static unsigned long step_events_completed; // The number of step events executed in the current block
  1497. static long advance_rate, advance, final_advance = 0;
  1498. static short old_advance = 0;
  1499. static short e_steps;
  1500. static unsigned char busy = false; // TRUE when SIG_OUTPUT_COMPARE1A is being serviced. Used to avoid retriggering that handler.
  1501. static long acceleration_time, deceleration_time;
  1502. static long accelerate_until, decelerate_after, acceleration_rate, initial_rate, final_rate, nominal_rate;
  1503. static unsigned short acc_step_rate; // needed for deccelaration start point
  1504. // __________________________
  1505. // /| |\ _________________ ^
  1506. // / | | \ /| |\ |
  1507. // / | | \ / | | \ s
  1508. // / | | | | | \ p
  1509. // / | | | | | \ e
  1510. // +-----+------------------------+---+--+---------------+----+ e
  1511. // | BLOCK 1 | BLOCK 2 | d
  1512. //
  1513. // time ----->
  1514. //
  1515. // The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates
  1516. // first block->accelerate_until step_events_completed, then keeps going at constant speed until
  1517. // step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset.
  1518. // The slope of acceleration is calculated with the leib ramp alghorithm.
  1519. void st_wake_up() {
  1520. // TCNT1 = 0;
  1521. ENABLE_STEPPER_DRIVER_INTERRUPT();
  1522. }
  1523. inline unsigned short calc_timer(unsigned short step_rate) {
  1524. unsigned short timer;
  1525. if(step_rate < 32) step_rate = 32;
  1526. step_rate -= 32; // Correct for minimal speed
  1527. if(step_rate >= (8*256)){ // higher step rate
  1528. unsigned short table_address = (unsigned short)&speed_lookuptable_fast[(unsigned char)(step_rate>>8)][0];
  1529. unsigned char tmp_step_rate = (step_rate & 0x00ff);
  1530. unsigned short gain = (unsigned short)pgm_read_word_near(table_address+2);
  1531. MultiU16X8toH16(timer, tmp_step_rate, gain);
  1532. timer = (unsigned short)pgm_read_word_near(table_address) - timer;
  1533. }
  1534. else { // lower step rates
  1535. unsigned short table_address = (unsigned short)&speed_lookuptable_slow[0][0];
  1536. table_address += ((step_rate)>>1) & 0xfffc;
  1537. timer = (unsigned short)pgm_read_word_near(table_address);
  1538. timer -= (((unsigned short)pgm_read_word_near(table_address+2) * (unsigned char)(step_rate & 0x0007))>>3);
  1539. }
  1540. if(timer < 100) timer = 100;
  1541. return timer;
  1542. }
  1543. // Initializes the trapezoid generator from the current block. Called whenever a new
  1544. // block begins.
  1545. inline void trapezoid_generator_reset() {
  1546. accelerate_until = current_block->accelerate_until;
  1547. decelerate_after = current_block->decelerate_after;
  1548. acceleration_rate = current_block->acceleration_rate;
  1549. initial_rate = current_block->initial_rate;
  1550. final_rate = current_block->final_rate;
  1551. nominal_rate = current_block->nominal_rate;
  1552. advance = current_block->initial_advance;
  1553. final_advance = current_block->final_advance;
  1554. deceleration_time = 0;
  1555. advance_rate = current_block->advance_rate;
  1556. // step_rate to timer interval
  1557. acc_step_rate = initial_rate;
  1558. acceleration_time = calc_timer(acc_step_rate);
  1559. OCR1A = acceleration_time;
  1560. }
  1561. // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse.
  1562. // It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately.
  1563. ISR(TIMER1_COMPA_vect)
  1564. {
  1565. if(busy){ /*Serial.println("BUSY")*/;
  1566. return;
  1567. } // The busy-flag is used to avoid reentering this interrupt
  1568. busy = true;
  1569. sei(); // Re enable interrupts (normally disabled while inside an interrupt handler)
  1570. // If there is no current block, attempt to pop one from the buffer
  1571. if (current_block == NULL) {
  1572. // Anything in the buffer?
  1573. current_block = plan_get_current_block();
  1574. if (current_block != NULL) {
  1575. trapezoid_generator_reset();
  1576. counter_x = -(current_block->step_event_count >> 1);
  1577. counter_y = counter_x;
  1578. counter_z = counter_x;
  1579. counter_e = counter_x;
  1580. step_events_completed = 0;
  1581. e_steps = 0;
  1582. }
  1583. else {
  1584. DISABLE_STEPPER_DRIVER_INTERRUPT();
  1585. }
  1586. }
  1587. if (current_block != NULL) {
  1588. // Set directions TO DO This should be done once during init of trapezoid. Endstops -> interrupt
  1589. out_bits = current_block->direction_bits;
  1590. #ifdef ADVANCE
  1591. // Calculate E early.
  1592. counter_e += current_block->steps_e;
  1593. if (counter_e > 0) {
  1594. counter_e -= current_block->step_event_count;
  1595. if ((out_bits & (1<<E_AXIS)) != 0) { // - direction
  1596. CRITICAL_SECTION_START;
  1597. e_steps--;
  1598. CRITICAL_SECTION_END;
  1599. }
  1600. else {
  1601. CRITICAL_SECTION_START;
  1602. e_steps++;
  1603. CRITICAL_SECTION_END;
  1604. }
  1605. }
  1606. // Do E steps + advance steps
  1607. CRITICAL_SECTION_START;
  1608. e_steps += ((advance >> 16) - old_advance);
  1609. CRITICAL_SECTION_END;
  1610. old_advance = advance >> 16;
  1611. #endif //ADVANCE
  1612. // Set direction en check limit switches
  1613. if ((out_bits & (1<<X_AXIS)) != 0) { // -direction
  1614. WRITE(X_DIR_PIN, INVERT_X_DIR);
  1615. if(READ(X_MIN_PIN) != ENDSTOPS_INVERTING) {
  1616. step_events_completed = current_block->step_event_count;
  1617. }
  1618. }
  1619. else // +direction
  1620. WRITE(X_DIR_PIN,!INVERT_X_DIR);
  1621. if ((out_bits & (1<<Y_AXIS)) != 0) { // -direction
  1622. WRITE(Y_DIR_PIN,INVERT_Y_DIR);
  1623. if(READ(Y_MIN_PIN) != ENDSTOPS_INVERTING) {
  1624. step_events_completed = current_block->step_event_count;
  1625. }
  1626. }
  1627. else // +direction
  1628. WRITE(Y_DIR_PIN,!INVERT_Y_DIR);
  1629. if ((out_bits & (1<<Z_AXIS)) != 0) { // -direction
  1630. WRITE(Z_DIR_PIN,INVERT_Z_DIR);
  1631. if(READ(Z_MIN_PIN) != ENDSTOPS_INVERTING) {
  1632. step_events_completed = current_block->step_event_count;
  1633. }
  1634. }
  1635. else // +direction
  1636. WRITE(Z_DIR_PIN,!INVERT_Z_DIR);
  1637. #ifndef ADVANCE
  1638. if ((out_bits & (1<<E_AXIS)) != 0) // -direction
  1639. WRITE(E_DIR_PIN,INVERT_E_DIR);
  1640. else // +direction
  1641. WRITE(E_DIR_PIN,!INVERT_E_DIR);
  1642. #endif //!ADVANCE
  1643. counter_x += current_block->steps_x;
  1644. if (counter_x > 0) {
  1645. WRITE(X_STEP_PIN, HIGH);
  1646. counter_x -= current_block->step_event_count;
  1647. WRITE(X_STEP_PIN, LOW);
  1648. }
  1649. counter_y += current_block->steps_y;
  1650. if (counter_y > 0) {
  1651. WRITE(Y_STEP_PIN, HIGH);
  1652. counter_y -= current_block->step_event_count;
  1653. WRITE(Y_STEP_PIN, LOW);
  1654. }
  1655. counter_z += current_block->steps_z;
  1656. if (counter_z > 0) {
  1657. WRITE(Z_STEP_PIN, HIGH);
  1658. counter_z -= current_block->step_event_count;
  1659. WRITE(Z_STEP_PIN, LOW);
  1660. }
  1661. #ifndef ADVANCE
  1662. counter_e += current_block->steps_e;
  1663. if (counter_e > 0) {
  1664. WRITE(E_STEP_PIN, HIGH);
  1665. counter_e -= current_block->step_event_count;
  1666. WRITE(E_STEP_PIN, LOW);
  1667. }
  1668. #endif //!ADVANCE
  1669. // Calculare new timer value
  1670. unsigned short timer;
  1671. unsigned short step_rate;
  1672. if (step_events_completed < accelerate_until) {
  1673. MultiU24X24toH16(acc_step_rate, acceleration_time, acceleration_rate);
  1674. acc_step_rate += initial_rate;
  1675. // upper limit
  1676. if(acc_step_rate > nominal_rate)
  1677. acc_step_rate = nominal_rate;
  1678. // step_rate to timer interval
  1679. timer = calc_timer(acc_step_rate);
  1680. advance += advance_rate;
  1681. acceleration_time += timer;
  1682. OCR1A = timer;
  1683. }
  1684. else if (step_events_completed >= decelerate_after) {
  1685. MultiU24X24toH16(step_rate, deceleration_time, acceleration_rate);
  1686. if(step_rate > acc_step_rate) { // Check step_rate stays positive
  1687. step_rate = final_rate;
  1688. }
  1689. else {
  1690. step_rate = acc_step_rate - step_rate; // Decelerate from aceleration end point.
  1691. }
  1692. // lower limit
  1693. if(step_rate < final_rate)
  1694. step_rate = final_rate;
  1695. // step_rate to timer interval
  1696. timer = calc_timer(step_rate);
  1697. #ifdef ADVANCE
  1698. advance -= advance_rate;
  1699. if(advance < final_advance)
  1700. advance = final_advance;
  1701. #endif //ADVANCE
  1702. deceleration_time += timer;
  1703. OCR1A = timer;
  1704. }
  1705. // If current block is finished, reset pointer
  1706. step_events_completed += 1;
  1707. if (step_events_completed >= current_block->step_event_count) {
  1708. current_block = NULL;
  1709. plan_discard_current_block();
  1710. }
  1711. }
  1712. busy=false;
  1713. }
  1714. #ifdef ADVANCE
  1715. unsigned char old_OCR0A;
  1716. // Timer interrupt for E. e_steps is set in the main routine;
  1717. // Timer 0 is shared with millies
  1718. ISR(TIMER0_COMPA_vect)
  1719. {
  1720. // Critical section needed because Timer 1 interrupt has higher priority.
  1721. // The pin set functions are placed on trategic position to comply with the stepper driver timing.
  1722. WRITE(E_STEP_PIN, LOW);
  1723. // Set E direction (Depends on E direction + advance)
  1724. if (e_steps < 0) {
  1725. WRITE(E_DIR_PIN,INVERT_E_DIR);
  1726. e_steps++;
  1727. WRITE(E_STEP_PIN, HIGH);
  1728. }
  1729. if (e_steps > 0) {
  1730. WRITE(E_DIR_PIN,!INVERT_E_DIR);
  1731. e_steps--;
  1732. WRITE(E_STEP_PIN, HIGH);
  1733. }
  1734. old_OCR0A += 25; // 10kHz interrupt
  1735. OCR0A = old_OCR0A;
  1736. }
  1737. #endif // ADVANCE
  1738. void st_init()
  1739. {
  1740. // waveform generation = 0100 = CTC
  1741. TCCR1B &= ~(1<<WGM13);
  1742. TCCR1B |= (1<<WGM12);
  1743. TCCR1A &= ~(1<<WGM11);
  1744. TCCR1A &= ~(1<<WGM10);
  1745. // output mode = 00 (disconnected)
  1746. TCCR1A &= ~(3<<COM1A0);
  1747. TCCR1A &= ~(3<<COM1B0);
  1748. TCCR1B = (TCCR1B & ~(0x07<<CS10)) | (2<<CS10); // 2MHz timer
  1749. OCR1A = 0x4000;
  1750. DISABLE_STEPPER_DRIVER_INTERRUPT();
  1751. #ifdef ADVANCE
  1752. e_steps = 0;
  1753. TIMSK0 |= (1<<OCIE0A);
  1754. #endif //ADVANCE
  1755. sei();
  1756. }
  1757. // Block until all buffered steps are executed
  1758. void st_synchronize()
  1759. {
  1760. while(plan_get_current_block()) {
  1761. manage_heater();
  1762. manage_inactivity(1);
  1763. }
  1764. }
  1765. // Temperature loop
  1766. void tp_init()
  1767. {
  1768. DIDR0 = 1<<5; // TEMP_0_PIN for GEN6
  1769. ADMUX = ((1 << REFS0) | (5 & 0x07));
  1770. ADCSRA = 1<<ADEN | 1<<ADSC | 1<<ADIF | 0x07; // ADC enable, Clear interrupt, 1/128 prescaler.
  1771. TCCR2B = 0; //Stop timer in case of running
  1772. #ifdef PIDTEMP
  1773. TCCR2A = 0x23; //OC2A disable; FastPWM noninverting; FastPWM mode 7
  1774. #else
  1775. TCCR2A = 0x03; //OC2A disable; FastPWM noninverting; FastPWM mode 7
  1776. #endif //PIDTEMP
  1777. OCR2A = 156; //Period is ~10ms
  1778. OCR2B = 0; //Duty Cycle for heater pin is 0 (startup)
  1779. TIMSK2 = 0x01; //Enable overflow interrupt
  1780. TCCR2B = 0x0F; //1/1024 prescaler, start
  1781. }
  1782. static unsigned char temp_count = 0;
  1783. static unsigned long raw_temp_value = 0;
  1784. ISR(TIMER2_OVF_vect)
  1785. {
  1786. // uint8_t low, high;
  1787. // low = ADCL;
  1788. // high = ADCH;
  1789. raw_temp_value += ADC;
  1790. // raw_temp_value = (ADCH <<8) | ADCL;
  1791. ADCSRA = 1<<ADEN | 1<<ADSC | 1<<ADIF | 0x07; // ADC enable, Clear interrupt, Enable Interrupt, 1/128 prescaler.
  1792. // raw_temp_value += (high <<8) | low;
  1793. temp_count++;
  1794. if(temp_count >= 16)
  1795. {
  1796. current_raw = 16383 - raw_temp_value;
  1797. temp_meas_ready = true;
  1798. temp_count = 0;
  1799. raw_temp_value = 0;
  1800. #ifdef MAXTEMP
  1801. if(current_raw >= maxttemp) {
  1802. target_raw = 0;
  1803. #ifdef PIDTEMP
  1804. OCR2B = 0;
  1805. #else
  1806. WRITE(HEATER_0_PIN,LOW);
  1807. #endif //PIDTEMP
  1808. }
  1809. #endif //MAXTEMP
  1810. #ifdef MINTEMP
  1811. if(current_raw <= minttemp) {
  1812. target_raw = 0;
  1813. #ifdef PIDTEMP
  1814. OCR2B = 0;
  1815. #else
  1816. WRITE(HEATER_0_PIN,LOW);
  1817. #endif //PIDTEMP
  1818. }
  1819. #endif //MAXTEMP
  1820. #ifndef PIDTEMP
  1821. if(current_raw >= target_raw)
  1822. {
  1823. WRITE(HEATER_0_PIN,LOW);
  1824. }
  1825. else
  1826. {
  1827. WRITE(HEATER_0_PIN,HIGH);
  1828. }
  1829. #endif //PIDTEMP
  1830. }
  1831. }