stepper.cpp 25 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854
  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 "speed_lookuptable.h"
  25. //===========================================================================
  26. //=============================public variables ============================
  27. //===========================================================================
  28. block_t *current_block; // A pointer to the block currently being traced
  29. //===========================================================================
  30. //=============================private variables ============================
  31. //===========================================================================
  32. //static makes it inpossible to be called from outside of this file by extern.!
  33. // Variables used by The Stepper Driver Interrupt
  34. static unsigned char out_bits; // The next stepping-bits to be output
  35. static unsigned char out_bits_prev = 0xFF;
  36. static long counter_x, // Counter variables for the bresenham line tracer
  37. counter_y,
  38. counter_z,
  39. counter_e;
  40. volatile static unsigned long step_events_completed; // The number of step events executed in the current block
  41. #ifdef ADVANCE
  42. static long advance_rate, advance, final_advance = 0;
  43. static long old_advance = 0;
  44. #endif
  45. static long e_steps[3];
  46. static long acceleration_time, deceleration_time;
  47. //static unsigned long accelerate_until, decelerate_after, acceleration_rate, initial_rate, final_rate, nominal_rate;
  48. static unsigned short acc_step_rate; // needed for deccelaration start point
  49. static char step_loops;
  50. static unsigned short OCR1A_nominal;
  51. volatile long endstops_trigsteps[3]={0,0,0};
  52. volatile long endstops_stepsTotal,endstops_stepsDone;
  53. static volatile bool endstop_x_hit=false;
  54. static volatile bool endstop_y_hit=false;
  55. static volatile bool endstop_z_hit=false;
  56. static bool old_x_min_endstop=false;
  57. static bool old_x_max_endstop=false;
  58. static bool old_y_min_endstop=false;
  59. static bool old_y_max_endstop=false;
  60. static bool old_z_min_endstop=false;
  61. static bool old_z_max_endstop=false;
  62. static bool check_endstops = true;
  63. volatile long count_position[NUM_AXIS] = { 0, 0, 0, 0};
  64. volatile char count_direction[NUM_AXIS] = { 1, 1, 1, 1};
  65. //===========================================================================
  66. //=============================functions ============================
  67. //===========================================================================
  68. #define CHECK_ENDSTOPS if(check_endstops)
  69. // intRes = intIn1 * intIn2 >> 16
  70. // uses:
  71. // r26 to store 0
  72. // r27 to store the byte 1 of the 24 bit result
  73. #define MultiU16X8toH16(intRes, charIn1, intIn2) \
  74. asm volatile ( \
  75. "clr r26 \n\t" \
  76. "mul %A1, %B2 \n\t" \
  77. "movw %A0, r0 \n\t" \
  78. "mul %A1, %A2 \n\t" \
  79. "add %A0, r1 \n\t" \
  80. "adc %B0, r26 \n\t" \
  81. "lsr r0 \n\t" \
  82. "adc %A0, r26 \n\t" \
  83. "adc %B0, r26 \n\t" \
  84. "clr r1 \n\t" \
  85. : \
  86. "=&r" (intRes) \
  87. : \
  88. "d" (charIn1), \
  89. "d" (intIn2) \
  90. : \
  91. "r26" \
  92. )
  93. // intRes = longIn1 * longIn2 >> 24
  94. // uses:
  95. // r26 to store 0
  96. // r27 to store the byte 1 of the 48bit result
  97. #define MultiU24X24toH16(intRes, longIn1, longIn2) \
  98. asm volatile ( \
  99. "clr r26 \n\t" \
  100. "mul %A1, %B2 \n\t" \
  101. "mov r27, r1 \n\t" \
  102. "mul %B1, %C2 \n\t" \
  103. "movw %A0, r0 \n\t" \
  104. "mul %C1, %C2 \n\t" \
  105. "add %B0, r0 \n\t" \
  106. "mul %C1, %B2 \n\t" \
  107. "add %A0, r0 \n\t" \
  108. "adc %B0, r1 \n\t" \
  109. "mul %A1, %C2 \n\t" \
  110. "add r27, r0 \n\t" \
  111. "adc %A0, r1 \n\t" \
  112. "adc %B0, r26 \n\t" \
  113. "mul %B1, %B2 \n\t" \
  114. "add r27, r0 \n\t" \
  115. "adc %A0, r1 \n\t" \
  116. "adc %B0, r26 \n\t" \
  117. "mul %C1, %A2 \n\t" \
  118. "add r27, r0 \n\t" \
  119. "adc %A0, r1 \n\t" \
  120. "adc %B0, r26 \n\t" \
  121. "mul %B1, %A2 \n\t" \
  122. "add r27, r1 \n\t" \
  123. "adc %A0, r26 \n\t" \
  124. "adc %B0, r26 \n\t" \
  125. "lsr r27 \n\t" \
  126. "adc %A0, r26 \n\t" \
  127. "adc %B0, r26 \n\t" \
  128. "clr r1 \n\t" \
  129. : \
  130. "=&r" (intRes) \
  131. : \
  132. "d" (longIn1), \
  133. "d" (longIn2) \
  134. : \
  135. "r26" , "r27" \
  136. )
  137. // Some useful constants
  138. #define ENABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 |= (1<<OCIE1A)
  139. #define DISABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 &= ~(1<<OCIE1A)
  140. void checkHitEndstops()
  141. {
  142. if( endstop_x_hit || endstop_y_hit || endstop_z_hit) {
  143. SERIAL_ECHO_START;
  144. SERIAL_ECHOPGM(MSG_ENDSTOPS_HIT);
  145. if(endstop_x_hit) {
  146. SERIAL_ECHOPAIR(" X:",(float)endstops_trigsteps[X_AXIS]/axis_steps_per_unit[X_AXIS]);
  147. }
  148. if(endstop_y_hit) {
  149. SERIAL_ECHOPAIR(" Y:",(float)endstops_trigsteps[Y_AXIS]/axis_steps_per_unit[Y_AXIS]);
  150. }
  151. if(endstop_z_hit) {
  152. SERIAL_ECHOPAIR(" Z:",(float)endstops_trigsteps[Z_AXIS]/axis_steps_per_unit[Z_AXIS]);
  153. }
  154. SERIAL_ECHOLN("");
  155. endstop_x_hit=false;
  156. endstop_y_hit=false;
  157. endstop_z_hit=false;
  158. }
  159. }
  160. void endstops_hit_on_purpose()
  161. {
  162. endstop_x_hit=false;
  163. endstop_y_hit=false;
  164. endstop_z_hit=false;
  165. }
  166. void enable_endstops(bool check)
  167. {
  168. check_endstops = check;
  169. }
  170. // __________________________
  171. // /| |\ _________________ ^
  172. // / | | \ /| |\ |
  173. // / | | \ / | | \ s
  174. // / | | | | | \ p
  175. // / | | | | | \ e
  176. // +-----+------------------------+---+--+---------------+----+ e
  177. // | BLOCK 1 | BLOCK 2 | d
  178. //
  179. // time ----->
  180. //
  181. // The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates
  182. // first block->accelerate_until step_events_completed, then keeps going at constant speed until
  183. // step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset.
  184. // The slope of acceleration is calculated with the leib ramp alghorithm.
  185. void st_wake_up() {
  186. // TCNT1 = 0;
  187. ENABLE_STEPPER_DRIVER_INTERRUPT();
  188. }
  189. FORCE_INLINE unsigned short calc_timer(unsigned short step_rate) {
  190. unsigned short timer;
  191. if(step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY;
  192. if(step_rate > 20000) { // If steprate > 20kHz >> step 4 times
  193. step_rate = (step_rate >> 2)&0x3fff;
  194. step_loops = 4;
  195. }
  196. else if(step_rate > 10000) { // If steprate > 10kHz >> step 2 times
  197. step_rate = (step_rate >> 1)&0x7fff;
  198. step_loops = 2;
  199. }
  200. else {
  201. step_loops = 1;
  202. }
  203. if(step_rate < (F_CPU/500000)) step_rate = (F_CPU/500000);
  204. step_rate -= (F_CPU/500000); // Correct for minimal speed
  205. if(step_rate >= (8*256)){ // higher step rate
  206. unsigned short table_address = (unsigned short)&speed_lookuptable_fast[(unsigned char)(step_rate>>8)][0];
  207. unsigned char tmp_step_rate = (step_rate & 0x00ff);
  208. unsigned short gain = (unsigned short)pgm_read_word_near(table_address+2);
  209. MultiU16X8toH16(timer, tmp_step_rate, gain);
  210. timer = (unsigned short)pgm_read_word_near(table_address) - timer;
  211. }
  212. else { // lower step rates
  213. unsigned short table_address = (unsigned short)&speed_lookuptable_slow[0][0];
  214. table_address += ((step_rate)>>1) & 0xfffc;
  215. timer = (unsigned short)pgm_read_word_near(table_address);
  216. timer -= (((unsigned short)pgm_read_word_near(table_address+2) * (unsigned char)(step_rate & 0x0007))>>3);
  217. }
  218. if(timer < 100) { timer = 100; MYSERIAL.print(MSG_STEPPER_TO_HIGH); MYSERIAL.println(step_rate); }//(20kHz this should never happen)
  219. return timer;
  220. }
  221. // Initializes the trapezoid generator from the current block. Called whenever a new
  222. // block begins.
  223. FORCE_INLINE void trapezoid_generator_reset() {
  224. #ifdef ADVANCE
  225. advance = current_block->initial_advance;
  226. final_advance = current_block->final_advance;
  227. // Do E steps + advance steps
  228. e_steps[current_block->active_extruder] += ((advance >>8) - old_advance);
  229. old_advance = advance >>8;
  230. #endif
  231. deceleration_time = 0;
  232. // step_rate to timer interval
  233. acc_step_rate = current_block->initial_rate;
  234. acceleration_time = calc_timer(acc_step_rate);
  235. OCR1A = acceleration_time;
  236. OCR1A_nominal = calc_timer(current_block->nominal_rate);
  237. // SERIAL_ECHO_START;
  238. // SERIAL_ECHOPGM("advance :");
  239. // SERIAL_ECHO(current_block->advance/256.0);
  240. // SERIAL_ECHOPGM("advance rate :");
  241. // SERIAL_ECHO(current_block->advance_rate/256.0);
  242. // SERIAL_ECHOPGM("initial advance :");
  243. // SERIAL_ECHO(current_block->initial_advance/256.0);
  244. // SERIAL_ECHOPGM("final advance :");
  245. // SERIAL_ECHOLN(current_block->final_advance/256.0);
  246. }
  247. // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse.
  248. // It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately.
  249. ISR(TIMER1_COMPA_vect)
  250. {
  251. // If there is no current block, attempt to pop one from the buffer
  252. if (current_block == NULL) {
  253. // Anything in the buffer?
  254. current_block = plan_get_current_block();
  255. if (current_block != NULL) {
  256. current_block->busy = true;
  257. trapezoid_generator_reset();
  258. counter_x = -(current_block->step_event_count >> 1);
  259. counter_y = counter_x;
  260. counter_z = counter_x;
  261. counter_e = counter_x;
  262. step_events_completed = 0;
  263. #ifdef Z_LATE_ENABLE
  264. if(current_block->steps_z > 0) {
  265. enable_z();
  266. OCR1A = 2000; //1ms wait
  267. return;
  268. }
  269. #endif
  270. // #ifdef ADVANCE
  271. // e_steps[current_block->active_extruder] = 0;
  272. // #endif
  273. }
  274. else {
  275. OCR1A=2000; // 1kHz.
  276. }
  277. }
  278. if (current_block != NULL) {
  279. // Set directions TO DO This should be done once during init of trapezoid. Endstops -> interrupt
  280. out_bits = current_block->direction_bits;
  281. // Oscillation delay
  282. #ifdef OSCILLATION_DELAY
  283. if (out_bits != out_bits_prev){
  284. delayMicroseconds(OSCILLATION_DELAY);
  285. }
  286. #endif
  287. // Set direction en check limit switches
  288. if ((out_bits & (1<<X_AXIS)) != 0) { // -direction
  289. WRITE(X_DIR_PIN, INVERT_X_DIR);
  290. count_direction[X_AXIS]=-1;
  291. CHECK_ENDSTOPS
  292. {
  293. #if X_MIN_PIN > -1
  294. bool x_min_endstop=(READ(X_MIN_PIN) != X_ENDSTOPS_INVERTING);
  295. if(x_min_endstop && old_x_min_endstop && (current_block->steps_x > 0)) {
  296. endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
  297. endstop_x_hit=true;
  298. step_events_completed = current_block->step_event_count;
  299. }
  300. old_x_min_endstop = x_min_endstop;
  301. #endif
  302. }
  303. }
  304. else { // +direction
  305. WRITE(X_DIR_PIN,!INVERT_X_DIR);
  306. count_direction[X_AXIS]=1;
  307. CHECK_ENDSTOPS
  308. {
  309. #if X_MAX_PIN > -1
  310. bool x_max_endstop=(READ(X_MAX_PIN) != X_ENDSTOPS_INVERTING);
  311. if(x_max_endstop && old_x_max_endstop && (current_block->steps_x > 0)){
  312. endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
  313. endstop_x_hit=true;
  314. step_events_completed = current_block->step_event_count;
  315. }
  316. old_x_max_endstop = x_max_endstop;
  317. #endif
  318. }
  319. }
  320. if ((out_bits & (1<<Y_AXIS)) != 0) { // -direction
  321. WRITE(Y_DIR_PIN,INVERT_Y_DIR);
  322. count_direction[Y_AXIS]=-1;
  323. CHECK_ENDSTOPS
  324. {
  325. #if Y_MIN_PIN > -1
  326. bool y_min_endstop=(READ(Y_MIN_PIN) != Y_ENDSTOPS_INVERTING);
  327. if(y_min_endstop && old_y_min_endstop && (current_block->steps_y > 0)) {
  328. endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
  329. endstop_y_hit=true;
  330. step_events_completed = current_block->step_event_count;
  331. }
  332. old_y_min_endstop = y_min_endstop;
  333. #endif
  334. }
  335. }
  336. else { // +direction
  337. WRITE(Y_DIR_PIN,!INVERT_Y_DIR);
  338. count_direction[Y_AXIS]=1;
  339. CHECK_ENDSTOPS
  340. {
  341. #if Y_MAX_PIN > -1
  342. bool y_max_endstop=(READ(Y_MAX_PIN) != Y_ENDSTOPS_INVERTING);
  343. if(y_max_endstop && old_y_max_endstop && (current_block->steps_y > 0)){
  344. endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
  345. endstop_y_hit=true;
  346. step_events_completed = current_block->step_event_count;
  347. }
  348. old_y_max_endstop = y_max_endstop;
  349. #endif
  350. }
  351. }
  352. if ((out_bits & (1<<Z_AXIS)) != 0) { // -direction
  353. WRITE(Z_DIR_PIN,INVERT_Z_DIR);
  354. count_direction[Z_AXIS]=-1;
  355. CHECK_ENDSTOPS
  356. {
  357. #if Z_MIN_PIN > -1
  358. bool z_min_endstop=(READ(Z_MIN_PIN) != Z_ENDSTOPS_INVERTING);
  359. if(z_min_endstop && old_z_min_endstop && (current_block->steps_z > 0)) {
  360. endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
  361. endstop_z_hit=true;
  362. step_events_completed = current_block->step_event_count;
  363. }
  364. old_z_min_endstop = z_min_endstop;
  365. #endif
  366. }
  367. }
  368. else { // +direction
  369. WRITE(Z_DIR_PIN,!INVERT_Z_DIR);
  370. count_direction[Z_AXIS]=1;
  371. CHECK_ENDSTOPS
  372. {
  373. #if Z_MAX_PIN > -1
  374. bool z_max_endstop=(READ(Z_MAX_PIN) != Z_ENDSTOPS_INVERTING);
  375. if(z_max_endstop && old_z_max_endstop && (current_block->steps_z > 0)) {
  376. endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
  377. endstop_z_hit=true;
  378. step_events_completed = current_block->step_event_count;
  379. }
  380. old_z_max_endstop = z_max_endstop;
  381. #endif
  382. }
  383. }
  384. #ifndef ADVANCE
  385. if ((out_bits & (1<<E_AXIS)) != 0) { // -direction
  386. REV_E_DIR();
  387. count_direction[E_AXIS]=-1;
  388. }
  389. else { // +direction
  390. NORM_E_DIR();
  391. count_direction[E_AXIS]=1;
  392. }
  393. #endif //!ADVANCE
  394. // Oscillation delay
  395. #ifdef OSCILLATION_DELAY
  396. if (out_bits != out_bits_prev){
  397. delayMicroseconds(OSCILLATION_DELAY);
  398. }
  399. out_bits_prev = out_bits;
  400. #endif
  401. for(int8_t i=0; i < step_loops; i++) { // Take multiple steps per interrupt (For high speed moves)
  402. #if MOTHERBOARD != 8 // !teensylu
  403. MSerial.checkRx(); // Check for serial chars.
  404. #endif
  405. #ifdef ADVANCE
  406. counter_e += current_block->steps_e;
  407. if (counter_e > 0) {
  408. counter_e -= current_block->step_event_count;
  409. if ((out_bits & (1<<E_AXIS)) != 0) { // - direction
  410. e_steps[current_block->active_extruder]--;
  411. }
  412. else {
  413. e_steps[current_block->active_extruder]++;
  414. }
  415. }
  416. #endif //ADVANCE
  417. counter_x += current_block->steps_x;
  418. if (counter_x > 0) {
  419. delayMicroseconds(3);
  420. WRITE(X_STEP_PIN, HIGH);
  421. counter_x -= current_block->step_event_count;
  422. delayMicroseconds(3);
  423. WRITE(X_STEP_PIN, LOW);
  424. count_position[X_AXIS]+=count_direction[X_AXIS];
  425. }
  426. counter_y += current_block->steps_y;
  427. if (counter_y > 0) {
  428. delayMicroseconds(3);
  429. WRITE(Y_STEP_PIN, HIGH);
  430. counter_y -= current_block->step_event_count;
  431. delayMicroseconds(3);
  432. WRITE(Y_STEP_PIN, LOW);
  433. count_position[Y_AXIS]+=count_direction[Y_AXIS];
  434. }
  435. counter_z += current_block->steps_z;
  436. if (counter_z > 0) {
  437. delayMicroseconds(3);
  438. WRITE(Z_STEP_PIN, HIGH);
  439. counter_z -= current_block->step_event_count;
  440. delayMicroseconds(3);
  441. WRITE(Z_STEP_PIN, LOW);
  442. count_position[Z_AXIS]+=count_direction[Z_AXIS];
  443. }
  444. #ifndef ADVANCE
  445. counter_e += current_block->steps_e;
  446. if (counter_e > 0) {
  447. delayMicroseconds(3);
  448. WRITE_E_STEP(HIGH);
  449. counter_e -= current_block->step_event_count;
  450. delayMicroseconds(3);
  451. WRITE_E_STEP(LOW);
  452. count_position[E_AXIS]+=count_direction[E_AXIS];
  453. }
  454. #endif //!ADVANCE
  455. step_events_completed += 1;
  456. if(step_events_completed >= current_block->step_event_count) break;
  457. }
  458. // Calculare new timer value
  459. unsigned short timer;
  460. unsigned short step_rate;
  461. if (step_events_completed <= (unsigned long int)current_block->accelerate_until) {
  462. MultiU24X24toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
  463. acc_step_rate += current_block->initial_rate;
  464. // upper limit
  465. if(acc_step_rate > current_block->nominal_rate)
  466. acc_step_rate = current_block->nominal_rate;
  467. // step_rate to timer interval
  468. timer = calc_timer(acc_step_rate);
  469. OCR1A = timer;
  470. acceleration_time += timer;
  471. #ifdef ADVANCE
  472. for(int8_t i=0; i < step_loops; i++) {
  473. advance += advance_rate;
  474. }
  475. //if(advance > current_block->advance) advance = current_block->advance;
  476. // Do E steps + advance steps
  477. e_steps[current_block->active_extruder] += ((advance >>8) - old_advance);
  478. old_advance = advance >>8;
  479. #endif
  480. }
  481. else if (step_events_completed > (unsigned long int)current_block->decelerate_after) {
  482. MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate);
  483. if(step_rate > acc_step_rate) { // Check step_rate stays positive
  484. step_rate = current_block->final_rate;
  485. }
  486. else {
  487. step_rate = acc_step_rate - step_rate; // Decelerate from aceleration end point.
  488. }
  489. // lower limit
  490. if(step_rate < current_block->final_rate)
  491. step_rate = current_block->final_rate;
  492. // step_rate to timer interval
  493. timer = calc_timer(step_rate);
  494. OCR1A = timer;
  495. deceleration_time += timer;
  496. #ifdef ADVANCE
  497. for(int8_t i=0; i < step_loops; i++) {
  498. advance -= advance_rate;
  499. }
  500. if(advance < final_advance) advance = final_advance;
  501. // Do E steps + advance steps
  502. e_steps[current_block->active_extruder] += ((advance >>8) - old_advance);
  503. old_advance = advance >>8;
  504. #endif //ADVANCE
  505. }
  506. else {
  507. OCR1A = OCR1A_nominal;
  508. }
  509. // If current block is finished, reset pointer
  510. if (step_events_completed >= current_block->step_event_count) {
  511. current_block = NULL;
  512. plan_discard_current_block();
  513. }
  514. }
  515. }
  516. #ifdef ADVANCE
  517. unsigned char old_OCR0A;
  518. // Timer interrupt for E. e_steps is set in the main routine;
  519. // Timer 0 is shared with millies
  520. ISR(TIMER0_COMPA_vect)
  521. {
  522. old_OCR0A += 52; // ~10kHz interrupt (250000 / 26 = 9615kHz)
  523. OCR0A = old_OCR0A;
  524. // Set E direction (Depends on E direction + advance)
  525. for(unsigned char i=0; i<4;i++) {
  526. if (e_steps[0] != 0) {
  527. WRITE(E0_STEP_PIN, LOW);
  528. if (e_steps[0] < 0) {
  529. WRITE(E0_DIR_PIN, INVERT_E0_DIR);
  530. e_steps[0]++;
  531. WRITE(E0_STEP_PIN, HIGH);
  532. }
  533. else if (e_steps[0] > 0) {
  534. WRITE(E0_DIR_PIN, !INVERT_E0_DIR);
  535. e_steps[0]--;
  536. WRITE(E0_STEP_PIN, HIGH);
  537. }
  538. }
  539. #if EXTRUDERS > 1
  540. if (e_steps[1] != 0) {
  541. WRITE(E1_STEP_PIN, LOW);
  542. if (e_steps[1] < 0) {
  543. WRITE(E1_DIR_PIN, INVERT_E1_DIR);
  544. e_steps[1]++;
  545. WRITE(E1_STEP_PIN, HIGH);
  546. }
  547. else if (e_steps[1] > 0) {
  548. WRITE(E1_DIR_PIN, !INVERT_E1_DIR);
  549. e_steps[1]--;
  550. WRITE(E1_STEP_PIN, HIGH);
  551. }
  552. }
  553. #endif
  554. #if EXTRUDERS > 2
  555. if (e_steps[2] != 0) {
  556. WRITE(E2_STEP_PIN, LOW);
  557. if (e_steps[2] < 0) {
  558. WRITE(E2_DIR_PIN, INVERT_E2_DIR);
  559. e_steps[2]++;
  560. WRITE(E2_STEP_PIN, HIGH);
  561. }
  562. else if (e_steps[2] > 0) {
  563. WRITE(E2_DIR_PIN, !INVERT_E2_DIR);
  564. e_steps[2]--;
  565. WRITE(E2_STEP_PIN, HIGH);
  566. }
  567. }
  568. #endif
  569. }
  570. }
  571. #endif // ADVANCE
  572. void st_init()
  573. {
  574. //Initialize Dir Pins
  575. #if X_DIR_PIN > -1
  576. SET_OUTPUT(X_DIR_PIN);
  577. #endif
  578. #if Y_DIR_PIN > -1
  579. SET_OUTPUT(Y_DIR_PIN);
  580. #endif
  581. #if Z_DIR_PIN > -1
  582. SET_OUTPUT(Z_DIR_PIN);
  583. #endif
  584. #if E0_DIR_PIN > -1
  585. SET_OUTPUT(E0_DIR_PIN);
  586. #endif
  587. #if defined(E1_DIR_PIN) && (E1_DIR_PIN > -1)
  588. SET_OUTPUT(E1_DIR_PIN);
  589. #endif
  590. #if defined(E2_DIR_PIN) && (E2_DIR_PIN > -1)
  591. SET_OUTPUT(E2_DIR_PIN);
  592. #endif
  593. //Initialize Enable Pins - steppers default to disabled.
  594. #if (X_ENABLE_PIN > -1)
  595. SET_OUTPUT(X_ENABLE_PIN);
  596. if(!X_ENABLE_ON) WRITE(X_ENABLE_PIN,HIGH);
  597. #endif
  598. #if (Y_ENABLE_PIN > -1)
  599. SET_OUTPUT(Y_ENABLE_PIN);
  600. if(!Y_ENABLE_ON) WRITE(Y_ENABLE_PIN,HIGH);
  601. #endif
  602. #if (Z_ENABLE_PIN > -1)
  603. SET_OUTPUT(Z_ENABLE_PIN);
  604. if(!Z_ENABLE_ON) WRITE(Z_ENABLE_PIN,HIGH);
  605. #endif
  606. #if (E0_ENABLE_PIN > -1)
  607. SET_OUTPUT(E0_ENABLE_PIN);
  608. if(!E_ENABLE_ON) WRITE(E0_ENABLE_PIN,HIGH);
  609. #endif
  610. #if defined(E1_ENABLE_PIN) && (E1_ENABLE_PIN > -1)
  611. SET_OUTPUT(E1_ENABLE_PIN);
  612. if(!E_ENABLE_ON) WRITE(E1_ENABLE_PIN,HIGH);
  613. #endif
  614. #if defined(E2_ENABLE_PIN) && (E2_ENABLE_PIN > -1)
  615. SET_OUTPUT(E2_ENABLE_PIN);
  616. if(!E_ENABLE_ON) WRITE(E2_ENABLE_PIN,HIGH);
  617. #endif
  618. //endstops and pullups
  619. #if X_MIN_PIN > -1
  620. SET_INPUT(X_MIN_PIN);
  621. #ifdef ENDSTOPPULLUP_XMIN
  622. WRITE(X_MIN_PIN,HIGH);
  623. #endif
  624. #endif
  625. #if Y_MIN_PIN > -1
  626. SET_INPUT(Y_MIN_PIN);
  627. #ifdef ENDSTOPPULLUP_YMIN
  628. WRITE(Y_MIN_PIN,HIGH);
  629. #endif
  630. #endif
  631. #if Z_MIN_PIN > -1
  632. SET_INPUT(Z_MIN_PIN);
  633. #ifdef ENDSTOPPULLUP_ZMIN
  634. WRITE(Z_MIN_PIN,HIGH);
  635. #endif
  636. #endif
  637. #if X_MAX_PIN > -1
  638. SET_INPUT(X_MAX_PIN);
  639. #ifdef ENDSTOPPULLUP_XMAX
  640. WRITE(X_MAX_PIN,HIGH);
  641. #endif
  642. #endif
  643. #if Y_MAX_PIN > -1
  644. SET_INPUT(Y_MAX_PIN);
  645. #ifdef ENDSTOPPULLUP_YMAX
  646. WRITE(Y_MAX_PIN,HIGH);
  647. #endif
  648. #endif
  649. #if Z_MAX_PIN > -1
  650. SET_INPUT(Z_MAX_PIN);
  651. #ifdef ENDSTOPPULLUP_ZMAX
  652. WRITE(Z_MAX_PIN,HIGH);
  653. #endif
  654. #endif
  655. //Initialize Step Pins
  656. #if (X_STEP_PIN > -1)
  657. SET_OUTPUT(X_STEP_PIN);
  658. if(!X_ENABLE_ON) WRITE(X_ENABLE_PIN,HIGH);
  659. #endif
  660. #if (Y_STEP_PIN > -1)
  661. SET_OUTPUT(Y_STEP_PIN);
  662. if(!Y_ENABLE_ON) WRITE(Y_ENABLE_PIN,HIGH);
  663. #endif
  664. #if (Z_STEP_PIN > -1)
  665. SET_OUTPUT(Z_STEP_PIN);
  666. if(!Z_ENABLE_ON) WRITE(Z_ENABLE_PIN,HIGH);
  667. #endif
  668. #if (E0_STEP_PIN > -1)
  669. SET_OUTPUT(E0_STEP_PIN);
  670. if(!E_ENABLE_ON) WRITE(E0_ENABLE_PIN,HIGH);
  671. #endif
  672. #if defined(E1_STEP_PIN) && (E1_STEP_PIN > -1)
  673. SET_OUTPUT(E1_STEP_PIN);
  674. if(!E_ENABLE_ON) WRITE(E1_ENABLE_PIN,HIGH);
  675. #endif
  676. #if defined(E2_STEP_PIN) && (E2_STEP_PIN > -1)
  677. SET_OUTPUT(E2_STEP_PIN);
  678. if(!E_ENABLE_ON) WRITE(E2_ENABLE_PIN,HIGH);
  679. #endif
  680. #ifdef CONTROLLERFAN_PIN
  681. SET_OUTPUT(CONTROLLERFAN_PIN); //Set pin used for driver cooling fan
  682. #endif
  683. // waveform generation = 0100 = CTC
  684. TCCR1B &= ~(1<<WGM13);
  685. TCCR1B |= (1<<WGM12);
  686. TCCR1A &= ~(1<<WGM11);
  687. TCCR1A &= ~(1<<WGM10);
  688. // output mode = 00 (disconnected)
  689. TCCR1A &= ~(3<<COM1A0);
  690. TCCR1A &= ~(3<<COM1B0);
  691. // Set the timer pre-scaler
  692. // Generally we use a divider of 8, resulting in a 2MHz timer
  693. // frequency on a 16MHz MCU. If you are going to change this, be
  694. // sure to regenerate speed_lookuptable.h with
  695. // create_speed_lookuptable.py
  696. TCCR1B = (TCCR1B & ~(0x07<<CS10)) | (2<<CS10);
  697. OCR1A = 0x4000;
  698. TCNT1 = 0;
  699. ENABLE_STEPPER_DRIVER_INTERRUPT();
  700. #ifdef ADVANCE
  701. #if defined(TCCR0A) && defined(WGM01)
  702. TCCR0A &= ~(1<<WGM01);
  703. TCCR0A &= ~(1<<WGM00);
  704. #endif
  705. e_steps[0] = 0;
  706. e_steps[1] = 0;
  707. e_steps[2] = 0;
  708. TIMSK0 |= (1<<OCIE0A);
  709. #endif //ADVANCE
  710. enable_endstops(true); // Start with endstops active. After homing they can be disabled
  711. sei();
  712. }
  713. // Block until all buffered steps are executed
  714. void st_synchronize()
  715. {
  716. while( blocks_queued()) {
  717. manage_heater();
  718. manage_inactivity(1);
  719. LCD_STATUS;
  720. }
  721. }
  722. void st_set_position(const long &x, const long &y, const long &z, const long &e)
  723. {
  724. CRITICAL_SECTION_START;
  725. count_position[X_AXIS] = x;
  726. count_position[Y_AXIS] = y;
  727. count_position[Z_AXIS] = z;
  728. count_position[E_AXIS] = e;
  729. CRITICAL_SECTION_END;
  730. }
  731. void st_set_e_position(const long &e)
  732. {
  733. CRITICAL_SECTION_START;
  734. count_position[E_AXIS] = e;
  735. CRITICAL_SECTION_END;
  736. }
  737. long st_get_position(uint8_t axis)
  738. {
  739. long count_pos;
  740. CRITICAL_SECTION_START;
  741. count_pos = count_position[axis];
  742. CRITICAL_SECTION_END;
  743. return count_pos;
  744. }
  745. void finishAndDisableSteppers()
  746. {
  747. st_synchronize();
  748. LCD_MESSAGEPGM(MSG_STEPPER_RELEASED);
  749. disable_x();
  750. disable_y();
  751. disable_z();
  752. disable_e0();
  753. disable_e1();
  754. disable_e2();
  755. }
  756. void quickStop()
  757. {
  758. DISABLE_STEPPER_DRIVER_INTERRUPT();
  759. while(blocks_queued())
  760. plan_discard_current_block();
  761. current_block = NULL;
  762. ENABLE_STEPPER_DRIVER_INTERRUPT();
  763. }