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.

stepper.cpp 32KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075
  1. /*
  2. stepper.c - stepper motor driver: executes motion plans using stepper motors
  3. Part of Grbl
  4. Copyright (c) 2009-2011 Simen Svale Skogsrud
  5. Grbl is free software: you can redistribute it and/or modify
  6. it under the terms of the GNU General Public License as published by
  7. the Free Software Foundation, either version 3 of the License, or
  8. (at your option) any later version.
  9. Grbl is distributed in the hope that it will be useful,
  10. but WITHOUT ANY WARRANTY; without even the implied warranty of
  11. MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  12. GNU General Public License for more details.
  13. You should have received a copy of the GNU General Public License
  14. along with Grbl. If not, see <http://www.gnu.org/licenses/>.
  15. */
  16. /* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith
  17. and Philipp Tiefenbacher. */
  18. #include "Marlin.h"
  19. #include "stepper.h"
  20. #include "planner.h"
  21. #include "temperature.h"
  22. #include "ultralcd.h"
  23. #include "language.h"
  24. #include "cardreader.h"
  25. #include "speed_lookuptable.h"
  26. #if DIGIPOTSS_PIN > -1
  27. #include <SPI.h>
  28. #endif
  29. //===========================================================================
  30. //=============================public variables ============================
  31. //===========================================================================
  32. block_t *current_block; // A pointer to the block currently being traced
  33. //===========================================================================
  34. //=============================private variables ============================
  35. //===========================================================================
  36. //static makes it inpossible to be called from outside of this file by extern.!
  37. // Variables used by The Stepper Driver Interrupt
  38. static unsigned char out_bits; // The next stepping-bits to be output
  39. static long counter_x, // Counter variables for the bresenham line tracer
  40. counter_y,
  41. counter_z,
  42. counter_e;
  43. volatile static unsigned long step_events_completed; // The number of step events executed in the current block
  44. #ifdef ADVANCE
  45. static long advance_rate, advance, final_advance = 0;
  46. static long old_advance = 0;
  47. static long e_steps[3];
  48. #endif
  49. static long acceleration_time, deceleration_time;
  50. //static unsigned long accelerate_until, decelerate_after, acceleration_rate, initial_rate, final_rate, nominal_rate;
  51. static unsigned short acc_step_rate; // needed for deccelaration start point
  52. static char step_loops;
  53. static unsigned short OCR1A_nominal;
  54. static unsigned short step_loops_nominal;
  55. volatile long endstops_trigsteps[3]={0,0,0};
  56. volatile long endstops_stepsTotal,endstops_stepsDone;
  57. static volatile bool endstop_x_hit=false;
  58. static volatile bool endstop_y_hit=false;
  59. static volatile bool endstop_z_hit=false;
  60. #ifdef ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED
  61. bool abort_on_endstop_hit = false;
  62. #endif
  63. static bool old_x_min_endstop=false;
  64. static bool old_x_max_endstop=false;
  65. static bool old_y_min_endstop=false;
  66. static bool old_y_max_endstop=false;
  67. static bool old_z_min_endstop=false;
  68. static bool old_z_max_endstop=false;
  69. static bool check_endstops = true;
  70. volatile long count_position[NUM_AXIS] = { 0, 0, 0, 0};
  71. volatile signed char count_direction[NUM_AXIS] = { 1, 1, 1, 1};
  72. //===========================================================================
  73. //=============================functions ============================
  74. //===========================================================================
  75. #define CHECK_ENDSTOPS if(check_endstops)
  76. // intRes = intIn1 * intIn2 >> 16
  77. // uses:
  78. // r26 to store 0
  79. // r27 to store the byte 1 of the 24 bit result
  80. #define MultiU16X8toH16(intRes, charIn1, intIn2) \
  81. asm volatile ( \
  82. "clr r26 \n\t" \
  83. "mul %A1, %B2 \n\t" \
  84. "movw %A0, r0 \n\t" \
  85. "mul %A1, %A2 \n\t" \
  86. "add %A0, r1 \n\t" \
  87. "adc %B0, r26 \n\t" \
  88. "lsr r0 \n\t" \
  89. "adc %A0, r26 \n\t" \
  90. "adc %B0, r26 \n\t" \
  91. "clr r1 \n\t" \
  92. : \
  93. "=&r" (intRes) \
  94. : \
  95. "d" (charIn1), \
  96. "d" (intIn2) \
  97. : \
  98. "r26" \
  99. )
  100. // intRes = longIn1 * longIn2 >> 24
  101. // uses:
  102. // r26 to store 0
  103. // r27 to store the byte 1 of the 48bit result
  104. #define MultiU24X24toH16(intRes, longIn1, longIn2) \
  105. asm volatile ( \
  106. "clr r26 \n\t" \
  107. "mul %A1, %B2 \n\t" \
  108. "mov r27, r1 \n\t" \
  109. "mul %B1, %C2 \n\t" \
  110. "movw %A0, r0 \n\t" \
  111. "mul %C1, %C2 \n\t" \
  112. "add %B0, r0 \n\t" \
  113. "mul %C1, %B2 \n\t" \
  114. "add %A0, r0 \n\t" \
  115. "adc %B0, r1 \n\t" \
  116. "mul %A1, %C2 \n\t" \
  117. "add r27, r0 \n\t" \
  118. "adc %A0, r1 \n\t" \
  119. "adc %B0, r26 \n\t" \
  120. "mul %B1, %B2 \n\t" \
  121. "add r27, r0 \n\t" \
  122. "adc %A0, r1 \n\t" \
  123. "adc %B0, r26 \n\t" \
  124. "mul %C1, %A2 \n\t" \
  125. "add r27, r0 \n\t" \
  126. "adc %A0, r1 \n\t" \
  127. "adc %B0, r26 \n\t" \
  128. "mul %B1, %A2 \n\t" \
  129. "add r27, r1 \n\t" \
  130. "adc %A0, r26 \n\t" \
  131. "adc %B0, r26 \n\t" \
  132. "lsr r27 \n\t" \
  133. "adc %A0, r26 \n\t" \
  134. "adc %B0, r26 \n\t" \
  135. "clr r1 \n\t" \
  136. : \
  137. "=&r" (intRes) \
  138. : \
  139. "d" (longIn1), \
  140. "d" (longIn2) \
  141. : \
  142. "r26" , "r27" \
  143. )
  144. // Some useful constants
  145. #define ENABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 |= (1<<OCIE1A)
  146. #define DISABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 &= ~(1<<OCIE1A)
  147. void checkHitEndstops()
  148. {
  149. if( endstop_x_hit || endstop_y_hit || endstop_z_hit) {
  150. SERIAL_ECHO_START;
  151. SERIAL_ECHOPGM(MSG_ENDSTOPS_HIT);
  152. if(endstop_x_hit) {
  153. SERIAL_ECHOPAIR(" X:",(float)endstops_trigsteps[X_AXIS]/axis_steps_per_unit[X_AXIS]);
  154. LCD_MESSAGEPGM(MSG_ENDSTOPS_HIT "X");
  155. }
  156. if(endstop_y_hit) {
  157. SERIAL_ECHOPAIR(" Y:",(float)endstops_trigsteps[Y_AXIS]/axis_steps_per_unit[Y_AXIS]);
  158. LCD_MESSAGEPGM(MSG_ENDSTOPS_HIT "Y");
  159. }
  160. if(endstop_z_hit) {
  161. SERIAL_ECHOPAIR(" Z:",(float)endstops_trigsteps[Z_AXIS]/axis_steps_per_unit[Z_AXIS]);
  162. LCD_MESSAGEPGM(MSG_ENDSTOPS_HIT "Z");
  163. }
  164. SERIAL_ECHOLN("");
  165. endstop_x_hit=false;
  166. endstop_y_hit=false;
  167. endstop_z_hit=false;
  168. #ifdef ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED
  169. if (abort_on_endstop_hit)
  170. {
  171. card.sdprinting = false;
  172. card.closefile();
  173. quickStop();
  174. setTargetHotend0(0);
  175. setTargetHotend1(0);
  176. setTargetHotend2(0);
  177. }
  178. #endif
  179. }
  180. }
  181. void endstops_hit_on_purpose()
  182. {
  183. endstop_x_hit=false;
  184. endstop_y_hit=false;
  185. endstop_z_hit=false;
  186. }
  187. void enable_endstops(bool check)
  188. {
  189. check_endstops = check;
  190. }
  191. // __________________________
  192. // /| |\ _________________ ^
  193. // / | | \ /| |\ |
  194. // / | | \ / | | \ s
  195. // / | | | | | \ p
  196. // / | | | | | \ e
  197. // +-----+------------------------+---+--+---------------+----+ e
  198. // | BLOCK 1 | BLOCK 2 | d
  199. //
  200. // time ----->
  201. //
  202. // The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates
  203. // first block->accelerate_until step_events_completed, then keeps going at constant speed until
  204. // step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset.
  205. // The slope of acceleration is calculated with the leib ramp alghorithm.
  206. void st_wake_up() {
  207. // TCNT1 = 0;
  208. ENABLE_STEPPER_DRIVER_INTERRUPT();
  209. }
  210. void step_wait(){
  211. for(int8_t i=0; i < 6; i++){
  212. }
  213. }
  214. FORCE_INLINE unsigned short calc_timer(unsigned short step_rate) {
  215. unsigned short timer;
  216. if(step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY;
  217. if(step_rate > 20000) { // If steprate > 20kHz >> step 4 times
  218. step_rate = (step_rate >> 2)&0x3fff;
  219. step_loops = 4;
  220. }
  221. else if(step_rate > 10000) { // If steprate > 10kHz >> step 2 times
  222. step_rate = (step_rate >> 1)&0x7fff;
  223. step_loops = 2;
  224. }
  225. else {
  226. step_loops = 1;
  227. }
  228. if(step_rate < (F_CPU/500000)) step_rate = (F_CPU/500000);
  229. step_rate -= (F_CPU/500000); // Correct for minimal speed
  230. if(step_rate >= (8*256)){ // higher step rate
  231. unsigned short table_address = (unsigned short)&speed_lookuptable_fast[(unsigned char)(step_rate>>8)][0];
  232. unsigned char tmp_step_rate = (step_rate & 0x00ff);
  233. unsigned short gain = (unsigned short)pgm_read_word_near(table_address+2);
  234. MultiU16X8toH16(timer, tmp_step_rate, gain);
  235. timer = (unsigned short)pgm_read_word_near(table_address) - timer;
  236. }
  237. else { // lower step rates
  238. unsigned short table_address = (unsigned short)&speed_lookuptable_slow[0][0];
  239. table_address += ((step_rate)>>1) & 0xfffc;
  240. timer = (unsigned short)pgm_read_word_near(table_address);
  241. timer -= (((unsigned short)pgm_read_word_near(table_address+2) * (unsigned char)(step_rate & 0x0007))>>3);
  242. }
  243. if(timer < 100) { timer = 100; MYSERIAL.print(MSG_STEPPER_TO_HIGH); MYSERIAL.println(step_rate); }//(20kHz this should never happen)
  244. return timer;
  245. }
  246. // Initializes the trapezoid generator from the current block. Called whenever a new
  247. // block begins.
  248. FORCE_INLINE void trapezoid_generator_reset() {
  249. #ifdef ADVANCE
  250. advance = current_block->initial_advance;
  251. final_advance = current_block->final_advance;
  252. // Do E steps + advance steps
  253. e_steps[current_block->active_extruder] += ((advance >>8) - old_advance);
  254. old_advance = advance >>8;
  255. #endif
  256. deceleration_time = 0;
  257. // step_rate to timer interval
  258. OCR1A_nominal = calc_timer(current_block->nominal_rate);
  259. // make a note of the number of step loops required at nominal speed
  260. step_loops_nominal = step_loops;
  261. acc_step_rate = current_block->initial_rate;
  262. acceleration_time = calc_timer(acc_step_rate);
  263. OCR1A = acceleration_time;
  264. // SERIAL_ECHO_START;
  265. // SERIAL_ECHOPGM("advance :");
  266. // SERIAL_ECHO(current_block->advance/256.0);
  267. // SERIAL_ECHOPGM("advance rate :");
  268. // SERIAL_ECHO(current_block->advance_rate/256.0);
  269. // SERIAL_ECHOPGM("initial advance :");
  270. // SERIAL_ECHO(current_block->initial_advance/256.0);
  271. // SERIAL_ECHOPGM("final advance :");
  272. // SERIAL_ECHOLN(current_block->final_advance/256.0);
  273. }
  274. // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse.
  275. // It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately.
  276. ISR(TIMER1_COMPA_vect)
  277. {
  278. // If there is no current block, attempt to pop one from the buffer
  279. if (current_block == NULL) {
  280. // Anything in the buffer?
  281. current_block = plan_get_current_block();
  282. if (current_block != NULL) {
  283. current_block->busy = true;
  284. trapezoid_generator_reset();
  285. counter_x = -(current_block->step_event_count >> 1);
  286. counter_y = counter_x;
  287. counter_z = counter_x;
  288. counter_e = counter_x;
  289. step_events_completed = 0;
  290. #ifdef Z_LATE_ENABLE
  291. if(current_block->steps_z > 0) {
  292. enable_z();
  293. OCR1A = 2000; //1ms wait
  294. return;
  295. }
  296. #endif
  297. // #ifdef ADVANCE
  298. // e_steps[current_block->active_extruder] = 0;
  299. // #endif
  300. }
  301. else {
  302. OCR1A=2000; // 1kHz.
  303. }
  304. }
  305. if (current_block != NULL) {
  306. // Set directions TO DO This should be done once during init of trapezoid. Endstops -> interrupt
  307. out_bits = current_block->direction_bits;
  308. // Set direction en check limit switches
  309. if ((out_bits & (1<<X_AXIS)) != 0) { // stepping along -X axis
  310. #if !defined COREXY //NOT COREXY
  311. WRITE(X_DIR_PIN, INVERT_X_DIR);
  312. #endif
  313. count_direction[X_AXIS]=-1;
  314. CHECK_ENDSTOPS
  315. {
  316. #if X_MIN_PIN > -1
  317. bool x_min_endstop=(READ(X_MIN_PIN) != X_ENDSTOPS_INVERTING);
  318. if(x_min_endstop && old_x_min_endstop && (current_block->steps_x > 0)) {
  319. endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
  320. endstop_x_hit=true;
  321. step_events_completed = current_block->step_event_count;
  322. }
  323. old_x_min_endstop = x_min_endstop;
  324. #endif
  325. }
  326. }
  327. else { // +direction
  328. #if !defined COREXY //NOT COREXY
  329. WRITE(X_DIR_PIN,!INVERT_X_DIR);
  330. #endif
  331. count_direction[X_AXIS]=1;
  332. CHECK_ENDSTOPS
  333. {
  334. #if X_MAX_PIN > -1
  335. bool x_max_endstop=(READ(X_MAX_PIN) != X_ENDSTOPS_INVERTING);
  336. if(x_max_endstop && old_x_max_endstop && (current_block->steps_x > 0)){
  337. endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
  338. endstop_x_hit=true;
  339. step_events_completed = current_block->step_event_count;
  340. }
  341. old_x_max_endstop = x_max_endstop;
  342. #endif
  343. }
  344. }
  345. if ((out_bits & (1<<Y_AXIS)) != 0) { // -direction
  346. #if !defined COREXY //NOT COREXY
  347. WRITE(Y_DIR_PIN,INVERT_Y_DIR);
  348. #endif
  349. count_direction[Y_AXIS]=-1;
  350. CHECK_ENDSTOPS
  351. {
  352. #if Y_MIN_PIN > -1
  353. bool y_min_endstop=(READ(Y_MIN_PIN) != Y_ENDSTOPS_INVERTING);
  354. if(y_min_endstop && old_y_min_endstop && (current_block->steps_y > 0)) {
  355. endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
  356. endstop_y_hit=true;
  357. step_events_completed = current_block->step_event_count;
  358. }
  359. old_y_min_endstop = y_min_endstop;
  360. #endif
  361. }
  362. }
  363. else { // +direction
  364. #if !defined COREXY //NOT COREXY
  365. WRITE(Y_DIR_PIN,!INVERT_Y_DIR);
  366. #endif
  367. count_direction[Y_AXIS]=1;
  368. CHECK_ENDSTOPS
  369. {
  370. #if Y_MAX_PIN > -1
  371. bool y_max_endstop=(READ(Y_MAX_PIN) != Y_ENDSTOPS_INVERTING);
  372. if(y_max_endstop && old_y_max_endstop && (current_block->steps_y > 0)){
  373. endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
  374. endstop_y_hit=true;
  375. step_events_completed = current_block->step_event_count;
  376. }
  377. old_y_max_endstop = y_max_endstop;
  378. #endif
  379. }
  380. }
  381. #ifdef COREXY //coreXY kinematics defined
  382. if((current_block->steps_x >= current_block->steps_y)&&((out_bits & (1<<X_AXIS)) == 0)){ //+X is major axis
  383. WRITE(X_DIR_PIN, !INVERT_X_DIR);
  384. WRITE(Y_DIR_PIN, !INVERT_Y_DIR);
  385. }
  386. if((current_block->steps_x >= current_block->steps_y)&&((out_bits & (1<<X_AXIS)) != 0)){ //-X is major axis
  387. WRITE(X_DIR_PIN, INVERT_X_DIR);
  388. WRITE(Y_DIR_PIN, INVERT_Y_DIR);
  389. }
  390. if((current_block->steps_y > current_block->steps_x)&&((out_bits & (1<<Y_AXIS)) == 0)){ //+Y is major axis
  391. WRITE(X_DIR_PIN, !INVERT_X_DIR);
  392. WRITE(Y_DIR_PIN, INVERT_Y_DIR);
  393. }
  394. if((current_block->steps_y > current_block->steps_x)&&((out_bits & (1<<Y_AXIS)) != 0)){ //-Y is major axis
  395. WRITE(X_DIR_PIN, INVERT_X_DIR);
  396. WRITE(Y_DIR_PIN, !INVERT_Y_DIR);
  397. }
  398. #endif //coreXY
  399. if ((out_bits & (1<<Z_AXIS)) != 0) { // -direction
  400. WRITE(Z_DIR_PIN,INVERT_Z_DIR);
  401. #ifdef Z_DUAL_STEPPER_DRIVERS
  402. WRITE(Z2_DIR_PIN,INVERT_Z_DIR);
  403. #endif
  404. count_direction[Z_AXIS]=-1;
  405. CHECK_ENDSTOPS
  406. {
  407. #if Z_MIN_PIN > -1
  408. bool z_min_endstop=(READ(Z_MIN_PIN) != Z_ENDSTOPS_INVERTING);
  409. if(z_min_endstop && old_z_min_endstop && (current_block->steps_z > 0)) {
  410. endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
  411. endstop_z_hit=true;
  412. step_events_completed = current_block->step_event_count;
  413. }
  414. old_z_min_endstop = z_min_endstop;
  415. #endif
  416. }
  417. }
  418. else { // +direction
  419. WRITE(Z_DIR_PIN,!INVERT_Z_DIR);
  420. #ifdef Z_DUAL_STEPPER_DRIVERS
  421. WRITE(Z2_DIR_PIN,!INVERT_Z_DIR);
  422. #endif
  423. count_direction[Z_AXIS]=1;
  424. CHECK_ENDSTOPS
  425. {
  426. #if Z_MAX_PIN > -1
  427. bool z_max_endstop=(READ(Z_MAX_PIN) != Z_ENDSTOPS_INVERTING);
  428. if(z_max_endstop && old_z_max_endstop && (current_block->steps_z > 0)) {
  429. endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
  430. endstop_z_hit=true;
  431. step_events_completed = current_block->step_event_count;
  432. }
  433. old_z_max_endstop = z_max_endstop;
  434. #endif
  435. }
  436. }
  437. #ifndef ADVANCE
  438. if ((out_bits & (1<<E_AXIS)) != 0) { // -direction
  439. REV_E_DIR();
  440. count_direction[E_AXIS]=-1;
  441. }
  442. else { // +direction
  443. NORM_E_DIR();
  444. count_direction[E_AXIS]=1;
  445. }
  446. #endif //!ADVANCE
  447. for(int8_t i=0; i < step_loops; i++) { // Take multiple steps per interrupt (For high speed moves)
  448. #ifndef AT90USB
  449. MSerial.checkRx(); // Check for serial chars.
  450. #endif
  451. #ifdef ADVANCE
  452. counter_e += current_block->steps_e;
  453. if (counter_e > 0) {
  454. counter_e -= current_block->step_event_count;
  455. if ((out_bits & (1<<E_AXIS)) != 0) { // - direction
  456. e_steps[current_block->active_extruder]--;
  457. }
  458. else {
  459. e_steps[current_block->active_extruder]++;
  460. }
  461. }
  462. #endif //ADVANCE
  463. #if !defined COREXY
  464. counter_x += current_block->steps_x;
  465. if (counter_x > 0) {
  466. WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN);
  467. counter_x -= current_block->step_event_count;
  468. count_position[X_AXIS]+=count_direction[X_AXIS];
  469. WRITE(X_STEP_PIN, INVERT_X_STEP_PIN);
  470. }
  471. counter_y += current_block->steps_y;
  472. if (counter_y > 0) {
  473. WRITE(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
  474. counter_y -= current_block->step_event_count;
  475. count_position[Y_AXIS]+=count_direction[Y_AXIS];
  476. WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN);
  477. }
  478. #endif
  479. #ifdef COREXY
  480. counter_x += current_block->steps_x;
  481. counter_y += current_block->steps_y;
  482. if ((counter_x > 0)&&!(counter_y>0)){ //X step only
  483. WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN);
  484. WRITE(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
  485. counter_x -= current_block->step_event_count;
  486. count_position[X_AXIS]+=count_direction[X_AXIS];
  487. WRITE(X_STEP_PIN, INVERT_X_STEP_PIN);
  488. WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN);
  489. }
  490. if (!(counter_x > 0)&&(counter_y>0)){ //Y step only
  491. WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN);
  492. WRITE(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
  493. counter_y -= current_block->step_event_count;
  494. count_position[Y_AXIS]+=count_direction[Y_AXIS];
  495. WRITE(X_STEP_PIN, INVERT_X_STEP_PIN);
  496. WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN);
  497. }
  498. if ((counter_x > 0)&&(counter_y>0)){ //step in both axes
  499. if (((out_bits & (1<<X_AXIS)) == 0)^((out_bits & (1<<Y_AXIS)) == 0)){ //X and Y in different directions
  500. WRITE(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
  501. counter_x -= current_block->step_event_count;
  502. WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN);
  503. step_wait();
  504. count_position[X_AXIS]+=count_direction[X_AXIS];
  505. count_position[Y_AXIS]+=count_direction[Y_AXIS];
  506. WRITE(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
  507. counter_y -= current_block->step_event_count;
  508. WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN);
  509. }
  510. else{ //X and Y in same direction
  511. WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN);
  512. counter_x -= current_block->step_event_count;
  513. WRITE(X_STEP_PIN, INVERT_X_STEP_PIN) ;
  514. step_wait();
  515. count_position[X_AXIS]+=count_direction[X_AXIS];
  516. count_position[Y_AXIS]+=count_direction[Y_AXIS];
  517. WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN);
  518. counter_y -= current_block->step_event_count;
  519. WRITE(X_STEP_PIN, INVERT_X_STEP_PIN);
  520. }
  521. }
  522. #endif //corexy
  523. counter_z += current_block->steps_z;
  524. if (counter_z > 0) {
  525. WRITE(Z_STEP_PIN, !INVERT_Z_STEP_PIN);
  526. #ifdef Z_DUAL_STEPPER_DRIVERS
  527. WRITE(Z2_STEP_PIN, !INVERT_Z_STEP_PIN);
  528. #endif
  529. counter_z -= current_block->step_event_count;
  530. count_position[Z_AXIS]+=count_direction[Z_AXIS];
  531. WRITE(Z_STEP_PIN, INVERT_Z_STEP_PIN);
  532. #ifdef Z_DUAL_STEPPER_DRIVERS
  533. WRITE(Z2_STEP_PIN, INVERT_Z_STEP_PIN);
  534. #endif
  535. }
  536. #ifndef ADVANCE
  537. counter_e += current_block->steps_e;
  538. if (counter_e > 0) {
  539. WRITE_E_STEP(!INVERT_E_STEP_PIN);
  540. counter_e -= current_block->step_event_count;
  541. count_position[E_AXIS]+=count_direction[E_AXIS];
  542. WRITE_E_STEP(INVERT_E_STEP_PIN);
  543. }
  544. #endif //!ADVANCE
  545. step_events_completed += 1;
  546. if(step_events_completed >= current_block->step_event_count) break;
  547. }
  548. // Calculare new timer value
  549. unsigned short timer;
  550. unsigned short step_rate;
  551. if (step_events_completed <= (unsigned long int)current_block->accelerate_until) {
  552. MultiU24X24toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
  553. acc_step_rate += current_block->initial_rate;
  554. // upper limit
  555. if(acc_step_rate > current_block->nominal_rate)
  556. acc_step_rate = current_block->nominal_rate;
  557. // step_rate to timer interval
  558. timer = calc_timer(acc_step_rate);
  559. OCR1A = timer;
  560. acceleration_time += timer;
  561. #ifdef ADVANCE
  562. for(int8_t i=0; i < step_loops; i++) {
  563. advance += advance_rate;
  564. }
  565. //if(advance > current_block->advance) advance = current_block->advance;
  566. // Do E steps + advance steps
  567. e_steps[current_block->active_extruder] += ((advance >>8) - old_advance);
  568. old_advance = advance >>8;
  569. #endif
  570. }
  571. else if (step_events_completed > (unsigned long int)current_block->decelerate_after) {
  572. MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate);
  573. if(step_rate > acc_step_rate) { // Check step_rate stays positive
  574. step_rate = current_block->final_rate;
  575. }
  576. else {
  577. step_rate = acc_step_rate - step_rate; // Decelerate from aceleration end point.
  578. }
  579. // lower limit
  580. if(step_rate < current_block->final_rate)
  581. step_rate = current_block->final_rate;
  582. // step_rate to timer interval
  583. timer = calc_timer(step_rate);
  584. OCR1A = timer;
  585. deceleration_time += timer;
  586. #ifdef ADVANCE
  587. for(int8_t i=0; i < step_loops; i++) {
  588. advance -= advance_rate;
  589. }
  590. if(advance < final_advance) advance = final_advance;
  591. // Do E steps + advance steps
  592. e_steps[current_block->active_extruder] += ((advance >>8) - old_advance);
  593. old_advance = advance >>8;
  594. #endif //ADVANCE
  595. }
  596. else {
  597. OCR1A = OCR1A_nominal;
  598. // ensure we're running at the correct step rate, even if we just came off an acceleration
  599. step_loops = step_loops_nominal;
  600. }
  601. // If current block is finished, reset pointer
  602. if (step_events_completed >= current_block->step_event_count) {
  603. current_block = NULL;
  604. plan_discard_current_block();
  605. }
  606. }
  607. }
  608. #ifdef ADVANCE
  609. unsigned char old_OCR0A;
  610. // Timer interrupt for E. e_steps is set in the main routine;
  611. // Timer 0 is shared with millies
  612. ISR(TIMER0_COMPA_vect)
  613. {
  614. old_OCR0A += 52; // ~10kHz interrupt (250000 / 26 = 9615kHz)
  615. OCR0A = old_OCR0A;
  616. // Set E direction (Depends on E direction + advance)
  617. for(unsigned char i=0; i<4;i++) {
  618. if (e_steps[0] != 0) {
  619. WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN);
  620. if (e_steps[0] < 0) {
  621. WRITE(E0_DIR_PIN, INVERT_E0_DIR);
  622. e_steps[0]++;
  623. WRITE(E0_STEP_PIN, !INVERT_E_STEP_PIN);
  624. }
  625. else if (e_steps[0] > 0) {
  626. WRITE(E0_DIR_PIN, !INVERT_E0_DIR);
  627. e_steps[0]--;
  628. WRITE(E0_STEP_PIN, !INVERT_E_STEP_PIN);
  629. }
  630. }
  631. #if EXTRUDERS > 1
  632. if (e_steps[1] != 0) {
  633. WRITE(E1_STEP_PIN, INVERT_E_STEP_PIN);
  634. if (e_steps[1] < 0) {
  635. WRITE(E1_DIR_PIN, INVERT_E1_DIR);
  636. e_steps[1]++;
  637. WRITE(E1_STEP_PIN, !INVERT_E_STEP_PIN);
  638. }
  639. else if (e_steps[1] > 0) {
  640. WRITE(E1_DIR_PIN, !INVERT_E1_DIR);
  641. e_steps[1]--;
  642. WRITE(E1_STEP_PIN, !INVERT_E_STEP_PIN);
  643. }
  644. }
  645. #endif
  646. #if EXTRUDERS > 2
  647. if (e_steps[2] != 0) {
  648. WRITE(E2_STEP_PIN, INVERT_E_STEP_PIN);
  649. if (e_steps[2] < 0) {
  650. WRITE(E2_DIR_PIN, INVERT_E2_DIR);
  651. e_steps[2]++;
  652. WRITE(E2_STEP_PIN, !INVERT_E_STEP_PIN);
  653. }
  654. else if (e_steps[2] > 0) {
  655. WRITE(E2_DIR_PIN, !INVERT_E2_DIR);
  656. e_steps[2]--;
  657. WRITE(E2_STEP_PIN, !INVERT_E_STEP_PIN);
  658. }
  659. }
  660. #endif
  661. }
  662. }
  663. #endif // ADVANCE
  664. void st_init()
  665. {
  666. digipot_init(); //Initialize Digipot Motor Current
  667. microstep_init(); //Initialize Microstepping Pins
  668. //Initialize Dir Pins
  669. #if X_DIR_PIN > -1
  670. SET_OUTPUT(X_DIR_PIN);
  671. #endif
  672. #if Y_DIR_PIN > -1
  673. SET_OUTPUT(Y_DIR_PIN);
  674. #endif
  675. #if Z_DIR_PIN > -1
  676. SET_OUTPUT(Z_DIR_PIN);
  677. #if defined(Z_DUAL_STEPPER_DRIVERS) && (Z2_DIR_PIN > -1)
  678. SET_OUTPUT(Z2_DIR_PIN);
  679. #endif
  680. #endif
  681. #if E0_DIR_PIN > -1
  682. SET_OUTPUT(E0_DIR_PIN);
  683. #endif
  684. #if defined(E1_DIR_PIN) && (E1_DIR_PIN > -1)
  685. SET_OUTPUT(E1_DIR_PIN);
  686. #endif
  687. #if defined(E2_DIR_PIN) && (E2_DIR_PIN > -1)
  688. SET_OUTPUT(E2_DIR_PIN);
  689. #endif
  690. //Initialize Enable Pins - steppers default to disabled.
  691. #if (X_ENABLE_PIN > -1)
  692. SET_OUTPUT(X_ENABLE_PIN);
  693. if(!X_ENABLE_ON) WRITE(X_ENABLE_PIN,HIGH);
  694. #endif
  695. #if (Y_ENABLE_PIN > -1)
  696. SET_OUTPUT(Y_ENABLE_PIN);
  697. if(!Y_ENABLE_ON) WRITE(Y_ENABLE_PIN,HIGH);
  698. #endif
  699. #if (Z_ENABLE_PIN > -1)
  700. SET_OUTPUT(Z_ENABLE_PIN);
  701. if(!Z_ENABLE_ON) WRITE(Z_ENABLE_PIN,HIGH);
  702. #if defined(Z_DUAL_STEPPER_DRIVERS) && (Z2_ENABLE_PIN > -1)
  703. SET_OUTPUT(Z2_ENABLE_PIN);
  704. if(!Z_ENABLE_ON) WRITE(Z2_ENABLE_PIN,HIGH);
  705. #endif
  706. #endif
  707. #if (E0_ENABLE_PIN > -1)
  708. SET_OUTPUT(E0_ENABLE_PIN);
  709. if(!E_ENABLE_ON) WRITE(E0_ENABLE_PIN,HIGH);
  710. #endif
  711. #if defined(E1_ENABLE_PIN) && (E1_ENABLE_PIN > -1)
  712. SET_OUTPUT(E1_ENABLE_PIN);
  713. if(!E_ENABLE_ON) WRITE(E1_ENABLE_PIN,HIGH);
  714. #endif
  715. #if defined(E2_ENABLE_PIN) && (E2_ENABLE_PIN > -1)
  716. SET_OUTPUT(E2_ENABLE_PIN);
  717. if(!E_ENABLE_ON) WRITE(E2_ENABLE_PIN,HIGH);
  718. #endif
  719. //endstops and pullups
  720. #if X_MIN_PIN > -1
  721. SET_INPUT(X_MIN_PIN);
  722. #ifdef ENDSTOPPULLUP_XMIN
  723. WRITE(X_MIN_PIN,HIGH);
  724. #endif
  725. #endif
  726. #if Y_MIN_PIN > -1
  727. SET_INPUT(Y_MIN_PIN);
  728. #ifdef ENDSTOPPULLUP_YMIN
  729. WRITE(Y_MIN_PIN,HIGH);
  730. #endif
  731. #endif
  732. #if Z_MIN_PIN > -1
  733. SET_INPUT(Z_MIN_PIN);
  734. #ifdef ENDSTOPPULLUP_ZMIN
  735. WRITE(Z_MIN_PIN,HIGH);
  736. #endif
  737. #endif
  738. #if X_MAX_PIN > -1
  739. SET_INPUT(X_MAX_PIN);
  740. #ifdef ENDSTOPPULLUP_XMAX
  741. WRITE(X_MAX_PIN,HIGH);
  742. #endif
  743. #endif
  744. #if Y_MAX_PIN > -1
  745. SET_INPUT(Y_MAX_PIN);
  746. #ifdef ENDSTOPPULLUP_YMAX
  747. WRITE(Y_MAX_PIN,HIGH);
  748. #endif
  749. #endif
  750. #if Z_MAX_PIN > -1
  751. SET_INPUT(Z_MAX_PIN);
  752. #ifdef ENDSTOPPULLUP_ZMAX
  753. WRITE(Z_MAX_PIN,HIGH);
  754. #endif
  755. #endif
  756. //Initialize Step Pins
  757. #if (X_STEP_PIN > -1)
  758. SET_OUTPUT(X_STEP_PIN);
  759. WRITE(X_STEP_PIN,INVERT_X_STEP_PIN);
  760. disable_x();
  761. #endif
  762. #if (Y_STEP_PIN > -1)
  763. SET_OUTPUT(Y_STEP_PIN);
  764. WRITE(Y_STEP_PIN,INVERT_Y_STEP_PIN);
  765. disable_y();
  766. #endif
  767. #if (Z_STEP_PIN > -1)
  768. SET_OUTPUT(Z_STEP_PIN);
  769. WRITE(Z_STEP_PIN,INVERT_Z_STEP_PIN);
  770. #if defined(Z_DUAL_STEPPER_DRIVERS) && (Z2_STEP_PIN > -1)
  771. SET_OUTPUT(Z2_STEP_PIN);
  772. WRITE(Z2_STEP_PIN,INVERT_Z_STEP_PIN);
  773. #endif
  774. disable_z();
  775. #endif
  776. #if (E0_STEP_PIN > -1)
  777. SET_OUTPUT(E0_STEP_PIN);
  778. WRITE(E0_STEP_PIN,INVERT_E_STEP_PIN);
  779. disable_e0();
  780. #endif
  781. #if defined(E1_STEP_PIN) && (E1_STEP_PIN > -1)
  782. SET_OUTPUT(E1_STEP_PIN);
  783. WRITE(E1_STEP_PIN,INVERT_E_STEP_PIN);
  784. disable_e1();
  785. #endif
  786. #if defined(E2_STEP_PIN) && (E2_STEP_PIN > -1)
  787. SET_OUTPUT(E2_STEP_PIN);
  788. WRITE(E2_STEP_PIN,INVERT_E_STEP_PIN);
  789. disable_e2();
  790. #endif
  791. #ifdef CONTROLLERFAN_PIN
  792. SET_OUTPUT(CONTROLLERFAN_PIN); //Set pin used for driver cooling fan
  793. #endif
  794. // waveform generation = 0100 = CTC
  795. TCCR1B &= ~(1<<WGM13);
  796. TCCR1B |= (1<<WGM12);
  797. TCCR1A &= ~(1<<WGM11);
  798. TCCR1A &= ~(1<<WGM10);
  799. // output mode = 00 (disconnected)
  800. TCCR1A &= ~(3<<COM1A0);
  801. TCCR1A &= ~(3<<COM1B0);
  802. // Set the timer pre-scaler
  803. // Generally we use a divider of 8, resulting in a 2MHz timer
  804. // frequency on a 16MHz MCU. If you are going to change this, be
  805. // sure to regenerate speed_lookuptable.h with
  806. // create_speed_lookuptable.py
  807. TCCR1B = (TCCR1B & ~(0x07<<CS10)) | (2<<CS10);
  808. OCR1A = 0x4000;
  809. TCNT1 = 0;
  810. ENABLE_STEPPER_DRIVER_INTERRUPT();
  811. #ifdef ADVANCE
  812. #if defined(TCCR0A) && defined(WGM01)
  813. TCCR0A &= ~(1<<WGM01);
  814. TCCR0A &= ~(1<<WGM00);
  815. #endif
  816. e_steps[0] = 0;
  817. e_steps[1] = 0;
  818. e_steps[2] = 0;
  819. TIMSK0 |= (1<<OCIE0A);
  820. #endif //ADVANCE
  821. enable_endstops(true); // Start with endstops active. After homing they can be disabled
  822. sei();
  823. }
  824. // Block until all buffered steps are executed
  825. void st_synchronize()
  826. {
  827. while( blocks_queued()) {
  828. manage_heater();
  829. manage_inactivity();
  830. lcd_update();
  831. }
  832. }
  833. void st_set_position(const long &x, const long &y, const long &z, const long &e)
  834. {
  835. CRITICAL_SECTION_START;
  836. count_position[X_AXIS] = x;
  837. count_position[Y_AXIS] = y;
  838. count_position[Z_AXIS] = z;
  839. count_position[E_AXIS] = e;
  840. CRITICAL_SECTION_END;
  841. }
  842. void st_set_e_position(const long &e)
  843. {
  844. CRITICAL_SECTION_START;
  845. count_position[E_AXIS] = e;
  846. CRITICAL_SECTION_END;
  847. }
  848. long st_get_position(uint8_t axis)
  849. {
  850. long count_pos;
  851. CRITICAL_SECTION_START;
  852. count_pos = count_position[axis];
  853. CRITICAL_SECTION_END;
  854. return count_pos;
  855. }
  856. void finishAndDisableSteppers()
  857. {
  858. st_synchronize();
  859. disable_x();
  860. disable_y();
  861. disable_z();
  862. disable_e0();
  863. disable_e1();
  864. disable_e2();
  865. }
  866. void quickStop()
  867. {
  868. DISABLE_STEPPER_DRIVER_INTERRUPT();
  869. while(blocks_queued())
  870. plan_discard_current_block();
  871. current_block = NULL;
  872. ENABLE_STEPPER_DRIVER_INTERRUPT();
  873. }
  874. void digitalPotWrite(int address, int value) // From Arduino DigitalPotControl example
  875. {
  876. #if DIGIPOTSS_PIN > -1
  877. digitalWrite(DIGIPOTSS_PIN,LOW); // take the SS pin low to select the chip
  878. SPI.transfer(address); // send in the address and value via SPI:
  879. SPI.transfer(value);
  880. digitalWrite(DIGIPOTSS_PIN,HIGH); // take the SS pin high to de-select the chip:
  881. //delay(10);
  882. #endif
  883. }
  884. void digipot_init() //Initialize Digipot Motor Current
  885. {
  886. #if DIGIPOTSS_PIN > -1
  887. const uint8_t digipot_motor_current[] = DIGIPOT_MOTOR_CURRENT;
  888. SPI.begin();
  889. pinMode(DIGIPOTSS_PIN, OUTPUT);
  890. for(int i=0;i<=4;i++)
  891. //digitalPotWrite(digipot_ch[i], digipot_motor_current[i]);
  892. digipot_current(i,digipot_motor_current[i]);
  893. #endif
  894. }
  895. void digipot_current(uint8_t driver, int current)
  896. {
  897. #if DIGIPOTSS_PIN > -1
  898. const uint8_t digipot_ch[] = DIGIPOT_CHANNELS;
  899. digitalPotWrite(digipot_ch[driver], current);
  900. #endif
  901. }
  902. void microstep_init()
  903. {
  904. #if X_MS1_PIN > -1
  905. const uint8_t microstep_modes[] = MICROSTEP_MODES;
  906. pinMode(X_MS2_PIN,OUTPUT);
  907. pinMode(Y_MS2_PIN,OUTPUT);
  908. pinMode(Z_MS2_PIN,OUTPUT);
  909. pinMode(E0_MS2_PIN,OUTPUT);
  910. pinMode(E1_MS2_PIN,OUTPUT);
  911. for(int i=0;i<=4;i++) microstep_mode(i,microstep_modes[i]);
  912. #endif
  913. }
  914. void microstep_ms(uint8_t driver, int8_t ms1, int8_t ms2)
  915. {
  916. if(ms1 > -1) switch(driver)
  917. {
  918. case 0: digitalWrite( X_MS1_PIN,ms1); break;
  919. case 1: digitalWrite( Y_MS1_PIN,ms1); break;
  920. case 2: digitalWrite( Z_MS1_PIN,ms1); break;
  921. case 3: digitalWrite(E0_MS1_PIN,ms1); break;
  922. case 4: digitalWrite(E1_MS1_PIN,ms1); break;
  923. }
  924. if(ms2 > -1) switch(driver)
  925. {
  926. case 0: digitalWrite( X_MS2_PIN,ms2); break;
  927. case 1: digitalWrite( Y_MS2_PIN,ms2); break;
  928. case 2: digitalWrite( Z_MS2_PIN,ms2); break;
  929. case 3: digitalWrite(E0_MS2_PIN,ms2); break;
  930. case 4: digitalWrite(E1_MS2_PIN,ms2); break;
  931. }
  932. }
  933. void microstep_mode(uint8_t driver, uint8_t stepping_mode)
  934. {
  935. switch(stepping_mode)
  936. {
  937. case 1: microstep_ms(driver,MICROSTEP1); break;
  938. case 2: microstep_ms(driver,MICROSTEP2); break;
  939. case 4: microstep_ms(driver,MICROSTEP4); break;
  940. case 8: microstep_ms(driver,MICROSTEP8); break;
  941. case 16: microstep_ms(driver,MICROSTEP16); break;
  942. }
  943. }
  944. void microstep_readings()
  945. {
  946. SERIAL_PROTOCOLPGM("MS1,MS2 Pins\n");
  947. SERIAL_PROTOCOLPGM("X: ");
  948. SERIAL_PROTOCOL( digitalRead(X_MS1_PIN));
  949. SERIAL_PROTOCOLLN( digitalRead(X_MS2_PIN));
  950. SERIAL_PROTOCOLPGM("Y: ");
  951. SERIAL_PROTOCOL( digitalRead(Y_MS1_PIN));
  952. SERIAL_PROTOCOLLN( digitalRead(Y_MS2_PIN));
  953. SERIAL_PROTOCOLPGM("Z: ");
  954. SERIAL_PROTOCOL( digitalRead(Z_MS1_PIN));
  955. SERIAL_PROTOCOLLN( digitalRead(Z_MS2_PIN));
  956. SERIAL_PROTOCOLPGM("E0: ");
  957. SERIAL_PROTOCOL( digitalRead(E0_MS1_PIN));
  958. SERIAL_PROTOCOLLN( digitalRead(E0_MS2_PIN));
  959. SERIAL_PROTOCOLPGM("E1: ");
  960. SERIAL_PROTOCOL( digitalRead(E1_MS1_PIN));
  961. SERIAL_PROTOCOLLN( digitalRead(E1_MS2_PIN));
  962. }