My Marlin configs for Fabrikator Mini and CTC i3 Pro B
Du kan inte välja fler än 25 ämnen Ämnen måste starta med en bokstav eller siffra, kan innehålla bindestreck ('-') och vara max 35 tecken långa.

stepper.cpp 29KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992
  1. /**
  2. * Marlin 3D Printer Firmware
  3. * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
  4. *
  5. * Based on Sprinter and grbl.
  6. * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
  7. *
  8. * This program is free software: you can redistribute it and/or modify
  9. * it under the terms of the GNU General Public License as published by
  10. * the Free Software Foundation, either version 3 of the License, or
  11. * (at your option) any later version.
  12. *
  13. * This program is distributed in the hope that it will be useful,
  14. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  16. * GNU General Public License for more details.
  17. *
  18. * You should have received a copy of the GNU General Public License
  19. * along with this program. If not, see <http://www.gnu.org/licenses/>.
  20. *
  21. */
  22. /**
  23. * stepper.cpp - A singleton object to execute motion plans using stepper motors
  24. * Marlin Firmware
  25. *
  26. * Derived from Grbl
  27. * Copyright (c) 2009-2011 Simen Svale Skogsrud
  28. *
  29. * Grbl is free software: you can redistribute it and/or modify
  30. * it under the terms of the GNU General Public License as published by
  31. * the Free Software Foundation, either version 3 of the License, or
  32. * (at your option) any later version.
  33. *
  34. * Grbl is distributed in the hope that it will be useful,
  35. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  36. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  37. * GNU General Public License for more details.
  38. *
  39. * You should have received a copy of the GNU General Public License
  40. * along with Grbl. If not, see <http://www.gnu.org/licenses/>.
  41. */
  42. /* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith
  43. and Philipp Tiefenbacher. */
  44. #include "Marlin.h"
  45. #include "stepper.h"
  46. #include "endstops.h"
  47. #include "planner.h"
  48. #include "temperature.h"
  49. #include "ultralcd.h"
  50. #include "language.h"
  51. #include "cardreader.h"
  52. #include "speed_lookuptable.h"
  53. #if HAS_DIGIPOTSS
  54. #include <SPI.h>
  55. #endif
  56. Stepper stepper; // Singleton
  57. #if ENABLED(DUAL_X_CARRIAGE)
  58. #define X_APPLY_DIR(v,ALWAYS) \
  59. if (extruder_duplication_enabled || ALWAYS) { \
  60. X_DIR_WRITE(v); \
  61. X2_DIR_WRITE(v); \
  62. } \
  63. else { \
  64. if (current_block->active_extruder) X2_DIR_WRITE(v); else X_DIR_WRITE(v); \
  65. }
  66. #define X_APPLY_STEP(v,ALWAYS) \
  67. if (extruder_duplication_enabled || ALWAYS) { \
  68. X_STEP_WRITE(v); \
  69. X2_STEP_WRITE(v); \
  70. } \
  71. else { \
  72. if (current_block->active_extruder != 0) X2_STEP_WRITE(v); else X_STEP_WRITE(v); \
  73. }
  74. #else
  75. #define X_APPLY_DIR(v,Q) X_DIR_WRITE(v)
  76. #define X_APPLY_STEP(v,Q) X_STEP_WRITE(v)
  77. #endif
  78. #if ENABLED(Y_DUAL_STEPPER_DRIVERS)
  79. #define Y_APPLY_DIR(v,Q) { Y_DIR_WRITE(v); Y2_DIR_WRITE((v) != INVERT_Y2_VS_Y_DIR); }
  80. #define Y_APPLY_STEP(v,Q) { Y_STEP_WRITE(v); Y2_STEP_WRITE(v); }
  81. #else
  82. #define Y_APPLY_DIR(v,Q) Y_DIR_WRITE(v)
  83. #define Y_APPLY_STEP(v,Q) Y_STEP_WRITE(v)
  84. #endif
  85. #if ENABLED(Z_DUAL_STEPPER_DRIVERS)
  86. #define Z_APPLY_DIR(v,Q) { Z_DIR_WRITE(v); Z2_DIR_WRITE(v); }
  87. #if ENABLED(Z_DUAL_ENDSTOPS)
  88. #define Z_APPLY_STEP(v,Q) \
  89. if (performing_homing) { \
  90. if (Z_HOME_DIR > 0) {\
  91. if (!(TEST(endstops.old_endstop_bits, Z_MAX) && (count_direction[Z_AXIS] > 0)) && !locked_z_motor) Z_STEP_WRITE(v); \
  92. if (!(TEST(endstops.old_endstop_bits, Z2_MAX) && (count_direction[Z_AXIS] > 0)) && !locked_z2_motor) Z2_STEP_WRITE(v); \
  93. } \
  94. else { \
  95. if (!(TEST(endstops.old_endstop_bits, Z_MIN) && (count_direction[Z_AXIS] < 0)) && !locked_z_motor) Z_STEP_WRITE(v); \
  96. if (!(TEST(endstops.old_endstop_bits, Z2_MIN) && (count_direction[Z_AXIS] < 0)) && !locked_z2_motor) Z2_STEP_WRITE(v); \
  97. } \
  98. } \
  99. else { \
  100. Z_STEP_WRITE(v); \
  101. Z2_STEP_WRITE(v); \
  102. }
  103. #else
  104. #define Z_APPLY_STEP(v,Q) { Z_STEP_WRITE(v); Z2_STEP_WRITE(v); }
  105. #endif
  106. #else
  107. #define Z_APPLY_DIR(v,Q) Z_DIR_WRITE(v)
  108. #define Z_APPLY_STEP(v,Q) Z_STEP_WRITE(v)
  109. #endif
  110. #define E_APPLY_STEP(v,Q) E_STEP_WRITE(v)
  111. // intRes = longIn1 * longIn2 >> 24
  112. // uses:
  113. // r26 to store 0
  114. // r27 to store bits 16-23 of the 48bit result. The top bit is used to round the two byte result.
  115. // note that the lower two bytes and the upper byte of the 48bit result are not calculated.
  116. // this can cause the result to be out by one as the lower bytes may cause carries into the upper ones.
  117. // B0 A0 are bits 24-39 and are the returned value
  118. // C1 B1 A1 is longIn1
  119. // D2 C2 B2 A2 is longIn2
  120. //
  121. #define MultiU24X32toH16(intRes, longIn1, longIn2) \
  122. asm volatile ( \
  123. "clr r26 \n\t" \
  124. "mul %A1, %B2 \n\t" \
  125. "mov r27, r1 \n\t" \
  126. "mul %B1, %C2 \n\t" \
  127. "movw %A0, r0 \n\t" \
  128. "mul %C1, %C2 \n\t" \
  129. "add %B0, r0 \n\t" \
  130. "mul %C1, %B2 \n\t" \
  131. "add %A0, r0 \n\t" \
  132. "adc %B0, r1 \n\t" \
  133. "mul %A1, %C2 \n\t" \
  134. "add r27, r0 \n\t" \
  135. "adc %A0, r1 \n\t" \
  136. "adc %B0, r26 \n\t" \
  137. "mul %B1, %B2 \n\t" \
  138. "add r27, r0 \n\t" \
  139. "adc %A0, r1 \n\t" \
  140. "adc %B0, r26 \n\t" \
  141. "mul %C1, %A2 \n\t" \
  142. "add r27, r0 \n\t" \
  143. "adc %A0, r1 \n\t" \
  144. "adc %B0, r26 \n\t" \
  145. "mul %B1, %A2 \n\t" \
  146. "add r27, r1 \n\t" \
  147. "adc %A0, r26 \n\t" \
  148. "adc %B0, r26 \n\t" \
  149. "lsr r27 \n\t" \
  150. "adc %A0, r26 \n\t" \
  151. "adc %B0, r26 \n\t" \
  152. "mul %D2, %A1 \n\t" \
  153. "add %A0, r0 \n\t" \
  154. "adc %B0, r1 \n\t" \
  155. "mul %D2, %B1 \n\t" \
  156. "add %B0, r0 \n\t" \
  157. "clr r1 \n\t" \
  158. : \
  159. "=&r" (intRes) \
  160. : \
  161. "d" (longIn1), \
  162. "d" (longIn2) \
  163. : \
  164. "r26" , "r27" \
  165. )
  166. // Some useful constants
  167. #define ENABLE_STEPPER_DRIVER_INTERRUPT() SBI(TIMSK1, OCIE1A)
  168. #define DISABLE_STEPPER_DRIVER_INTERRUPT() CBI(TIMSK1, OCIE1A)
  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 using v = u + at where t is the accumulated timer values of the steps so far.
  185. */
  186. void Stepper::wake_up() {
  187. // TCNT1 = 0;
  188. ENABLE_STEPPER_DRIVER_INTERRUPT();
  189. }
  190. /**
  191. * Set the stepper direction of each axis
  192. *
  193. * COREXY: X_AXIS=A_AXIS and Y_AXIS=B_AXIS
  194. * COREXZ: X_AXIS=A_AXIS and Z_AXIS=C_AXIS
  195. * COREYZ: Y_AXIS=B_AXIS and Z_AXIS=C_AXIS
  196. */
  197. void Stepper::set_directions() {
  198. #define SET_STEP_DIR(AXIS) \
  199. if (motor_direction(AXIS ##_AXIS)) { \
  200. AXIS ##_APPLY_DIR(INVERT_## AXIS ##_DIR, false); \
  201. count_direction[AXIS ##_AXIS] = -1; \
  202. } \
  203. else { \
  204. AXIS ##_APPLY_DIR(!INVERT_## AXIS ##_DIR, false); \
  205. count_direction[AXIS ##_AXIS] = 1; \
  206. }
  207. SET_STEP_DIR(X); // A
  208. SET_STEP_DIR(Y); // B
  209. SET_STEP_DIR(Z); // C
  210. #if DISABLED(ADVANCE)
  211. if (motor_direction(E_AXIS)) {
  212. REV_E_DIR();
  213. count_direction[E_AXIS] = -1;
  214. }
  215. else {
  216. NORM_E_DIR();
  217. count_direction[E_AXIS] = 1;
  218. }
  219. #endif //!ADVANCE
  220. }
  221. // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse.
  222. // It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately.
  223. ISR(TIMER1_COMPA_vect) { stepper.isr(); }
  224. void Stepper::isr() {
  225. if (cleaning_buffer_counter) {
  226. current_block = NULL;
  227. planner.discard_current_block();
  228. #ifdef SD_FINISHED_RELEASECOMMAND
  229. if ((cleaning_buffer_counter == 1) && (SD_FINISHED_STEPPERRELEASE)) enqueue_and_echo_commands_P(PSTR(SD_FINISHED_RELEASECOMMAND));
  230. #endif
  231. cleaning_buffer_counter--;
  232. OCR1A = 200;
  233. return;
  234. }
  235. // If there is no current block, attempt to pop one from the buffer
  236. if (!current_block) {
  237. // Anything in the buffer?
  238. current_block = planner.get_current_block();
  239. if (current_block) {
  240. current_block->busy = true;
  241. trapezoid_generator_reset();
  242. counter_X = -(current_block->step_event_count >> 1);
  243. counter_Y = counter_Z = counter_E = counter_X;
  244. step_events_completed = 0;
  245. #if ENABLED(Z_LATE_ENABLE)
  246. if (current_block->steps[Z_AXIS] > 0) {
  247. enable_z();
  248. OCR1A = 2000; //1ms wait
  249. return;
  250. }
  251. #endif
  252. // #if ENABLED(ADVANCE)
  253. // e_steps[current_block->active_extruder] = 0;
  254. // #endif
  255. }
  256. else {
  257. OCR1A = 2000; // 1kHz.
  258. }
  259. }
  260. if (current_block != NULL) {
  261. // Update endstops state, if enabled
  262. #if HAS_BED_PROBE
  263. if (endstops.enabled || endstops.z_probe_enabled) endstops.update();
  264. #else
  265. if (endstops.enabled) endstops.update();
  266. #endif
  267. // Take multiple steps per interrupt (For high speed moves)
  268. for (int8_t i = 0; i < step_loops; i++) {
  269. #ifndef USBCON
  270. customizedSerial.checkRx(); // Check for serial chars.
  271. #endif
  272. #if ENABLED(ADVANCE)
  273. counter_E += current_block->steps[E_AXIS];
  274. if (counter_E > 0) {
  275. counter_E -= current_block->step_event_count;
  276. e_steps[current_block->active_extruder] += motor_direction(E_AXIS) ? -1 : 1;
  277. }
  278. #endif //ADVANCE
  279. #define _COUNTER(AXIS) counter_## AXIS
  280. #define _APPLY_STEP(AXIS) AXIS ##_APPLY_STEP
  281. #define _INVERT_STEP_PIN(AXIS) INVERT_## AXIS ##_STEP_PIN
  282. #define STEP_ADD(AXIS) \
  283. _COUNTER(AXIS) += current_block->steps[_AXIS(AXIS)]; \
  284. if (_COUNTER(AXIS) > 0) { _APPLY_STEP(AXIS)(!_INVERT_STEP_PIN(AXIS),0); }
  285. STEP_ADD(X);
  286. STEP_ADD(Y);
  287. STEP_ADD(Z);
  288. #if DISABLED(ADVANCE)
  289. STEP_ADD(E);
  290. #endif
  291. #define STEP_IF_COUNTER(AXIS) \
  292. if (_COUNTER(AXIS) > 0) { \
  293. _COUNTER(AXIS) -= current_block->step_event_count; \
  294. count_position[_AXIS(AXIS)] += count_direction[_AXIS(AXIS)]; \
  295. _APPLY_STEP(AXIS)(_INVERT_STEP_PIN(AXIS),0); \
  296. }
  297. STEP_IF_COUNTER(X);
  298. STEP_IF_COUNTER(Y);
  299. STEP_IF_COUNTER(Z);
  300. #if DISABLED(ADVANCE)
  301. STEP_IF_COUNTER(E);
  302. #endif
  303. step_events_completed++;
  304. if (step_events_completed >= current_block->step_event_count) break;
  305. }
  306. // Calculate new timer value
  307. unsigned short timer, step_rate;
  308. if (step_events_completed <= (unsigned long)current_block->accelerate_until) {
  309. MultiU24X32toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
  310. acc_step_rate += current_block->initial_rate;
  311. // upper limit
  312. NOMORE(acc_step_rate, current_block->nominal_rate);
  313. // step_rate to timer interval
  314. timer = calc_timer(acc_step_rate);
  315. OCR1A = timer;
  316. acceleration_time += timer;
  317. #if ENABLED(ADVANCE)
  318. advance += advance_rate * step_loops;
  319. //NOLESS(advance, current_block->advance);
  320. // Do E steps + advance steps
  321. e_steps[current_block->active_extruder] += ((advance >> 8) - old_advance);
  322. old_advance = advance >> 8;
  323. #endif //ADVANCE
  324. }
  325. else if (step_events_completed > (unsigned long)current_block->decelerate_after) {
  326. MultiU24X32toH16(step_rate, deceleration_time, current_block->acceleration_rate);
  327. if (step_rate <= acc_step_rate) { // Still decelerating?
  328. step_rate = acc_step_rate - step_rate;
  329. NOLESS(step_rate, current_block->final_rate);
  330. }
  331. else
  332. step_rate = current_block->final_rate;
  333. // step_rate to timer interval
  334. timer = calc_timer(step_rate);
  335. OCR1A = timer;
  336. deceleration_time += timer;
  337. #if ENABLED(ADVANCE)
  338. advance -= advance_rate * step_loops;
  339. NOLESS(advance, final_advance);
  340. // Do E steps + advance steps
  341. uint32_t advance_whole = advance >> 8;
  342. e_steps[current_block->active_extruder] += advance_whole - old_advance;
  343. old_advance = advance_whole;
  344. #endif //ADVANCE
  345. }
  346. else {
  347. OCR1A = OCR1A_nominal;
  348. // ensure we're running at the correct step rate, even if we just came off an acceleration
  349. step_loops = step_loops_nominal;
  350. }
  351. OCR1A = (OCR1A < (TCNT1 + 16)) ? (TCNT1 + 16) : OCR1A;
  352. // If current block is finished, reset pointer
  353. if (step_events_completed >= current_block->step_event_count) {
  354. current_block = NULL;
  355. planner.discard_current_block();
  356. }
  357. }
  358. }
  359. #if ENABLED(ADVANCE)
  360. // Timer interrupt for E. e_steps is set in the main routine;
  361. // Timer 0 is shared with millies
  362. ISR(TIMER0_COMPA_vect) { stepper.advance_isr(); }
  363. void Stepper::advance_isr() {
  364. old_OCR0A += 52; // ~10kHz interrupt (250000 / 26 = 9615kHz)
  365. OCR0A = old_OCR0A;
  366. #define STEP_E_ONCE(INDEX) \
  367. if (e_steps[INDEX] != 0) { \
  368. E## INDEX ##_STEP_WRITE(INVERT_E_STEP_PIN); \
  369. if (e_steps[INDEX] < 0) { \
  370. E## INDEX ##_DIR_WRITE(INVERT_E## INDEX ##_DIR); \
  371. e_steps[INDEX]++; \
  372. } \
  373. else if (e_steps[INDEX] > 0) { \
  374. E## INDEX ##_DIR_WRITE(!INVERT_E## INDEX ##_DIR); \
  375. e_steps[INDEX]--; \
  376. } \
  377. E## INDEX ##_STEP_WRITE(!INVERT_E_STEP_PIN); \
  378. }
  379. // Step all E steppers that have steps, up to 4 steps per interrupt
  380. for (unsigned char i = 0; i < 4; i++) {
  381. STEP_E_ONCE(0);
  382. #if EXTRUDERS > 1
  383. STEP_E_ONCE(1);
  384. #if EXTRUDERS > 2
  385. STEP_E_ONCE(2);
  386. #if EXTRUDERS > 3
  387. STEP_E_ONCE(3);
  388. #endif
  389. #endif
  390. #endif
  391. }
  392. }
  393. #endif // ADVANCE
  394. void Stepper::init() {
  395. digipot_init(); //Initialize Digipot Motor Current
  396. microstep_init(); //Initialize Microstepping Pins
  397. // initialise TMC Steppers
  398. #if ENABLED(HAVE_TMCDRIVER)
  399. tmc_init();
  400. #endif
  401. // initialise L6470 Steppers
  402. #if ENABLED(HAVE_L6470DRIVER)
  403. L6470_init();
  404. #endif
  405. // Initialize Dir Pins
  406. #if HAS_X_DIR
  407. X_DIR_INIT;
  408. #endif
  409. #if HAS_X2_DIR
  410. X2_DIR_INIT;
  411. #endif
  412. #if HAS_Y_DIR
  413. Y_DIR_INIT;
  414. #if ENABLED(Y_DUAL_STEPPER_DRIVERS) && HAS_Y2_DIR
  415. Y2_DIR_INIT;
  416. #endif
  417. #endif
  418. #if HAS_Z_DIR
  419. Z_DIR_INIT;
  420. #if ENABLED(Z_DUAL_STEPPER_DRIVERS) && HAS_Z2_DIR
  421. Z2_DIR_INIT;
  422. #endif
  423. #endif
  424. #if HAS_E0_DIR
  425. E0_DIR_INIT;
  426. #endif
  427. #if HAS_E1_DIR
  428. E1_DIR_INIT;
  429. #endif
  430. #if HAS_E2_DIR
  431. E2_DIR_INIT;
  432. #endif
  433. #if HAS_E3_DIR
  434. E3_DIR_INIT;
  435. #endif
  436. //Initialize Enable Pins - steppers default to disabled.
  437. #if HAS_X_ENABLE
  438. X_ENABLE_INIT;
  439. if (!X_ENABLE_ON) X_ENABLE_WRITE(HIGH);
  440. #if ENABLED(DUAL_X_CARRIAGE) && HAS_X2_ENABLE
  441. X2_ENABLE_INIT;
  442. if (!X_ENABLE_ON) X2_ENABLE_WRITE(HIGH);
  443. #endif
  444. #endif
  445. #if HAS_Y_ENABLE
  446. Y_ENABLE_INIT;
  447. if (!Y_ENABLE_ON) Y_ENABLE_WRITE(HIGH);
  448. #if ENABLED(Y_DUAL_STEPPER_DRIVERS) && HAS_Y2_ENABLE
  449. Y2_ENABLE_INIT;
  450. if (!Y_ENABLE_ON) Y2_ENABLE_WRITE(HIGH);
  451. #endif
  452. #endif
  453. #if HAS_Z_ENABLE
  454. Z_ENABLE_INIT;
  455. if (!Z_ENABLE_ON) Z_ENABLE_WRITE(HIGH);
  456. #if ENABLED(Z_DUAL_STEPPER_DRIVERS) && HAS_Z2_ENABLE
  457. Z2_ENABLE_INIT;
  458. if (!Z_ENABLE_ON) Z2_ENABLE_WRITE(HIGH);
  459. #endif
  460. #endif
  461. #if HAS_E0_ENABLE
  462. E0_ENABLE_INIT;
  463. if (!E_ENABLE_ON) E0_ENABLE_WRITE(HIGH);
  464. #endif
  465. #if HAS_E1_ENABLE
  466. E1_ENABLE_INIT;
  467. if (!E_ENABLE_ON) E1_ENABLE_WRITE(HIGH);
  468. #endif
  469. #if HAS_E2_ENABLE
  470. E2_ENABLE_INIT;
  471. if (!E_ENABLE_ON) E2_ENABLE_WRITE(HIGH);
  472. #endif
  473. #if HAS_E3_ENABLE
  474. E3_ENABLE_INIT;
  475. if (!E_ENABLE_ON) E3_ENABLE_WRITE(HIGH);
  476. #endif
  477. //
  478. // Init endstops and pullups here
  479. //
  480. endstops.init();
  481. #define _STEP_INIT(AXIS) AXIS ##_STEP_INIT
  482. #define _WRITE_STEP(AXIS, HIGHLOW) AXIS ##_STEP_WRITE(HIGHLOW)
  483. #define _DISABLE(axis) disable_## axis()
  484. #define AXIS_INIT(axis, AXIS, PIN) \
  485. _STEP_INIT(AXIS); \
  486. _WRITE_STEP(AXIS, _INVERT_STEP_PIN(PIN)); \
  487. _DISABLE(axis)
  488. #define E_AXIS_INIT(NUM) AXIS_INIT(e## NUM, E## NUM, E)
  489. // Initialize Step Pins
  490. #if HAS_X_STEP
  491. AXIS_INIT(x, X, X);
  492. #if ENABLED(DUAL_X_CARRIAGE) && HAS_X2_STEP
  493. AXIS_INIT(x, X2, X);
  494. #endif
  495. #endif
  496. #if HAS_Y_STEP
  497. #if ENABLED(Y_DUAL_STEPPER_DRIVERS) && HAS_Y2_STEP
  498. Y2_STEP_INIT;
  499. Y2_STEP_WRITE(INVERT_Y_STEP_PIN);
  500. #endif
  501. AXIS_INIT(y, Y, Y);
  502. #endif
  503. #if HAS_Z_STEP
  504. #if ENABLED(Z_DUAL_STEPPER_DRIVERS) && HAS_Z2_STEP
  505. Z2_STEP_INIT;
  506. Z2_STEP_WRITE(INVERT_Z_STEP_PIN);
  507. #endif
  508. AXIS_INIT(z, Z, Z);
  509. #endif
  510. #if HAS_E0_STEP
  511. E_AXIS_INIT(0);
  512. #endif
  513. #if HAS_E1_STEP
  514. E_AXIS_INIT(1);
  515. #endif
  516. #if HAS_E2_STEP
  517. E_AXIS_INIT(2);
  518. #endif
  519. #if HAS_E3_STEP
  520. E_AXIS_INIT(3);
  521. #endif
  522. // waveform generation = 0100 = CTC
  523. CBI(TCCR1B, WGM13);
  524. SBI(TCCR1B, WGM12);
  525. CBI(TCCR1A, WGM11);
  526. CBI(TCCR1A, WGM10);
  527. // output mode = 00 (disconnected)
  528. TCCR1A &= ~(3 << COM1A0);
  529. TCCR1A &= ~(3 << COM1B0);
  530. // Set the timer pre-scaler
  531. // Generally we use a divider of 8, resulting in a 2MHz timer
  532. // frequency on a 16MHz MCU. If you are going to change this, be
  533. // sure to regenerate speed_lookuptable.h with
  534. // create_speed_lookuptable.py
  535. TCCR1B = (TCCR1B & ~(0x07 << CS10)) | (2 << CS10);
  536. OCR1A = 0x4000;
  537. TCNT1 = 0;
  538. ENABLE_STEPPER_DRIVER_INTERRUPT();
  539. #if ENABLED(ADVANCE)
  540. #if defined(TCCR0A) && defined(WGM01)
  541. CBI(TCCR0A, WGM01);
  542. CBI(TCCR0A, WGM00);
  543. #endif
  544. e_steps[0] = e_steps[1] = e_steps[2] = e_steps[3] = 0;
  545. SBI(TIMSK0, OCIE0A);
  546. #endif //ADVANCE
  547. endstops.enable(true); // Start with endstops active. After homing they can be disabled
  548. sei();
  549. set_directions(); // Init directions to last_direction_bits = 0
  550. }
  551. /**
  552. * Block until all buffered steps are executed
  553. */
  554. void Stepper::synchronize() { while (planner.blocks_queued()) idle(); }
  555. /**
  556. * Set the stepper positions directly in steps
  557. *
  558. * The input is based on the typical per-axis XYZ steps.
  559. * For CORE machines XYZ needs to be translated to ABC.
  560. *
  561. * This allows get_axis_position_mm to correctly
  562. * derive the current XYZ position later on.
  563. */
  564. void Stepper::set_position(const long& x, const long& y, const long& z, const long& e) {
  565. CRITICAL_SECTION_START;
  566. #if ENABLED(COREXY)
  567. // corexy positioning
  568. // these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
  569. count_position[A_AXIS] = x + y;
  570. count_position[B_AXIS] = x - y;
  571. count_position[Z_AXIS] = z;
  572. #elif ENABLED(COREXZ)
  573. // corexz planning
  574. count_position[A_AXIS] = x + z;
  575. count_position[Y_AXIS] = y;
  576. count_position[C_AXIS] = x - z;
  577. #elif ENABLED(COREYZ)
  578. // coreyz planning
  579. count_position[X_AXIS] = x;
  580. count_position[B_AXIS] = y + z;
  581. count_position[C_AXIS] = y - z;
  582. #else
  583. // default non-h-bot planning
  584. count_position[X_AXIS] = x;
  585. count_position[Y_AXIS] = y;
  586. count_position[Z_AXIS] = z;
  587. #endif
  588. count_position[E_AXIS] = e;
  589. CRITICAL_SECTION_END;
  590. }
  591. void Stepper::set_e_position(const long& e) {
  592. CRITICAL_SECTION_START;
  593. count_position[E_AXIS] = e;
  594. CRITICAL_SECTION_END;
  595. }
  596. /**
  597. * Get a stepper's position in steps.
  598. */
  599. long Stepper::position(AxisEnum axis) {
  600. CRITICAL_SECTION_START;
  601. long count_pos = count_position[axis];
  602. CRITICAL_SECTION_END;
  603. return count_pos;
  604. }
  605. /**
  606. * Get an axis position according to stepper position(s)
  607. * For CORE machines apply translation from ABC to XYZ.
  608. */
  609. float Stepper::get_axis_position_mm(AxisEnum axis) {
  610. float axis_steps;
  611. #if ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ)
  612. // Requesting one of the "core" axes?
  613. if (axis == CORE_AXIS_1 || axis == CORE_AXIS_2) {
  614. CRITICAL_SECTION_START;
  615. long pos1 = count_position[CORE_AXIS_1],
  616. pos2 = count_position[CORE_AXIS_2];
  617. CRITICAL_SECTION_END;
  618. // ((a1+a2)+(a1-a2))/2 -> (a1+a2+a1-a2)/2 -> (a1+a1)/2 -> a1
  619. // ((a1+a2)-(a1-a2))/2 -> (a1+a2-a1+a2)/2 -> (a2+a2)/2 -> a2
  620. axis_steps = (pos1 + ((axis == CORE_AXIS_1) ? pos2 : -pos2)) / 2.0f;
  621. }
  622. else
  623. axis_steps = position(axis);
  624. #else
  625. axis_steps = position(axis);
  626. #endif
  627. return axis_steps / planner.axis_steps_per_unit[axis];
  628. }
  629. void Stepper::finish_and_disable() {
  630. synchronize();
  631. disable_all_steppers();
  632. }
  633. void Stepper::quick_stop() {
  634. cleaning_buffer_counter = 5000;
  635. DISABLE_STEPPER_DRIVER_INTERRUPT();
  636. while (planner.blocks_queued()) planner.discard_current_block();
  637. current_block = NULL;
  638. ENABLE_STEPPER_DRIVER_INTERRUPT();
  639. }
  640. void Stepper::endstop_triggered(AxisEnum axis) {
  641. #if ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ)
  642. float axis_pos = count_position[axis];
  643. if (axis == CORE_AXIS_1)
  644. axis_pos = (axis_pos + count_position[CORE_AXIS_2]) / 2;
  645. else if (axis == CORE_AXIS_2)
  646. axis_pos = (count_position[CORE_AXIS_1] - axis_pos) / 2;
  647. endstops_trigsteps[axis] = axis_pos;
  648. #else // !COREXY && !COREXZ && !COREYZ
  649. endstops_trigsteps[axis] = count_position[axis];
  650. #endif // !COREXY && !COREXZ && !COREYZ
  651. kill_current_block();
  652. }
  653. void Stepper::report_positions() {
  654. CRITICAL_SECTION_START;
  655. long xpos = count_position[X_AXIS],
  656. ypos = count_position[Y_AXIS],
  657. zpos = count_position[Z_AXIS];
  658. CRITICAL_SECTION_END;
  659. #if ENABLED(COREXY) || ENABLED(COREXZ)
  660. SERIAL_PROTOCOLPGM(MSG_COUNT_A);
  661. #else
  662. SERIAL_PROTOCOLPGM(MSG_COUNT_X);
  663. #endif
  664. SERIAL_PROTOCOL(xpos);
  665. #if ENABLED(COREXY) || ENABLED(COREYZ)
  666. SERIAL_PROTOCOLPGM(" B:");
  667. #else
  668. SERIAL_PROTOCOLPGM(" Y:");
  669. #endif
  670. SERIAL_PROTOCOL(ypos);
  671. #if ENABLED(COREXZ) || ENABLED(COREYZ)
  672. SERIAL_PROTOCOLPGM(" C:");
  673. #else
  674. SERIAL_PROTOCOLPGM(" Z:");
  675. #endif
  676. SERIAL_PROTOCOL(zpos);
  677. SERIAL_EOL;
  678. }
  679. #if ENABLED(BABYSTEPPING)
  680. // MUST ONLY BE CALLED BY AN ISR,
  681. // No other ISR should ever interrupt this!
  682. void Stepper::babystep(const uint8_t axis, const bool direction) {
  683. #define _ENABLE(axis) enable_## axis()
  684. #define _READ_DIR(AXIS) AXIS ##_DIR_READ
  685. #define _INVERT_DIR(AXIS) INVERT_## AXIS ##_DIR
  686. #define _APPLY_DIR(AXIS, INVERT) AXIS ##_APPLY_DIR(INVERT, true)
  687. #define BABYSTEP_AXIS(axis, AXIS, INVERT) { \
  688. _ENABLE(axis); \
  689. uint8_t old_pin = _READ_DIR(AXIS); \
  690. _APPLY_DIR(AXIS, _INVERT_DIR(AXIS)^direction^INVERT); \
  691. _APPLY_STEP(AXIS)(!_INVERT_STEP_PIN(AXIS), true); \
  692. delayMicroseconds(2); \
  693. _APPLY_STEP(AXIS)(_INVERT_STEP_PIN(AXIS), true); \
  694. _APPLY_DIR(AXIS, old_pin); \
  695. }
  696. switch (axis) {
  697. case X_AXIS:
  698. BABYSTEP_AXIS(x, X, false);
  699. break;
  700. case Y_AXIS:
  701. BABYSTEP_AXIS(y, Y, false);
  702. break;
  703. case Z_AXIS: {
  704. #if DISABLED(DELTA)
  705. BABYSTEP_AXIS(z, Z, BABYSTEP_INVERT_Z);
  706. #else // DELTA
  707. bool z_direction = direction ^ BABYSTEP_INVERT_Z;
  708. enable_x();
  709. enable_y();
  710. enable_z();
  711. uint8_t old_x_dir_pin = X_DIR_READ,
  712. old_y_dir_pin = Y_DIR_READ,
  713. old_z_dir_pin = Z_DIR_READ;
  714. //setup new step
  715. X_DIR_WRITE(INVERT_X_DIR ^ z_direction);
  716. Y_DIR_WRITE(INVERT_Y_DIR ^ z_direction);
  717. Z_DIR_WRITE(INVERT_Z_DIR ^ z_direction);
  718. //perform step
  719. X_STEP_WRITE(!INVERT_X_STEP_PIN);
  720. Y_STEP_WRITE(!INVERT_Y_STEP_PIN);
  721. Z_STEP_WRITE(!INVERT_Z_STEP_PIN);
  722. delayMicroseconds(2);
  723. X_STEP_WRITE(INVERT_X_STEP_PIN);
  724. Y_STEP_WRITE(INVERT_Y_STEP_PIN);
  725. Z_STEP_WRITE(INVERT_Z_STEP_PIN);
  726. //get old pin state back.
  727. X_DIR_WRITE(old_x_dir_pin);
  728. Y_DIR_WRITE(old_y_dir_pin);
  729. Z_DIR_WRITE(old_z_dir_pin);
  730. #endif
  731. } break;
  732. default: break;
  733. }
  734. }
  735. #endif //BABYSTEPPING
  736. /**
  737. * Software-controlled Stepper Motor Current
  738. */
  739. #if HAS_DIGIPOTSS
  740. // From Arduino DigitalPotControl example
  741. void Stepper::digitalPotWrite(int address, int value) {
  742. digitalWrite(DIGIPOTSS_PIN, LOW); // take the SS pin low to select the chip
  743. SPI.transfer(address); // send in the address and value via SPI:
  744. SPI.transfer(value);
  745. digitalWrite(DIGIPOTSS_PIN, HIGH); // take the SS pin high to de-select the chip:
  746. //delay(10);
  747. }
  748. #endif //HAS_DIGIPOTSS
  749. void Stepper::digipot_init() {
  750. #if HAS_DIGIPOTSS
  751. const uint8_t digipot_motor_current[] = DIGIPOT_MOTOR_CURRENT;
  752. SPI.begin();
  753. pinMode(DIGIPOTSS_PIN, OUTPUT);
  754. for (int i = 0; i < COUNT(digipot_motor_current); i++) {
  755. //digitalPotWrite(digipot_ch[i], digipot_motor_current[i]);
  756. digipot_current(i, digipot_motor_current[i]);
  757. }
  758. #endif
  759. #if HAS_MOTOR_CURRENT_PWM
  760. #if PIN_EXISTS(MOTOR_CURRENT_PWM_XY)
  761. pinMode(MOTOR_CURRENT_PWM_XY_PIN, OUTPUT);
  762. digipot_current(0, motor_current_setting[0]);
  763. #endif
  764. #if PIN_EXISTS(MOTOR_CURRENT_PWM_Z)
  765. pinMode(MOTOR_CURRENT_PWM_Z_PIN, OUTPUT);
  766. digipot_current(1, motor_current_setting[1]);
  767. #endif
  768. #if PIN_EXISTS(MOTOR_CURRENT_PWM_E)
  769. pinMode(MOTOR_CURRENT_PWM_E_PIN, OUTPUT);
  770. digipot_current(2, motor_current_setting[2]);
  771. #endif
  772. //Set timer5 to 31khz so the PWM of the motor power is as constant as possible. (removes a buzzing noise)
  773. TCCR5B = (TCCR5B & ~(_BV(CS50) | _BV(CS51) | _BV(CS52))) | _BV(CS50);
  774. #endif
  775. }
  776. void Stepper::digipot_current(uint8_t driver, int current) {
  777. #if HAS_DIGIPOTSS
  778. const uint8_t digipot_ch[] = DIGIPOT_CHANNELS;
  779. digitalPotWrite(digipot_ch[driver], current);
  780. #elif HAS_MOTOR_CURRENT_PWM
  781. #define _WRITE_CURRENT_PWM(P) analogWrite(P, 255L * current / (MOTOR_CURRENT_PWM_RANGE))
  782. switch (driver) {
  783. #if PIN_EXISTS(MOTOR_CURRENT_PWM_XY)
  784. case 0: _WRITE_CURRENT_PWM(MOTOR_CURRENT_PWM_XY_PIN); break;
  785. #endif
  786. #if PIN_EXISTS(MOTOR_CURRENT_PWM_Z)
  787. case 1: _WRITE_CURRENT_PWM(MOTOR_CURRENT_PWM_Z_PIN); break;
  788. #endif
  789. #if PIN_EXISTS(MOTOR_CURRENT_PWM_E)
  790. case 2: _WRITE_CURRENT_PWM(MOTOR_CURRENT_PWM_E_PIN); break;
  791. #endif
  792. }
  793. #else
  794. UNUSED(driver);
  795. UNUSED(current);
  796. #endif
  797. }
  798. void Stepper::microstep_init() {
  799. #if HAS_MICROSTEPS_E1
  800. pinMode(E1_MS1_PIN, OUTPUT);
  801. pinMode(E1_MS2_PIN, OUTPUT);
  802. #endif
  803. #if HAS_MICROSTEPS
  804. pinMode(X_MS1_PIN, OUTPUT);
  805. pinMode(X_MS2_PIN, OUTPUT);
  806. pinMode(Y_MS1_PIN, OUTPUT);
  807. pinMode(Y_MS2_PIN, OUTPUT);
  808. pinMode(Z_MS1_PIN, OUTPUT);
  809. pinMode(Z_MS2_PIN, OUTPUT);
  810. pinMode(E0_MS1_PIN, OUTPUT);
  811. pinMode(E0_MS2_PIN, OUTPUT);
  812. const uint8_t microstep_modes[] = MICROSTEP_MODES;
  813. for (uint16_t i = 0; i < COUNT(microstep_modes); i++)
  814. microstep_mode(i, microstep_modes[i]);
  815. #endif
  816. }
  817. /**
  818. * Software-controlled Microstepping
  819. */
  820. void Stepper::microstep_ms(uint8_t driver, int8_t ms1, int8_t ms2) {
  821. if (ms1 >= 0) switch (driver) {
  822. case 0: digitalWrite(X_MS1_PIN, ms1); break;
  823. case 1: digitalWrite(Y_MS1_PIN, ms1); break;
  824. case 2: digitalWrite(Z_MS1_PIN, ms1); break;
  825. case 3: digitalWrite(E0_MS1_PIN, ms1); break;
  826. #if HAS_MICROSTEPS_E1
  827. case 4: digitalWrite(E1_MS1_PIN, ms1); break;
  828. #endif
  829. }
  830. if (ms2 >= 0) switch (driver) {
  831. case 0: digitalWrite(X_MS2_PIN, ms2); break;
  832. case 1: digitalWrite(Y_MS2_PIN, ms2); break;
  833. case 2: digitalWrite(Z_MS2_PIN, ms2); break;
  834. case 3: digitalWrite(E0_MS2_PIN, ms2); break;
  835. #if PIN_EXISTS(E1_MS2)
  836. case 4: digitalWrite(E1_MS2_PIN, ms2); break;
  837. #endif
  838. }
  839. }
  840. void Stepper::microstep_mode(uint8_t driver, uint8_t stepping_mode) {
  841. switch (stepping_mode) {
  842. case 1: microstep_ms(driver, MICROSTEP1); break;
  843. case 2: microstep_ms(driver, MICROSTEP2); break;
  844. case 4: microstep_ms(driver, MICROSTEP4); break;
  845. case 8: microstep_ms(driver, MICROSTEP8); break;
  846. case 16: microstep_ms(driver, MICROSTEP16); break;
  847. }
  848. }
  849. void Stepper::microstep_readings() {
  850. SERIAL_PROTOCOLPGM("MS1,MS2 Pins\n");
  851. SERIAL_PROTOCOLPGM("X: ");
  852. SERIAL_PROTOCOL(digitalRead(X_MS1_PIN));
  853. SERIAL_PROTOCOLLN(digitalRead(X_MS2_PIN));
  854. SERIAL_PROTOCOLPGM("Y: ");
  855. SERIAL_PROTOCOL(digitalRead(Y_MS1_PIN));
  856. SERIAL_PROTOCOLLN(digitalRead(Y_MS2_PIN));
  857. SERIAL_PROTOCOLPGM("Z: ");
  858. SERIAL_PROTOCOL(digitalRead(Z_MS1_PIN));
  859. SERIAL_PROTOCOLLN(digitalRead(Z_MS2_PIN));
  860. SERIAL_PROTOCOLPGM("E0: ");
  861. SERIAL_PROTOCOL(digitalRead(E0_MS1_PIN));
  862. SERIAL_PROTOCOLLN(digitalRead(E0_MS2_PIN));
  863. #if HAS_MICROSTEPS_E1
  864. SERIAL_PROTOCOLPGM("E1: ");
  865. SERIAL_PROTOCOL(digitalRead(E1_MS1_PIN));
  866. SERIAL_PROTOCOLLN(digitalRead(E1_MS2_PIN));
  867. #endif
  868. }