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.
 
 
 
 
 

2555 lines
94 KiB

  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. /**
  43. * Timer calculations informed by the 'RepRap cartesian firmware' by Zack Smith
  44. * and Philipp Tiefenbacher.
  45. */
  46. /**
  47. * __________________________
  48. * /| |\ _________________ ^
  49. * / | | \ /| |\ |
  50. * / | | \ / | | \ s
  51. * / | | | | | \ p
  52. * / | | | | | \ e
  53. * +-----+------------------------+---+--+---------------+----+ e
  54. * | BLOCK 1 | BLOCK 2 | d
  55. *
  56. * time ----->
  57. *
  58. * The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates
  59. * first block->accelerate_until step_events_completed, then keeps going at constant speed until
  60. * step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset.
  61. * The slope of acceleration is calculated using v = u + at where t is the accumulated timer values of the steps so far.
  62. */
  63. /**
  64. * Marlin uses the Bresenham algorithm. For a detailed explanation of theory and
  65. * method see https://www.cs.helsinki.fi/group/goa/mallinnus/lines/bresenh.html
  66. */
  67. /**
  68. * Jerk controlled movements planner added Apr 2018 by Eduardo José Tagle.
  69. * Equations based on Synthethos TinyG2 sources, but the fixed-point
  70. * implementation is new, as we are running the ISR with a variable period.
  71. * Also implemented the Bézier velocity curve evaluation in ARM assembler,
  72. * to avoid impacting ISR speed.
  73. */
  74. #include "stepper.h"
  75. #ifdef __AVR__
  76. #include "speed_lookuptable.h"
  77. #endif
  78. #include "endstops.h"
  79. #include "planner.h"
  80. #include "motion.h"
  81. #include "../module/temperature.h"
  82. #include "../lcd/ultralcd.h"
  83. #include "../core/language.h"
  84. #include "../gcode/queue.h"
  85. #include "../sd/cardreader.h"
  86. #include "../Marlin.h"
  87. #include "../HAL/Delay.h"
  88. #if MB(ALLIGATOR)
  89. #include "../feature/dac/dac_dac084s085.h"
  90. #endif
  91. #if HAS_DIGIPOTSS
  92. #include <SPI.h>
  93. #endif
  94. Stepper stepper; // Singleton
  95. // public:
  96. block_t* Stepper::current_block = NULL; // A pointer to the block currently being traced
  97. #if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS)
  98. bool Stepper::homing_dual_axis = false;
  99. #endif
  100. #if HAS_MOTOR_CURRENT_PWM
  101. uint32_t Stepper::motor_current_setting[3]; // Initialized by settings.load()
  102. #endif
  103. // private:
  104. uint8_t Stepper::last_direction_bits = 0,
  105. Stepper::axis_did_move;
  106. bool Stepper::abort_current_block;
  107. #if DISABLED(MIXING_EXTRUDER)
  108. uint8_t Stepper::last_moved_extruder = 0xFF;
  109. #endif
  110. #if ENABLED(X_DUAL_ENDSTOPS)
  111. bool Stepper::locked_X_motor = false, Stepper::locked_X2_motor = false;
  112. #endif
  113. #if ENABLED(Y_DUAL_ENDSTOPS)
  114. bool Stepper::locked_Y_motor = false, Stepper::locked_Y2_motor = false;
  115. #endif
  116. #if ENABLED(Z_DUAL_ENDSTOPS)
  117. bool Stepper::locked_Z_motor = false, Stepper::locked_Z2_motor = false;
  118. #endif
  119. uint32_t Stepper::acceleration_time, Stepper::deceleration_time;
  120. uint8_t Stepper::steps_per_isr;
  121. #if DISABLED(ADAPTIVE_STEP_SMOOTHING)
  122. constexpr
  123. #endif
  124. uint8_t Stepper::oversampling_factor;
  125. int32_t Stepper::delta_error[XYZE] = { 0 };
  126. uint32_t Stepper::advance_dividend[XYZE] = { 0 },
  127. Stepper::advance_divisor = 0,
  128. Stepper::step_events_completed = 0, // The number of step events executed in the current block
  129. Stepper::accelerate_until, // The point from where we need to stop acceleration
  130. Stepper::decelerate_after, // The point from where we need to start decelerating
  131. Stepper::step_event_count; // The total event count for the current block
  132. #if ENABLED(MIXING_EXTRUDER)
  133. int32_t Stepper::delta_error_m[MIXING_STEPPERS];
  134. uint32_t Stepper::advance_dividend_m[MIXING_STEPPERS],
  135. Stepper::advance_divisor_m;
  136. #else
  137. int8_t Stepper::active_extruder; // Active extruder
  138. #endif
  139. #if ENABLED(S_CURVE_ACCELERATION)
  140. int32_t __attribute__((used)) Stepper::bezier_A __asm__("bezier_A"); // A coefficient in Bézier speed curve with alias for assembler
  141. int32_t __attribute__((used)) Stepper::bezier_B __asm__("bezier_B"); // B coefficient in Bézier speed curve with alias for assembler
  142. int32_t __attribute__((used)) Stepper::bezier_C __asm__("bezier_C"); // C coefficient in Bézier speed curve with alias for assembler
  143. uint32_t __attribute__((used)) Stepper::bezier_F __asm__("bezier_F"); // F coefficient in Bézier speed curve with alias for assembler
  144. uint32_t __attribute__((used)) Stepper::bezier_AV __asm__("bezier_AV"); // AV coefficient in Bézier speed curve with alias for assembler
  145. #ifdef __AVR__
  146. bool __attribute__((used)) Stepper::A_negative __asm__("A_negative"); // If A coefficient was negative
  147. #endif
  148. bool Stepper::bezier_2nd_half; // =false If Bézier curve has been initialized or not
  149. #endif
  150. uint32_t Stepper::nextMainISR = 0;
  151. #if ENABLED(LIN_ADVANCE)
  152. constexpr uint32_t LA_ADV_NEVER = 0xFFFFFFFF;
  153. uint32_t Stepper::nextAdvanceISR = LA_ADV_NEVER,
  154. Stepper::LA_isr_rate = LA_ADV_NEVER;
  155. uint16_t Stepper::LA_current_adv_steps = 0,
  156. Stepper::LA_final_adv_steps,
  157. Stepper::LA_max_adv_steps;
  158. int8_t Stepper::LA_steps = 0;
  159. bool Stepper::LA_use_advance_lead;
  160. #endif // LIN_ADVANCE
  161. int32_t Stepper::ticks_nominal = -1;
  162. #if DISABLED(S_CURVE_ACCELERATION)
  163. uint32_t Stepper::acc_step_rate; // needed for deceleration start point
  164. #endif
  165. volatile int32_t Stepper::endstops_trigsteps[XYZ];
  166. volatile int32_t Stepper::count_position[NUM_AXIS] = { 0 };
  167. int8_t Stepper::count_direction[NUM_AXIS] = { 0, 0, 0, 0 };
  168. #if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS)
  169. #define DUAL_ENDSTOP_APPLY_STEP(A,V) \
  170. if (homing_dual_axis) { \
  171. if (A##_HOME_DIR < 0) { \
  172. if (!(TEST(endstops.state(), A##_MIN) && count_direction[_AXIS(A)] < 0) && !locked_##A##_motor) A##_STEP_WRITE(V); \
  173. if (!(TEST(endstops.state(), A##2_MIN) && count_direction[_AXIS(A)] < 0) && !locked_##A##2_motor) A##2_STEP_WRITE(V); \
  174. } \
  175. else { \
  176. if (!(TEST(endstops.state(), A##_MAX) && count_direction[_AXIS(A)] > 0) && !locked_##A##_motor) A##_STEP_WRITE(V); \
  177. if (!(TEST(endstops.state(), A##2_MAX) && count_direction[_AXIS(A)] > 0) && !locked_##A##2_motor) A##2_STEP_WRITE(V); \
  178. } \
  179. } \
  180. else { \
  181. A##_STEP_WRITE(V); \
  182. A##2_STEP_WRITE(V); \
  183. }
  184. #endif
  185. #if ENABLED(X_DUAL_STEPPER_DRIVERS)
  186. #define X_APPLY_DIR(v,Q) do{ X_DIR_WRITE(v); X2_DIR_WRITE((v) != INVERT_X2_VS_X_DIR); }while(0)
  187. #if ENABLED(X_DUAL_ENDSTOPS)
  188. #define X_APPLY_STEP(v,Q) DUAL_ENDSTOP_APPLY_STEP(X,v)
  189. #else
  190. #define X_APPLY_STEP(v,Q) do{ X_STEP_WRITE(v); X2_STEP_WRITE(v); }while(0)
  191. #endif
  192. #elif ENABLED(DUAL_X_CARRIAGE)
  193. #define X_APPLY_DIR(v,ALWAYS) \
  194. if (extruder_duplication_enabled || ALWAYS) { \
  195. X_DIR_WRITE(v); \
  196. X2_DIR_WRITE(v); \
  197. } \
  198. else { \
  199. if (movement_extruder()) X2_DIR_WRITE(v); else X_DIR_WRITE(v); \
  200. }
  201. #define X_APPLY_STEP(v,ALWAYS) \
  202. if (extruder_duplication_enabled || ALWAYS) { \
  203. X_STEP_WRITE(v); \
  204. X2_STEP_WRITE(v); \
  205. } \
  206. else { \
  207. if (movement_extruder()) X2_STEP_WRITE(v); else X_STEP_WRITE(v); \
  208. }
  209. #else
  210. #define X_APPLY_DIR(v,Q) X_DIR_WRITE(v)
  211. #define X_APPLY_STEP(v,Q) X_STEP_WRITE(v)
  212. #endif
  213. #if ENABLED(Y_DUAL_STEPPER_DRIVERS)
  214. #define Y_APPLY_DIR(v,Q) do{ Y_DIR_WRITE(v); Y2_DIR_WRITE((v) != INVERT_Y2_VS_Y_DIR); }while(0)
  215. #if ENABLED(Y_DUAL_ENDSTOPS)
  216. #define Y_APPLY_STEP(v,Q) DUAL_ENDSTOP_APPLY_STEP(Y,v)
  217. #else
  218. #define Y_APPLY_STEP(v,Q) do{ Y_STEP_WRITE(v); Y2_STEP_WRITE(v); }while(0)
  219. #endif
  220. #else
  221. #define Y_APPLY_DIR(v,Q) Y_DIR_WRITE(v)
  222. #define Y_APPLY_STEP(v,Q) Y_STEP_WRITE(v)
  223. #endif
  224. #if ENABLED(Z_DUAL_STEPPER_DRIVERS)
  225. #define Z_APPLY_DIR(v,Q) do{ Z_DIR_WRITE(v); Z2_DIR_WRITE(v); }while(0)
  226. #if ENABLED(Z_DUAL_ENDSTOPS)
  227. #define Z_APPLY_STEP(v,Q) DUAL_ENDSTOP_APPLY_STEP(Z,v)
  228. #else
  229. #define Z_APPLY_STEP(v,Q) do{ Z_STEP_WRITE(v); Z2_STEP_WRITE(v); }while(0)
  230. #endif
  231. #else
  232. #define Z_APPLY_DIR(v,Q) Z_DIR_WRITE(v)
  233. #define Z_APPLY_STEP(v,Q) Z_STEP_WRITE(v)
  234. #endif
  235. #if DISABLED(MIXING_EXTRUDER)
  236. #define E_APPLY_STEP(v,Q) E_STEP_WRITE(active_extruder, v)
  237. #endif
  238. void Stepper::wake_up() {
  239. // TCNT1 = 0;
  240. ENABLE_STEPPER_DRIVER_INTERRUPT();
  241. }
  242. /**
  243. * Set the stepper direction of each axis
  244. *
  245. * COREXY: X_AXIS=A_AXIS and Y_AXIS=B_AXIS
  246. * COREXZ: X_AXIS=A_AXIS and Z_AXIS=C_AXIS
  247. * COREYZ: Y_AXIS=B_AXIS and Z_AXIS=C_AXIS
  248. */
  249. void Stepper::set_directions() {
  250. #define SET_STEP_DIR(A) \
  251. if (motor_direction(_AXIS(A))) { \
  252. A##_APPLY_DIR(INVERT_## A##_DIR, false); \
  253. count_direction[_AXIS(A)] = -1; \
  254. } \
  255. else { \
  256. A##_APPLY_DIR(!INVERT_## A##_DIR, false); \
  257. count_direction[_AXIS(A)] = 1; \
  258. }
  259. #if HAS_X_DIR
  260. SET_STEP_DIR(X); // A
  261. #endif
  262. #if HAS_Y_DIR
  263. SET_STEP_DIR(Y); // B
  264. #endif
  265. #if HAS_Z_DIR
  266. SET_STEP_DIR(Z); // C
  267. #endif
  268. #if DISABLED(LIN_ADVANCE)
  269. #if ENABLED(MIXING_EXTRUDER)
  270. if (motor_direction(E_AXIS)) {
  271. MIXING_STEPPERS_LOOP(j) REV_E_DIR(j);
  272. count_direction[E_AXIS] = -1;
  273. }
  274. else {
  275. MIXING_STEPPERS_LOOP(j) NORM_E_DIR(j);
  276. count_direction[E_AXIS] = 1;
  277. }
  278. #else
  279. if (motor_direction(E_AXIS)) {
  280. REV_E_DIR(active_extruder);
  281. count_direction[E_AXIS] = -1;
  282. }
  283. else {
  284. NORM_E_DIR(active_extruder);
  285. count_direction[E_AXIS] = 1;
  286. }
  287. #endif
  288. #endif // !LIN_ADVANCE
  289. }
  290. #if ENABLED(S_CURVE_ACCELERATION)
  291. /**
  292. * This uses a quintic (fifth-degree) Bézier polynomial for the velocity curve, giving
  293. * a "linear pop" velocity curve; with pop being the sixth derivative of position:
  294. * velocity - 1st, acceleration - 2nd, jerk - 3rd, snap - 4th, crackle - 5th, pop - 6th
  295. *
  296. * The Bézier curve takes the form:
  297. *
  298. * V(t) = P_0 * B_0(t) + P_1 * B_1(t) + P_2 * B_2(t) + P_3 * B_3(t) + P_4 * B_4(t) + P_5 * B_5(t)
  299. *
  300. * Where 0 <= t <= 1, and V(t) is the velocity. P_0 through P_5 are the control points, and B_0(t)
  301. * through B_5(t) are the Bernstein basis as follows:
  302. *
  303. * B_0(t) = (1-t)^5 = -t^5 + 5t^4 - 10t^3 + 10t^2 - 5t + 1
  304. * B_1(t) = 5(1-t)^4 * t = 5t^5 - 20t^4 + 30t^3 - 20t^2 + 5t
  305. * B_2(t) = 10(1-t)^3 * t^2 = -10t^5 + 30t^4 - 30t^3 + 10t^2
  306. * B_3(t) = 10(1-t)^2 * t^3 = 10t^5 - 20t^4 + 10t^3
  307. * B_4(t) = 5(1-t) * t^4 = -5t^5 + 5t^4
  308. * B_5(t) = t^5 = t^5
  309. * ^ ^ ^ ^ ^ ^
  310. * | | | | | |
  311. * A B C D E F
  312. *
  313. * Unfortunately, we cannot use forward-differencing to calculate each position through
  314. * the curve, as Marlin uses variable timer periods. So, we require a formula of the form:
  315. *
  316. * V_f(t) = A*t^5 + B*t^4 + C*t^3 + D*t^2 + E*t + F
  317. *
  318. * Looking at the above B_0(t) through B_5(t) expanded forms, if we take the coefficients of t^5
  319. * through t of the Bézier form of V(t), we can determine that:
  320. *
  321. * A = -P_0 + 5*P_1 - 10*P_2 + 10*P_3 - 5*P_4 + P_5
  322. * B = 5*P_0 - 20*P_1 + 30*P_2 - 20*P_3 + 5*P_4
  323. * C = -10*P_0 + 30*P_1 - 30*P_2 + 10*P_3
  324. * D = 10*P_0 - 20*P_1 + 10*P_2
  325. * E = - 5*P_0 + 5*P_1
  326. * F = P_0
  327. *
  328. * Now, since we will (currently) *always* want the initial acceleration and jerk values to be 0,
  329. * We set P_i = P_0 = P_1 = P_2 (initial velocity), and P_t = P_3 = P_4 = P_5 (target velocity),
  330. * which, after simplification, resolves to:
  331. *
  332. * A = - 6*P_i + 6*P_t = 6*(P_t - P_i)
  333. * B = 15*P_i - 15*P_t = 15*(P_i - P_t)
  334. * C = -10*P_i + 10*P_t = 10*(P_t - P_i)
  335. * D = 0
  336. * E = 0
  337. * F = P_i
  338. *
  339. * As the t is evaluated in non uniform steps here, there is no other way rather than evaluating
  340. * the Bézier curve at each point:
  341. *
  342. * V_f(t) = A*t^5 + B*t^4 + C*t^3 + F [0 <= t <= 1]
  343. *
  344. * Floating point arithmetic execution time cost is prohibitive, so we will transform the math to
  345. * use fixed point values to be able to evaluate it in realtime. Assuming a maximum of 250000 steps
  346. * per second (driver pulses should at least be 2µS hi/2µS lo), and allocating 2 bits to avoid
  347. * overflows on the evaluation of the Bézier curve, means we can use
  348. *
  349. * t: unsigned Q0.32 (0 <= t < 1) |range 0 to 0xFFFFFFFF unsigned
  350. * A: signed Q24.7 , |range = +/- 250000 * 6 * 128 = +/- 192000000 = 0x0B71B000 | 28 bits + sign
  351. * B: signed Q24.7 , |range = +/- 250000 *15 * 128 = +/- 480000000 = 0x1C9C3800 | 29 bits + sign
  352. * C: signed Q24.7 , |range = +/- 250000 *10 * 128 = +/- 320000000 = 0x1312D000 | 29 bits + sign
  353. * F: signed Q24.7 , |range = +/- 250000 * 128 = 32000000 = 0x01E84800 | 25 bits + sign
  354. *
  355. * The trapezoid generator state contains the following information, that we will use to create and evaluate
  356. * the Bézier curve:
  357. *
  358. * blk->step_event_count [TS] = The total count of steps for this movement. (=distance)
  359. * blk->initial_rate [VI] = The initial steps per second (=velocity)
  360. * blk->final_rate [VF] = The ending steps per second (=velocity)
  361. * and the count of events completed (step_events_completed) [CS] (=distance until now)
  362. *
  363. * Note the abbreviations we use in the following formulae are between []s
  364. *
  365. * For Any 32bit CPU:
  366. *
  367. * At the start of each trapezoid, calculate the coefficients A,B,C,F and Advance [AV], as follows:
  368. *
  369. * A = 6*128*(VF - VI) = 768*(VF - VI)
  370. * B = 15*128*(VI - VF) = 1920*(VI - VF)
  371. * C = 10*128*(VF - VI) = 1280*(VF - VI)
  372. * F = 128*VI = 128*VI
  373. * AV = (1<<32)/TS ~= 0xFFFFFFFF / TS (To use ARM UDIV, that is 32 bits) (this is computed at the planner, to offload expensive calculations from the ISR)
  374. *
  375. * And for each point, evaluate the curve with the following sequence:
  376. *
  377. * void lsrs(uint32_t& d, uint32_t s, int cnt) {
  378. * d = s >> cnt;
  379. * }
  380. * void lsls(uint32_t& d, uint32_t s, int cnt) {
  381. * d = s << cnt;
  382. * }
  383. * void lsrs(int32_t& d, uint32_t s, int cnt) {
  384. * d = uint32_t(s) >> cnt;
  385. * }
  386. * void lsls(int32_t& d, uint32_t s, int cnt) {
  387. * d = uint32_t(s) << cnt;
  388. * }
  389. * void umull(uint32_t& rlo, uint32_t& rhi, uint32_t op1, uint32_t op2) {
  390. * uint64_t res = uint64_t(op1) * op2;
  391. * rlo = uint32_t(res & 0xFFFFFFFF);
  392. * rhi = uint32_t((res >> 32) & 0xFFFFFFFF);
  393. * }
  394. * void smlal(int32_t& rlo, int32_t& rhi, int32_t op1, int32_t op2) {
  395. * int64_t mul = int64_t(op1) * op2;
  396. * int64_t s = int64_t(uint32_t(rlo) | ((uint64_t(uint32_t(rhi)) << 32U)));
  397. * mul += s;
  398. * rlo = int32_t(mul & 0xFFFFFFFF);
  399. * rhi = int32_t((mul >> 32) & 0xFFFFFFFF);
  400. * }
  401. * int32_t _eval_bezier_curve_arm(uint32_t curr_step) {
  402. * register uint32_t flo = 0;
  403. * register uint32_t fhi = bezier_AV * curr_step;
  404. * register uint32_t t = fhi;
  405. * register int32_t alo = bezier_F;
  406. * register int32_t ahi = 0;
  407. * register int32_t A = bezier_A;
  408. * register int32_t B = bezier_B;
  409. * register int32_t C = bezier_C;
  410. *
  411. * lsrs(ahi, alo, 1); // a = F << 31
  412. * lsls(alo, alo, 31); //
  413. * umull(flo, fhi, fhi, t); // f *= t
  414. * umull(flo, fhi, fhi, t); // f>>=32; f*=t
  415. * lsrs(flo, fhi, 1); //
  416. * smlal(alo, ahi, flo, C); // a+=(f>>33)*C
  417. * umull(flo, fhi, fhi, t); // f>>=32; f*=t
  418. * lsrs(flo, fhi, 1); //
  419. * smlal(alo, ahi, flo, B); // a+=(f>>33)*B
  420. * umull(flo, fhi, fhi, t); // f>>=32; f*=t
  421. * lsrs(flo, fhi, 1); // f>>=33;
  422. * smlal(alo, ahi, flo, A); // a+=(f>>33)*A;
  423. * lsrs(alo, ahi, 6); // a>>=38
  424. *
  425. * return alo;
  426. * }
  427. *
  428. * This is rewritten in ARM assembly for optimal performance (43 cycles to execute).
  429. *
  430. * For AVR, the precision of coefficients is scaled so the Bézier curve can be evaluated in real-time:
  431. * Let's reduce precision as much as possible. After some experimentation we found that:
  432. *
  433. * Assume t and AV with 24 bits is enough
  434. * A = 6*(VF - VI)
  435. * B = 15*(VI - VF)
  436. * C = 10*(VF - VI)
  437. * F = VI
  438. * AV = (1<<24)/TS (this is computed at the planner, to offload expensive calculations from the ISR)
  439. *
  440. * Instead of storing sign for each coefficient, we will store its absolute value,
  441. * and flag the sign of the A coefficient, so we can save to store the sign bit.
  442. * It always holds that sign(A) = - sign(B) = sign(C)
  443. *
  444. * So, the resulting range of the coefficients are:
  445. *
  446. * t: unsigned (0 <= t < 1) |range 0 to 0xFFFFFF unsigned
  447. * A: signed Q24 , range = 250000 * 6 = 1500000 = 0x16E360 | 21 bits
  448. * B: signed Q24 , range = 250000 *15 = 3750000 = 0x393870 | 22 bits
  449. * C: signed Q24 , range = 250000 *10 = 2500000 = 0x1312D0 | 21 bits
  450. * F: signed Q24 , range = 250000 = 250000 = 0x0ED090 | 20 bits
  451. *
  452. * And for each curve, estimate its coefficients with:
  453. *
  454. * void _calc_bezier_curve_coeffs(int32_t v0, int32_t v1, uint32_t av) {
  455. * // Calculate the Bézier coefficients
  456. * if (v1 < v0) {
  457. * A_negative = true;
  458. * bezier_A = 6 * (v0 - v1);
  459. * bezier_B = 15 * (v0 - v1);
  460. * bezier_C = 10 * (v0 - v1);
  461. * }
  462. * else {
  463. * A_negative = false;
  464. * bezier_A = 6 * (v1 - v0);
  465. * bezier_B = 15 * (v1 - v0);
  466. * bezier_C = 10 * (v1 - v0);
  467. * }
  468. * bezier_F = v0;
  469. * }
  470. *
  471. * And for each point, evaluate the curve with the following sequence:
  472. *
  473. * // unsigned multiplication of 24 bits x 24bits, return upper 16 bits
  474. * void umul24x24to16hi(uint16_t& r, uint24_t op1, uint24_t op2) {
  475. * r = (uint64_t(op1) * op2) >> 8;
  476. * }
  477. * // unsigned multiplication of 16 bits x 16bits, return upper 16 bits
  478. * void umul16x16to16hi(uint16_t& r, uint16_t op1, uint16_t op2) {
  479. * r = (uint32_t(op1) * op2) >> 16;
  480. * }
  481. * // unsigned multiplication of 16 bits x 24bits, return upper 24 bits
  482. * void umul16x24to24hi(uint24_t& r, uint16_t op1, uint24_t op2) {
  483. * r = uint24_t((uint64_t(op1) * op2) >> 16);
  484. * }
  485. *
  486. * int32_t _eval_bezier_curve(uint32_t curr_step) {
  487. * // To save computing, the first step is always the initial speed
  488. * if (!curr_step)
  489. * return bezier_F;
  490. *
  491. * uint16_t t;
  492. * umul24x24to16hi(t, bezier_AV, curr_step); // t: Range 0 - 1^16 = 16 bits
  493. * uint16_t f = t;
  494. * umul16x16to16hi(f, f, t); // Range 16 bits (unsigned)
  495. * umul16x16to16hi(f, f, t); // Range 16 bits : f = t^3 (unsigned)
  496. * uint24_t acc = bezier_F; // Range 20 bits (unsigned)
  497. * if (A_negative) {
  498. * uint24_t v;
  499. * umul16x24to24hi(v, f, bezier_C); // Range 21bits
  500. * acc -= v;
  501. * umul16x16to16hi(f, f, t); // Range 16 bits : f = t^4 (unsigned)
  502. * umul16x24to24hi(v, f, bezier_B); // Range 22bits
  503. * acc += v;
  504. * umul16x16to16hi(f, f, t); // Range 16 bits : f = t^5 (unsigned)
  505. * umul16x24to24hi(v, f, bezier_A); // Range 21bits + 15 = 36bits (plus sign)
  506. * acc -= v;
  507. * }
  508. * else {
  509. * uint24_t v;
  510. * umul16x24to24hi(v, f, bezier_C); // Range 21bits
  511. * acc += v;
  512. * umul16x16to16hi(f, f, t); // Range 16 bits : f = t^4 (unsigned)
  513. * umul16x24to24hi(v, f, bezier_B); // Range 22bits
  514. * acc -= v;
  515. * umul16x16to16hi(f, f, t); // Range 16 bits : f = t^5 (unsigned)
  516. * umul16x24to24hi(v, f, bezier_A); // Range 21bits + 15 = 36bits (plus sign)
  517. * acc += v;
  518. * }
  519. * return acc;
  520. * }
  521. * These functions are translated to assembler for optimal performance.
  522. * Coefficient calculation takes 70 cycles. Bezier point evaluation takes 150 cycles.
  523. */
  524. #ifdef __AVR__
  525. // For AVR we use assembly to maximize speed
  526. void Stepper::_calc_bezier_curve_coeffs(const int32_t v0, const int32_t v1, const uint32_t av) {
  527. // Store advance
  528. bezier_AV = av;
  529. // Calculate the rest of the coefficients
  530. register uint8_t r2 = v0 & 0xFF;
  531. register uint8_t r3 = (v0 >> 8) & 0xFF;
  532. register uint8_t r12 = (v0 >> 16) & 0xFF;
  533. register uint8_t r5 = v1 & 0xFF;
  534. register uint8_t r6 = (v1 >> 8) & 0xFF;
  535. register uint8_t r7 = (v1 >> 16) & 0xFF;
  536. register uint8_t r4,r8,r9,r10,r11;
  537. __asm__ __volatile__(
  538. /* Calculate the Bézier coefficients */
  539. /* %10:%1:%0 = v0*/
  540. /* %5:%4:%3 = v1*/
  541. /* %7:%6:%10 = temporary*/
  542. /* %9 = val (must be high register!)*/
  543. /* %10 (must be high register!)*/
  544. /* Store initial velocity*/
  545. A("sts bezier_F, %0")
  546. A("sts bezier_F+1, %1")
  547. A("sts bezier_F+2, %10") /* bezier_F = %10:%1:%0 = v0 */
  548. /* Get delta speed */
  549. A("ldi %2,-1") /* %2 = 0xFF, means A_negative = true */
  550. A("clr %8") /* %8 = 0 */
  551. A("sub %0,%3")
  552. A("sbc %1,%4")
  553. A("sbc %10,%5") /* v0 -= v1, C=1 if result is negative */
  554. A("brcc 1f") /* branch if result is positive (C=0), that means v0 >= v1 */
  555. /* Result was negative, get the absolute value*/
  556. A("com %10")
  557. A("com %1")
  558. A("neg %0")
  559. A("sbc %1,%2")
  560. A("sbc %10,%2") /* %10:%1:%0 +1 -> %10:%1:%0 = -(v0 - v1) = (v1 - v0) */
  561. A("clr %2") /* %2 = 0, means A_negative = false */
  562. /* Store negative flag*/
  563. L("1")
  564. A("sts A_negative, %2") /* Store negative flag */
  565. /* Compute coefficients A,B and C [20 cycles worst case]*/
  566. A("ldi %9,6") /* %9 = 6 */
  567. A("mul %0,%9") /* r1:r0 = 6*LO(v0-v1) */
  568. A("sts bezier_A, r0")
  569. A("mov %6,r1")
  570. A("clr %7") /* %7:%6:r0 = 6*LO(v0-v1) */
  571. A("mul %1,%9") /* r1:r0 = 6*MI(v0-v1) */
  572. A("add %6,r0")
  573. A("adc %7,r1") /* %7:%6:?? += 6*MI(v0-v1) << 8 */
  574. A("mul %10,%9") /* r1:r0 = 6*HI(v0-v1) */
  575. A("add %7,r0") /* %7:%6:?? += 6*HI(v0-v1) << 16 */
  576. A("sts bezier_A+1, %6")
  577. A("sts bezier_A+2, %7") /* bezier_A = %7:%6:?? = 6*(v0-v1) [35 cycles worst] */
  578. A("ldi %9,15") /* %9 = 15 */
  579. A("mul %0,%9") /* r1:r0 = 5*LO(v0-v1) */
  580. A("sts bezier_B, r0")
  581. A("mov %6,r1")
  582. A("clr %7") /* %7:%6:?? = 5*LO(v0-v1) */
  583. A("mul %1,%9") /* r1:r0 = 5*MI(v0-v1) */
  584. A("add %6,r0")
  585. A("adc %7,r1") /* %7:%6:?? += 5*MI(v0-v1) << 8 */
  586. A("mul %10,%9") /* r1:r0 = 5*HI(v0-v1) */
  587. A("add %7,r0") /* %7:%6:?? += 5*HI(v0-v1) << 16 */
  588. A("sts bezier_B+1, %6")
  589. A("sts bezier_B+2, %7") /* bezier_B = %7:%6:?? = 5*(v0-v1) [50 cycles worst] */
  590. A("ldi %9,10") /* %9 = 10 */
  591. A("mul %0,%9") /* r1:r0 = 10*LO(v0-v1) */
  592. A("sts bezier_C, r0")
  593. A("mov %6,r1")
  594. A("clr %7") /* %7:%6:?? = 10*LO(v0-v1) */
  595. A("mul %1,%9") /* r1:r0 = 10*MI(v0-v1) */
  596. A("add %6,r0")
  597. A("adc %7,r1") /* %7:%6:?? += 10*MI(v0-v1) << 8 */
  598. A("mul %10,%9") /* r1:r0 = 10*HI(v0-v1) */
  599. A("add %7,r0") /* %7:%6:?? += 10*HI(v0-v1) << 16 */
  600. A("sts bezier_C+1, %6")
  601. " sts bezier_C+2, %7" /* bezier_C = %7:%6:?? = 10*(v0-v1) [65 cycles worst] */
  602. : "+r" (r2),
  603. "+d" (r3),
  604. "=r" (r4),
  605. "+r" (r5),
  606. "+r" (r6),
  607. "+r" (r7),
  608. "=r" (r8),
  609. "=r" (r9),
  610. "=r" (r10),
  611. "=d" (r11),
  612. "+r" (r12)
  613. :
  614. : "r0", "r1", "cc", "memory"
  615. );
  616. }
  617. FORCE_INLINE int32_t Stepper::_eval_bezier_curve(const uint32_t curr_step) {
  618. // If dealing with the first step, save expensive computing and return the initial speed
  619. if (!curr_step)
  620. return bezier_F;
  621. register uint8_t r0 = 0; /* Zero register */
  622. register uint8_t r2 = (curr_step) & 0xFF;
  623. register uint8_t r3 = (curr_step >> 8) & 0xFF;
  624. register uint8_t r4 = (curr_step >> 16) & 0xFF;
  625. register uint8_t r1,r5,r6,r7,r8,r9,r10,r11; /* Temporary registers */
  626. __asm__ __volatile(
  627. /* umul24x24to16hi(t, bezier_AV, curr_step); t: Range 0 - 1^16 = 16 bits*/
  628. A("lds %9,bezier_AV") /* %9 = LO(AV)*/
  629. A("mul %9,%2") /* r1:r0 = LO(bezier_AV)*LO(curr_step)*/
  630. A("mov %7,r1") /* %7 = LO(bezier_AV)*LO(curr_step) >> 8*/
  631. A("clr %8") /* %8:%7 = LO(bezier_AV)*LO(curr_step) >> 8*/
  632. A("lds %10,bezier_AV+1") /* %10 = MI(AV)*/
  633. A("mul %10,%2") /* r1:r0 = MI(bezier_AV)*LO(curr_step)*/
  634. A("add %7,r0")
  635. A("adc %8,r1") /* %8:%7 += MI(bezier_AV)*LO(curr_step)*/
  636. A("lds r1,bezier_AV+2") /* r11 = HI(AV)*/
  637. A("mul r1,%2") /* r1:r0 = HI(bezier_AV)*LO(curr_step)*/
  638. A("add %8,r0") /* %8:%7 += HI(bezier_AV)*LO(curr_step) << 8*/
  639. A("mul %9,%3") /* r1:r0 = LO(bezier_AV)*MI(curr_step)*/
  640. A("add %7,r0")
  641. A("adc %8,r1") /* %8:%7 += LO(bezier_AV)*MI(curr_step)*/
  642. A("mul %10,%3") /* r1:r0 = MI(bezier_AV)*MI(curr_step)*/
  643. A("add %8,r0") /* %8:%7 += LO(bezier_AV)*MI(curr_step) << 8*/
  644. A("mul %9,%4") /* r1:r0 = LO(bezier_AV)*HI(curr_step)*/
  645. A("add %8,r0") /* %8:%7 += LO(bezier_AV)*HI(curr_step) << 8*/
  646. /* %8:%7 = t*/
  647. /* uint16_t f = t;*/
  648. A("mov %5,%7") /* %6:%5 = f*/
  649. A("mov %6,%8")
  650. /* %6:%5 = f*/
  651. /* umul16x16to16hi(f, f, t); / Range 16 bits (unsigned) [17] */
  652. A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/
  653. A("mov %9,r1") /* store MIL(LO(f) * LO(t)) in %9, we need it for rounding*/
  654. A("clr %10") /* %10 = 0*/
  655. A("clr %11") /* %11 = 0*/
  656. A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/
  657. A("add %9,r0") /* %9 += LO(LO(f) * HI(t))*/
  658. A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/
  659. A("adc %11,%0") /* %11 += carry*/
  660. A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/
  661. A("add %9,r0") /* %9 += LO(HI(f) * LO(t))*/
  662. A("adc %10,r1") /* %10 += HI(HI(f) * LO(t)) */
  663. A("adc %11,%0") /* %11 += carry*/
  664. A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/
  665. A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/
  666. A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/
  667. A("mov %5,%10") /* %6:%5 = */
  668. A("mov %6,%11") /* f = %10:%11*/
  669. /* umul16x16to16hi(f, f, t); / Range 16 bits : f = t^3 (unsigned) [17]*/
  670. A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/
  671. A("mov %1,r1") /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/
  672. A("clr %10") /* %10 = 0*/
  673. A("clr %11") /* %11 = 0*/
  674. A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/
  675. A("add %1,r0") /* %1 += LO(LO(f) * HI(t))*/
  676. A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/
  677. A("adc %11,%0") /* %11 += carry*/
  678. A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/
  679. A("add %1,r0") /* %1 += LO(HI(f) * LO(t))*/
  680. A("adc %10,r1") /* %10 += HI(HI(f) * LO(t))*/
  681. A("adc %11,%0") /* %11 += carry*/
  682. A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/
  683. A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/
  684. A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/
  685. A("mov %5,%10") /* %6:%5 =*/
  686. A("mov %6,%11") /* f = %10:%11*/
  687. /* [15 +17*2] = [49]*/
  688. /* %4:%3:%2 will be acc from now on*/
  689. /* uint24_t acc = bezier_F; / Range 20 bits (unsigned)*/
  690. A("clr %9") /* "decimal place we get for free"*/
  691. A("lds %2,bezier_F")
  692. A("lds %3,bezier_F+1")
  693. A("lds %4,bezier_F+2") /* %4:%3:%2 = acc*/
  694. /* if (A_negative) {*/
  695. A("lds r0,A_negative")
  696. A("or r0,%0") /* Is flag signalling negative? */
  697. A("brne 3f") /* If yes, Skip next instruction if A was negative*/
  698. A("rjmp 1f") /* Otherwise, jump */
  699. /* uint24_t v; */
  700. /* umul16x24to24hi(v, f, bezier_C); / Range 21bits [29] */
  701. /* acc -= v; */
  702. L("3")
  703. A("lds %10, bezier_C") /* %10 = LO(bezier_C)*/
  704. A("mul %10,%5") /* r1:r0 = LO(bezier_C) * LO(f)*/
  705. A("sub %9,r1")
  706. A("sbc %2,%0")
  707. A("sbc %3,%0")
  708. A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(LO(bezier_C) * LO(f))*/
  709. A("lds %11, bezier_C+1") /* %11 = MI(bezier_C)*/
  710. A("mul %11,%5") /* r1:r0 = MI(bezier_C) * LO(f)*/
  711. A("sub %9,r0")
  712. A("sbc %2,r1")
  713. A("sbc %3,%0")
  714. A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_C) * LO(f)*/
  715. A("lds %1, bezier_C+2") /* %1 = HI(bezier_C)*/
  716. A("mul %1,%5") /* r1:r0 = MI(bezier_C) * LO(f)*/
  717. A("sub %2,r0")
  718. A("sbc %3,r1")
  719. A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(bezier_C) * LO(f) << 8*/
  720. A("mul %10,%6") /* r1:r0 = LO(bezier_C) * MI(f)*/
  721. A("sub %9,r0")
  722. A("sbc %2,r1")
  723. A("sbc %3,%0")
  724. A("sbc %4,%0") /* %4:%3:%2:%9 -= LO(bezier_C) * MI(f)*/
  725. A("mul %11,%6") /* r1:r0 = MI(bezier_C) * MI(f)*/
  726. A("sub %2,r0")
  727. A("sbc %3,r1")
  728. A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_C) * MI(f) << 8*/
  729. A("mul %1,%6") /* r1:r0 = HI(bezier_C) * LO(f)*/
  730. A("sub %3,r0")
  731. A("sbc %4,r1") /* %4:%3:%2:%9 -= HI(bezier_C) * LO(f) << 16*/
  732. /* umul16x16to16hi(f, f, t); / Range 16 bits : f = t^3 (unsigned) [17]*/
  733. A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/
  734. A("mov %1,r1") /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/
  735. A("clr %10") /* %10 = 0*/
  736. A("clr %11") /* %11 = 0*/
  737. A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/
  738. A("add %1,r0") /* %1 += LO(LO(f) * HI(t))*/
  739. A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/
  740. A("adc %11,%0") /* %11 += carry*/
  741. A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/
  742. A("add %1,r0") /* %1 += LO(HI(f) * LO(t))*/
  743. A("adc %10,r1") /* %10 += HI(HI(f) * LO(t))*/
  744. A("adc %11,%0") /* %11 += carry*/
  745. A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/
  746. A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/
  747. A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/
  748. A("mov %5,%10") /* %6:%5 =*/
  749. A("mov %6,%11") /* f = %10:%11*/
  750. /* umul16x24to24hi(v, f, bezier_B); / Range 22bits [29]*/
  751. /* acc += v; */
  752. A("lds %10, bezier_B") /* %10 = LO(bezier_B)*/
  753. A("mul %10,%5") /* r1:r0 = LO(bezier_B) * LO(f)*/
  754. A("add %9,r1")
  755. A("adc %2,%0")
  756. A("adc %3,%0")
  757. A("adc %4,%0") /* %4:%3:%2:%9 += HI(LO(bezier_B) * LO(f))*/
  758. A("lds %11, bezier_B+1") /* %11 = MI(bezier_B)*/
  759. A("mul %11,%5") /* r1:r0 = MI(bezier_B) * LO(f)*/
  760. A("add %9,r0")
  761. A("adc %2,r1")
  762. A("adc %3,%0")
  763. A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_B) * LO(f)*/
  764. A("lds %1, bezier_B+2") /* %1 = HI(bezier_B)*/
  765. A("mul %1,%5") /* r1:r0 = MI(bezier_B) * LO(f)*/
  766. A("add %2,r0")
  767. A("adc %3,r1")
  768. A("adc %4,%0") /* %4:%3:%2:%9 += HI(bezier_B) * LO(f) << 8*/
  769. A("mul %10,%6") /* r1:r0 = LO(bezier_B) * MI(f)*/
  770. A("add %9,r0")
  771. A("adc %2,r1")
  772. A("adc %3,%0")
  773. A("adc %4,%0") /* %4:%3:%2:%9 += LO(bezier_B) * MI(f)*/
  774. A("mul %11,%6") /* r1:r0 = MI(bezier_B) * MI(f)*/
  775. A("add %2,r0")
  776. A("adc %3,r1")
  777. A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_B) * MI(f) << 8*/
  778. A("mul %1,%6") /* r1:r0 = HI(bezier_B) * LO(f)*/
  779. A("add %3,r0")
  780. A("adc %4,r1") /* %4:%3:%2:%9 += HI(bezier_B) * LO(f) << 16*/
  781. /* umul16x16to16hi(f, f, t); / Range 16 bits : f = t^5 (unsigned) [17]*/
  782. A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/
  783. A("mov %1,r1") /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/
  784. A("clr %10") /* %10 = 0*/
  785. A("clr %11") /* %11 = 0*/
  786. A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/
  787. A("add %1,r0") /* %1 += LO(LO(f) * HI(t))*/
  788. A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/
  789. A("adc %11,%0") /* %11 += carry*/
  790. A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/
  791. A("add %1,r0") /* %1 += LO(HI(f) * LO(t))*/
  792. A("adc %10,r1") /* %10 += HI(HI(f) * LO(t))*/
  793. A("adc %11,%0") /* %11 += carry*/
  794. A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/
  795. A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/
  796. A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/
  797. A("mov %5,%10") /* %6:%5 =*/
  798. A("mov %6,%11") /* f = %10:%11*/
  799. /* umul16x24to24hi(v, f, bezier_A); / Range 21bits [29]*/
  800. /* acc -= v; */
  801. A("lds %10, bezier_A") /* %10 = LO(bezier_A)*/
  802. A("mul %10,%5") /* r1:r0 = LO(bezier_A) * LO(f)*/
  803. A("sub %9,r1")
  804. A("sbc %2,%0")
  805. A("sbc %3,%0")
  806. A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(LO(bezier_A) * LO(f))*/
  807. A("lds %11, bezier_A+1") /* %11 = MI(bezier_A)*/
  808. A("mul %11,%5") /* r1:r0 = MI(bezier_A) * LO(f)*/
  809. A("sub %9,r0")
  810. A("sbc %2,r1")
  811. A("sbc %3,%0")
  812. A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_A) * LO(f)*/
  813. A("lds %1, bezier_A+2") /* %1 = HI(bezier_A)*/
  814. A("mul %1,%5") /* r1:r0 = MI(bezier_A) * LO(f)*/
  815. A("sub %2,r0")
  816. A("sbc %3,r1")
  817. A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(bezier_A) * LO(f) << 8*/
  818. A("mul %10,%6") /* r1:r0 = LO(bezier_A) * MI(f)*/
  819. A("sub %9,r0")
  820. A("sbc %2,r1")
  821. A("sbc %3,%0")
  822. A("sbc %4,%0") /* %4:%3:%2:%9 -= LO(bezier_A) * MI(f)*/
  823. A("mul %11,%6") /* r1:r0 = MI(bezier_A) * MI(f)*/
  824. A("sub %2,r0")
  825. A("sbc %3,r1")
  826. A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_A) * MI(f) << 8*/
  827. A("mul %1,%6") /* r1:r0 = HI(bezier_A) * LO(f)*/
  828. A("sub %3,r0")
  829. A("sbc %4,r1") /* %4:%3:%2:%9 -= HI(bezier_A) * LO(f) << 16*/
  830. A("jmp 2f") /* Done!*/
  831. L("1")
  832. /* uint24_t v; */
  833. /* umul16x24to24hi(v, f, bezier_C); / Range 21bits [29]*/
  834. /* acc += v; */
  835. A("lds %10, bezier_C") /* %10 = LO(bezier_C)*/
  836. A("mul %10,%5") /* r1:r0 = LO(bezier_C) * LO(f)*/
  837. A("add %9,r1")
  838. A("adc %2,%0")
  839. A("adc %3,%0")
  840. A("adc %4,%0") /* %4:%3:%2:%9 += HI(LO(bezier_C) * LO(f))*/
  841. A("lds %11, bezier_C+1") /* %11 = MI(bezier_C)*/
  842. A("mul %11,%5") /* r1:r0 = MI(bezier_C) * LO(f)*/
  843. A("add %9,r0")
  844. A("adc %2,r1")
  845. A("adc %3,%0")
  846. A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_C) * LO(f)*/
  847. A("lds %1, bezier_C+2") /* %1 = HI(bezier_C)*/
  848. A("mul %1,%5") /* r1:r0 = MI(bezier_C) * LO(f)*/
  849. A("add %2,r0")
  850. A("adc %3,r1")
  851. A("adc %4,%0") /* %4:%3:%2:%9 += HI(bezier_C) * LO(f) << 8*/
  852. A("mul %10,%6") /* r1:r0 = LO(bezier_C) * MI(f)*/
  853. A("add %9,r0")
  854. A("adc %2,r1")
  855. A("adc %3,%0")
  856. A("adc %4,%0") /* %4:%3:%2:%9 += LO(bezier_C) * MI(f)*/
  857. A("mul %11,%6") /* r1:r0 = MI(bezier_C) * MI(f)*/
  858. A("add %2,r0")
  859. A("adc %3,r1")
  860. A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_C) * MI(f) << 8*/
  861. A("mul %1,%6") /* r1:r0 = HI(bezier_C) * LO(f)*/
  862. A("add %3,r0")
  863. A("adc %4,r1") /* %4:%3:%2:%9 += HI(bezier_C) * LO(f) << 16*/
  864. /* umul16x16to16hi(f, f, t); / Range 16 bits : f = t^3 (unsigned) [17]*/
  865. A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/
  866. A("mov %1,r1") /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/
  867. A("clr %10") /* %10 = 0*/
  868. A("clr %11") /* %11 = 0*/
  869. A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/
  870. A("add %1,r0") /* %1 += LO(LO(f) * HI(t))*/
  871. A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/
  872. A("adc %11,%0") /* %11 += carry*/
  873. A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/
  874. A("add %1,r0") /* %1 += LO(HI(f) * LO(t))*/
  875. A("adc %10,r1") /* %10 += HI(HI(f) * LO(t))*/
  876. A("adc %11,%0") /* %11 += carry*/
  877. A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/
  878. A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/
  879. A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/
  880. A("mov %5,%10") /* %6:%5 =*/
  881. A("mov %6,%11") /* f = %10:%11*/
  882. /* umul16x24to24hi(v, f, bezier_B); / Range 22bits [29]*/
  883. /* acc -= v;*/
  884. A("lds %10, bezier_B") /* %10 = LO(bezier_B)*/
  885. A("mul %10,%5") /* r1:r0 = LO(bezier_B) * LO(f)*/
  886. A("sub %9,r1")
  887. A("sbc %2,%0")
  888. A("sbc %3,%0")
  889. A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(LO(bezier_B) * LO(f))*/
  890. A("lds %11, bezier_B+1") /* %11 = MI(bezier_B)*/
  891. A("mul %11,%5") /* r1:r0 = MI(bezier_B) * LO(f)*/
  892. A("sub %9,r0")
  893. A("sbc %2,r1")
  894. A("sbc %3,%0")
  895. A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_B) * LO(f)*/
  896. A("lds %1, bezier_B+2") /* %1 = HI(bezier_B)*/
  897. A("mul %1,%5") /* r1:r0 = MI(bezier_B) * LO(f)*/
  898. A("sub %2,r0")
  899. A("sbc %3,r1")
  900. A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(bezier_B) * LO(f) << 8*/
  901. A("mul %10,%6") /* r1:r0 = LO(bezier_B) * MI(f)*/
  902. A("sub %9,r0")
  903. A("sbc %2,r1")
  904. A("sbc %3,%0")
  905. A("sbc %4,%0") /* %4:%3:%2:%9 -= LO(bezier_B) * MI(f)*/
  906. A("mul %11,%6") /* r1:r0 = MI(bezier_B) * MI(f)*/
  907. A("sub %2,r0")
  908. A("sbc %3,r1")
  909. A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_B) * MI(f) << 8*/
  910. A("mul %1,%6") /* r1:r0 = HI(bezier_B) * LO(f)*/
  911. A("sub %3,r0")
  912. A("sbc %4,r1") /* %4:%3:%2:%9 -= HI(bezier_B) * LO(f) << 16*/
  913. /* umul16x16to16hi(f, f, t); / Range 16 bits : f = t^5 (unsigned) [17]*/
  914. A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/
  915. A("mov %1,r1") /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/
  916. A("clr %10") /* %10 = 0*/
  917. A("clr %11") /* %11 = 0*/
  918. A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/
  919. A("add %1,r0") /* %1 += LO(LO(f) * HI(t))*/
  920. A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/
  921. A("adc %11,%0") /* %11 += carry*/
  922. A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/
  923. A("add %1,r0") /* %1 += LO(HI(f) * LO(t))*/
  924. A("adc %10,r1") /* %10 += HI(HI(f) * LO(t))*/
  925. A("adc %11,%0") /* %11 += carry*/
  926. A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/
  927. A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/
  928. A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/
  929. A("mov %5,%10") /* %6:%5 =*/
  930. A("mov %6,%11") /* f = %10:%11*/
  931. /* umul16x24to24hi(v, f, bezier_A); / Range 21bits [29]*/
  932. /* acc += v; */
  933. A("lds %10, bezier_A") /* %10 = LO(bezier_A)*/
  934. A("mul %10,%5") /* r1:r0 = LO(bezier_A) * LO(f)*/
  935. A("add %9,r1")
  936. A("adc %2,%0")
  937. A("adc %3,%0")
  938. A("adc %4,%0") /* %4:%3:%2:%9 += HI(LO(bezier_A) * LO(f))*/
  939. A("lds %11, bezier_A+1") /* %11 = MI(bezier_A)*/
  940. A("mul %11,%5") /* r1:r0 = MI(bezier_A) * LO(f)*/
  941. A("add %9,r0")
  942. A("adc %2,r1")
  943. A("adc %3,%0")
  944. A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_A) * LO(f)*/
  945. A("lds %1, bezier_A+2") /* %1 = HI(bezier_A)*/
  946. A("mul %1,%5") /* r1:r0 = MI(bezier_A) * LO(f)*/
  947. A("add %2,r0")
  948. A("adc %3,r1")
  949. A("adc %4,%0") /* %4:%3:%2:%9 += HI(bezier_A) * LO(f) << 8*/
  950. A("mul %10,%6") /* r1:r0 = LO(bezier_A) * MI(f)*/
  951. A("add %9,r0")
  952. A("adc %2,r1")
  953. A("adc %3,%0")
  954. A("adc %4,%0") /* %4:%3:%2:%9 += LO(bezier_A) * MI(f)*/
  955. A("mul %11,%6") /* r1:r0 = MI(bezier_A) * MI(f)*/
  956. A("add %2,r0")
  957. A("adc %3,r1")
  958. A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_A) * MI(f) << 8*/
  959. A("mul %1,%6") /* r1:r0 = HI(bezier_A) * LO(f)*/
  960. A("add %3,r0")
  961. A("adc %4,r1") /* %4:%3:%2:%9 += HI(bezier_A) * LO(f) << 16*/
  962. L("2")
  963. " clr __zero_reg__" /* C runtime expects r1 = __zero_reg__ = 0 */
  964. : "+r"(r0),
  965. "+r"(r1),
  966. "+r"(r2),
  967. "+r"(r3),
  968. "+r"(r4),
  969. "+r"(r5),
  970. "+r"(r6),
  971. "+r"(r7),
  972. "+r"(r8),
  973. "+r"(r9),
  974. "+r"(r10),
  975. "+r"(r11)
  976. :
  977. :"cc","r0","r1"
  978. );
  979. return (r2 | (uint16_t(r3) << 8)) | (uint32_t(r4) << 16);
  980. }
  981. #else
  982. // For all the other 32bit CPUs
  983. FORCE_INLINE void Stepper::_calc_bezier_curve_coeffs(const int32_t v0, const int32_t v1, const uint32_t av) {
  984. // Calculate the Bézier coefficients
  985. bezier_A = 768 * (v1 - v0);
  986. bezier_B = 1920 * (v0 - v1);
  987. bezier_C = 1280 * (v1 - v0);
  988. bezier_F = 128 * v0;
  989. bezier_AV = av;
  990. }
  991. FORCE_INLINE int32_t Stepper::_eval_bezier_curve(const uint32_t curr_step) {
  992. #if defined(__ARM__) || defined(__thumb__)
  993. // For ARM Cortex M3/M4 CPUs, we have the optimized assembler version, that takes 43 cycles to execute
  994. register uint32_t flo = 0;
  995. register uint32_t fhi = bezier_AV * curr_step;
  996. register uint32_t t = fhi;
  997. register int32_t alo = bezier_F;
  998. register int32_t ahi = 0;
  999. register int32_t A = bezier_A;
  1000. register int32_t B = bezier_B;
  1001. register int32_t C = bezier_C;
  1002. __asm__ __volatile__(
  1003. ".syntax unified" "\n\t" // is to prevent CM0,CM1 non-unified syntax
  1004. A("lsrs %[ahi],%[alo],#1") // a = F << 31 1 cycles
  1005. A("lsls %[alo],%[alo],#31") // 1 cycles
  1006. A("umull %[flo],%[fhi],%[fhi],%[t]") // f *= t 5 cycles [fhi:flo=64bits]
  1007. A("umull %[flo],%[fhi],%[fhi],%[t]") // f>>=32; f*=t 5 cycles [fhi:flo=64bits]
  1008. A("lsrs %[flo],%[fhi],#1") // 1 cycles [31bits]
  1009. A("smlal %[alo],%[ahi],%[flo],%[C]") // a+=(f>>33)*C; 5 cycles
  1010. A("umull %[flo],%[fhi],%[fhi],%[t]") // f>>=32; f*=t 5 cycles [fhi:flo=64bits]
  1011. A("lsrs %[flo],%[fhi],#1") // 1 cycles [31bits]
  1012. A("smlal %[alo],%[ahi],%[flo],%[B]") // a+=(f>>33)*B; 5 cycles
  1013. A("umull %[flo],%[fhi],%[fhi],%[t]") // f>>=32; f*=t 5 cycles [fhi:flo=64bits]
  1014. A("lsrs %[flo],%[fhi],#1") // f>>=33; 1 cycles [31bits]
  1015. A("smlal %[alo],%[ahi],%[flo],%[A]") // a+=(f>>33)*A; 5 cycles
  1016. A("lsrs %[alo],%[ahi],#6") // a>>=38 1 cycles
  1017. : [alo]"+r"( alo ) ,
  1018. [flo]"+r"( flo ) ,
  1019. [fhi]"+r"( fhi ) ,
  1020. [ahi]"+r"( ahi ) ,
  1021. [A]"+r"( A ) , // <== Note: Even if A, B, C, and t registers are INPUT ONLY
  1022. [B]"+r"( B ) , // GCC does bad optimizations on the code if we list them as
  1023. [C]"+r"( C ) , // such, breaking this function. So, to avoid that problem,
  1024. [t]"+r"( t ) // we list all registers as input-outputs.
  1025. :
  1026. : "cc"
  1027. );
  1028. return alo;
  1029. #else
  1030. // For non ARM targets, we provide a fallback implementation. Really doubt it
  1031. // will be useful, unless the processor is fast and 32bit
  1032. uint32_t t = bezier_AV * curr_step; // t: Range 0 - 1^32 = 32 bits
  1033. uint64_t f = t;
  1034. f *= t; // Range 32*2 = 64 bits (unsigned)
  1035. f >>= 32; // Range 32 bits (unsigned)
  1036. f *= t; // Range 32*2 = 64 bits (unsigned)
  1037. f >>= 32; // Range 32 bits : f = t^3 (unsigned)
  1038. int64_t acc = (int64_t) bezier_F << 31; // Range 63 bits (signed)
  1039. acc += ((uint32_t) f >> 1) * (int64_t) bezier_C; // Range 29bits + 31 = 60bits (plus sign)
  1040. f *= t; // Range 32*2 = 64 bits
  1041. f >>= 32; // Range 32 bits : f = t^3 (unsigned)
  1042. acc += ((uint32_t) f >> 1) * (int64_t) bezier_B; // Range 29bits + 31 = 60bits (plus sign)
  1043. f *= t; // Range 32*2 = 64 bits
  1044. f >>= 32; // Range 32 bits : f = t^3 (unsigned)
  1045. acc += ((uint32_t) f >> 1) * (int64_t) bezier_A; // Range 28bits + 31 = 59bits (plus sign)
  1046. acc >>= (31 + 7); // Range 24bits (plus sign)
  1047. return (int32_t) acc;
  1048. #endif
  1049. }
  1050. #endif
  1051. #endif // S_CURVE_ACCELERATION
  1052. /**
  1053. * Stepper Driver Interrupt
  1054. *
  1055. * Directly pulses the stepper motors at high frequency.
  1056. */
  1057. HAL_STEP_TIMER_ISR {
  1058. HAL_timer_isr_prologue(STEP_TIMER_NUM);
  1059. Stepper::isr();
  1060. HAL_timer_isr_epilogue(STEP_TIMER_NUM);
  1061. }
  1062. #ifdef CPU_32_BIT
  1063. #define STEP_MULTIPLY(A,B) MultiU32X24toH32(A, B)
  1064. #else
  1065. #define STEP_MULTIPLY(A,B) MultiU24X32toH16(A, B)
  1066. #endif
  1067. void Stepper::isr() {
  1068. #ifndef __AVR__
  1069. // Disable interrupts, to avoid ISR preemption while we reprogram the period
  1070. // (AVR enters the ISR with global interrupts disabled, so no need to do it here)
  1071. DISABLE_ISRS();
  1072. #endif
  1073. // Program timer compare for the maximum period, so it does NOT
  1074. // flag an interrupt while this ISR is running - So changes from small
  1075. // periods to big periods are respected and the timer does not reset to 0
  1076. HAL_timer_set_compare(STEP_TIMER_NUM, HAL_TIMER_TYPE_MAX);
  1077. // Count of ticks for the next ISR
  1078. hal_timer_t next_isr_ticks = 0;
  1079. // Limit the amount of iterations
  1080. uint8_t max_loops = 10;
  1081. // We need this variable here to be able to use it in the following loop
  1082. hal_timer_t min_ticks;
  1083. do {
  1084. // Enable ISRs to reduce USART processing latency
  1085. ENABLE_ISRS();
  1086. // Run main stepping pulse phase ISR if we have to
  1087. if (!nextMainISR) Stepper::stepper_pulse_phase_isr();
  1088. #if ENABLED(LIN_ADVANCE)
  1089. // Run linear advance stepper ISR if we have to
  1090. if (!nextAdvanceISR) nextAdvanceISR = Stepper::advance_isr();
  1091. #endif
  1092. // ^== Time critical. NOTHING besides pulse generation should be above here!!!
  1093. // Run main stepping block processing ISR if we have to
  1094. if (!nextMainISR) nextMainISR = Stepper::stepper_block_phase_isr();
  1095. uint32_t interval =
  1096. #if ENABLED(LIN_ADVANCE)
  1097. MIN(nextAdvanceISR, nextMainISR) // Nearest time interval
  1098. #else
  1099. nextMainISR // Remaining stepper ISR time
  1100. #endif
  1101. ;
  1102. // Limit the value to the maximum possible value of the timer
  1103. NOMORE(interval, HAL_TIMER_TYPE_MAX);
  1104. // Compute the time remaining for the main isr
  1105. nextMainISR -= interval;
  1106. #if ENABLED(LIN_ADVANCE)
  1107. // Compute the time remaining for the advance isr
  1108. if (nextAdvanceISR != LA_ADV_NEVER) nextAdvanceISR -= interval;
  1109. #endif
  1110. /**
  1111. * This needs to avoid a race-condition caused by interleaving
  1112. * of interrupts required by both the LA and Stepper algorithms.
  1113. *
  1114. * Assume the following tick times for stepper pulses:
  1115. * Stepper ISR (S): 1 1000 2000 3000 4000
  1116. * Linear Adv. (E): 10 1010 2010 3010 4010
  1117. *
  1118. * The current algorithm tries to interleave them, giving:
  1119. * 1:S 10:E 1000:S 1010:E 2000:S 2010:E 3000:S 3010:E 4000:S 4010:E
  1120. *
  1121. * Ideal timing would yield these delta periods:
  1122. * 1:S 9:E 990:S 10:E 990:S 10:E 990:S 10:E 990:S 10:E
  1123. *
  1124. * But, since each event must fire an ISR with a minimum duration, the
  1125. * minimum delta might be 900, so deltas under 900 get rounded up:
  1126. * 900:S d900:E d990:S d900:E d990:S d900:E d990:S d900:E d990:S d900:E
  1127. *
  1128. * It works, but divides the speed of all motors by half, leading to a sudden
  1129. * reduction to 1/2 speed! Such jumps in speed lead to lost steps (not even
  1130. * accounting for double/quad stepping, which makes it even worse).
  1131. */
  1132. // Compute the tick count for the next ISR
  1133. next_isr_ticks += interval;
  1134. /**
  1135. * The following section must be done with global interrupts disabled.
  1136. * We want nothing to interrupt it, as that could mess the calculations
  1137. * we do for the next value to program in the period register of the
  1138. * stepper timer and lead to skipped ISRs (if the value we happen to program
  1139. * is less than the current count due to something preempting between the
  1140. * read and the write of the new period value).
  1141. */
  1142. DISABLE_ISRS();
  1143. /**
  1144. * Get the current tick value + margin
  1145. * Assuming at least 6µs between calls to this ISR...
  1146. * On AVR the ISR epilogue+prologue is estimated at 100 instructions - Give 8µs as margin
  1147. * On ARM the ISR epilogue+prologue is estimated at 20 instructions - Give 1µs as margin
  1148. */
  1149. min_ticks = HAL_timer_get_count(STEP_TIMER_NUM) + hal_timer_t(
  1150. #ifdef __AVR__
  1151. 8
  1152. #else
  1153. 1
  1154. #endif
  1155. * (HAL_TICKS_PER_US)
  1156. );
  1157. /**
  1158. * NB: If for some reason the stepper monopolizes the MPU, eventually the
  1159. * timer will wrap around (and so will 'next_isr_ticks'). So, limit the
  1160. * loop to 10 iterations. Beyond that, there's no way to ensure correct pulse
  1161. * timing, since the MCU isn't fast enough.
  1162. */
  1163. if (!--max_loops) next_isr_ticks = min_ticks;
  1164. // Advance pulses if not enough time to wait for the next ISR
  1165. } while (next_isr_ticks < min_ticks);
  1166. // Now 'next_isr_ticks' contains the period to the next Stepper ISR - And we are
  1167. // sure that the time has not arrived yet - Warrantied by the scheduler
  1168. // Set the next ISR to fire at the proper time
  1169. HAL_timer_set_compare(STEP_TIMER_NUM, hal_timer_t(next_isr_ticks));
  1170. // Don't forget to finally reenable interrupts
  1171. ENABLE_ISRS();
  1172. }
  1173. /**
  1174. * This phase of the ISR should ONLY create the pulses for the steppers.
  1175. * This prevents jitter caused by the interval between the start of the
  1176. * interrupt and the start of the pulses. DON'T add any logic ahead of the
  1177. * call to this method that might cause variation in the timing. The aim
  1178. * is to keep pulse timing as regular as possible.
  1179. */
  1180. void Stepper::stepper_pulse_phase_isr() {
  1181. // If we must abort the current block, do so!
  1182. if (abort_current_block) {
  1183. abort_current_block = false;
  1184. if (current_block) {
  1185. axis_did_move = 0;
  1186. current_block = NULL;
  1187. planner.discard_current_block();
  1188. }
  1189. }
  1190. // If there is no current block, do nothing
  1191. if (!current_block) return;
  1192. // Take multiple steps per interrupt (For high speed moves)
  1193. for (uint8_t i = steps_per_isr; i--;) {
  1194. #define _APPLY_STEP(AXIS) AXIS ##_APPLY_STEP
  1195. #define _INVERT_STEP_PIN(AXIS) INVERT_## AXIS ##_STEP_PIN
  1196. // Start an active pulse, if Bresenham says so, and update position
  1197. #define PULSE_START(AXIS) do{ \
  1198. delta_error[_AXIS(AXIS)] += advance_dividend[_AXIS(AXIS)]; \
  1199. if (delta_error[_AXIS(AXIS)] >= 0) { \
  1200. _APPLY_STEP(AXIS)(!_INVERT_STEP_PIN(AXIS), 0); \
  1201. count_position[_AXIS(AXIS)] += count_direction[_AXIS(AXIS)]; \
  1202. } \
  1203. }while(0)
  1204. // Stop an active pulse, if any, and adjust error term
  1205. #define PULSE_STOP(AXIS) do { \
  1206. if (delta_error[_AXIS(AXIS)] >= 0) { \
  1207. delta_error[_AXIS(AXIS)] -= advance_divisor; \
  1208. _APPLY_STEP(AXIS)(_INVERT_STEP_PIN(AXIS), 0); \
  1209. } \
  1210. }while(0)
  1211. #if MINIMUM_STEPPER_PULSE > 0
  1212. // Get the timer count and estimate the end of the pulse
  1213. hal_timer_t pulse_end = HAL_timer_get_count(PULSE_TIMER_NUM) + hal_timer_t((HAL_TICKS_PER_US) * (MINIMUM_STEPPER_PULSE));
  1214. #endif
  1215. // Pulse start
  1216. #if HAS_X_STEP
  1217. PULSE_START(X);
  1218. #endif
  1219. #if HAS_Y_STEP
  1220. PULSE_START(Y);
  1221. #endif
  1222. #if HAS_Z_STEP
  1223. PULSE_START(Z);
  1224. #endif
  1225. // Pulse E/Mixing extruders
  1226. #if ENABLED(LIN_ADVANCE)
  1227. // Tick the E axis, correct error term and update position
  1228. delta_error[E_AXIS] += advance_dividend[E_AXIS];
  1229. if (delta_error[E_AXIS] >= 0) {
  1230. count_position[E_AXIS] += count_direction[E_AXIS];
  1231. delta_error[E_AXIS] -= advance_divisor;
  1232. // Don't step E here - But remember the number of steps to perform
  1233. motor_direction(E_AXIS) ? --LA_steps : ++LA_steps;
  1234. }
  1235. #else // !LIN_ADVANCE - use linear interpolation for E also
  1236. #if ENABLED(MIXING_EXTRUDER)
  1237. // Tick the E axis
  1238. delta_error[E_AXIS] += advance_dividend[E_AXIS];
  1239. if (delta_error[E_AXIS] >= 0) {
  1240. count_position[E_AXIS] += count_direction[E_AXIS];
  1241. delta_error[E_AXIS] -= advance_divisor;
  1242. }
  1243. // Tick the counters used for this mix in proper proportion
  1244. MIXING_STEPPERS_LOOP(j) {
  1245. // Step mixing steppers (proportionally)
  1246. delta_error_m[j] += advance_dividend_m[j];
  1247. // Step when the counter goes over zero
  1248. if (delta_error_m[j] >= 0) E_STEP_WRITE(j, !INVERT_E_STEP_PIN);
  1249. }
  1250. #else // !MIXING_EXTRUDER
  1251. PULSE_START(E);
  1252. #endif
  1253. #endif // !LIN_ADVANCE
  1254. #if MINIMUM_STEPPER_PULSE > 0
  1255. // Just wait for the requested pulse time.
  1256. while (HAL_timer_get_count(PULSE_TIMER_NUM) < pulse_end) { /* nada */ }
  1257. // Get the timer count and estimate the end of the pulse for the OFF phase
  1258. pulse_end = HAL_timer_get_count(PULSE_TIMER_NUM) + hal_timer_t((HAL_TICKS_PER_US) * (MINIMUM_STEPPER_PULSE));
  1259. #endif
  1260. // Pulse stop
  1261. #if HAS_X_STEP
  1262. PULSE_STOP(X);
  1263. #endif
  1264. #if HAS_Y_STEP
  1265. PULSE_STOP(Y);
  1266. #endif
  1267. #if HAS_Z_STEP
  1268. PULSE_STOP(Z);
  1269. #endif
  1270. #if DISABLED(LIN_ADVANCE)
  1271. #if ENABLED(MIXING_EXTRUDER)
  1272. MIXING_STEPPERS_LOOP(j) {
  1273. if (delta_error_m[j] >= 0) {
  1274. delta_error_m[j] -= advance_divisor_m;
  1275. E_STEP_WRITE(j, INVERT_E_STEP_PIN);
  1276. }
  1277. }
  1278. #else // !MIXING_EXTRUDER
  1279. PULSE_STOP(E);
  1280. #endif
  1281. #endif // !LIN_ADVANCE
  1282. // If all events done, break loop now
  1283. if (++step_events_completed >= step_event_count) break;
  1284. #if MINIMUM_STEPPER_PULSE
  1285. // For minimum pulse time wait after stopping pulses also
  1286. // Just wait for the requested pulse time.
  1287. if (i) while (HAL_timer_get_count(PULSE_TIMER_NUM) < pulse_end) { /* nada */ }
  1288. #endif
  1289. } // steps_loop
  1290. }
  1291. // This is the last half of the stepper interrupt: This one processes and
  1292. // properly schedules blocks from the planner. This is executed after creating
  1293. // the step pulses, so it is not time critical, as pulses are already done.
  1294. uint32_t Stepper::stepper_block_phase_isr() {
  1295. // If no queued movements, just wait 1ms for the next move
  1296. uint32_t interval = (HAL_STEPPER_TIMER_RATE / 1000);
  1297. // If there is a current block
  1298. if (current_block) {
  1299. // If current block is finished, reset pointer
  1300. if (step_events_completed >= step_event_count) {
  1301. axis_did_move = 0;
  1302. current_block = NULL;
  1303. planner.discard_current_block();
  1304. }
  1305. else {
  1306. // Step events not completed yet...
  1307. // Are we in acceleration phase ?
  1308. if (step_events_completed <= accelerate_until) { // Calculate new timer value
  1309. #if ENABLED(S_CURVE_ACCELERATION)
  1310. // Get the next speed to use (Jerk limited!)
  1311. uint32_t acc_step_rate =
  1312. acceleration_time < current_block->acceleration_time
  1313. ? _eval_bezier_curve(acceleration_time)
  1314. : current_block->cruise_rate;
  1315. #else
  1316. acc_step_rate = STEP_MULTIPLY(acceleration_time, current_block->acceleration_rate) + current_block->initial_rate;
  1317. NOMORE(acc_step_rate, current_block->nominal_rate);
  1318. #endif
  1319. // acc_step_rate is in steps/second
  1320. // step_rate to timer interval and steps per stepper isr
  1321. interval = calc_timer_interval(acc_step_rate, oversampling_factor, &steps_per_isr);
  1322. acceleration_time += interval;
  1323. #if ENABLED(LIN_ADVANCE)
  1324. if (LA_use_advance_lead) {
  1325. // Wake up eISR on first acceleration loop and fire ISR if final adv_rate is reached
  1326. if (step_events_completed == steps_per_isr || (LA_steps && LA_isr_rate != current_block->advance_speed)) {
  1327. nextAdvanceISR = 0;
  1328. LA_isr_rate = current_block->advance_speed;
  1329. }
  1330. }
  1331. else {
  1332. LA_isr_rate = LA_ADV_NEVER;
  1333. if (LA_steps) nextAdvanceISR = 0;
  1334. }
  1335. #endif // LIN_ADVANCE
  1336. }
  1337. // Are we in Deceleration phase ?
  1338. else if (step_events_completed > decelerate_after) {
  1339. uint32_t step_rate;
  1340. #if ENABLED(S_CURVE_ACCELERATION)
  1341. // If this is the 1st time we process the 2nd half of the trapezoid...
  1342. if (!bezier_2nd_half) {
  1343. // Initialize the Bézier speed curve
  1344. _calc_bezier_curve_coeffs(current_block->cruise_rate, current_block->final_rate, current_block->deceleration_time_inverse);
  1345. bezier_2nd_half = true;
  1346. // The first point starts at cruise rate. Just save evaluation of the Bézier curve
  1347. step_rate = current_block->cruise_rate;
  1348. }
  1349. else {
  1350. // Calculate the next speed to use
  1351. step_rate = deceleration_time < current_block->deceleration_time
  1352. ? _eval_bezier_curve(deceleration_time)
  1353. : current_block->final_rate;
  1354. }
  1355. #else
  1356. // Using the old trapezoidal control
  1357. step_rate = STEP_MULTIPLY(deceleration_time, current_block->acceleration_rate);
  1358. if (step_rate < acc_step_rate) { // Still decelerating?
  1359. step_rate = acc_step_rate - step_rate;
  1360. NOLESS(step_rate, current_block->final_rate);
  1361. }
  1362. else
  1363. step_rate = current_block->final_rate;
  1364. #endif
  1365. // step_rate is in steps/second
  1366. // step_rate to timer interval and steps per stepper isr
  1367. interval = calc_timer_interval(step_rate, oversampling_factor, &steps_per_isr);
  1368. deceleration_time += interval;
  1369. #if ENABLED(LIN_ADVANCE)
  1370. if (LA_use_advance_lead) {
  1371. if (step_events_completed <= decelerate_after + steps_per_isr ||
  1372. (LA_steps && LA_isr_rate != current_block->advance_speed)
  1373. ) {
  1374. nextAdvanceISR = 0; // Wake up eISR on first deceleration loop
  1375. LA_isr_rate = current_block->advance_speed;
  1376. }
  1377. }
  1378. else {
  1379. LA_isr_rate = LA_ADV_NEVER;
  1380. if (LA_steps) nextAdvanceISR = 0;
  1381. }
  1382. #endif // LIN_ADVANCE
  1383. }
  1384. // We must be in cruise phase otherwise
  1385. else {
  1386. #if ENABLED(LIN_ADVANCE)
  1387. // If there are any esteps, fire the next advance_isr "now"
  1388. if (LA_steps && LA_isr_rate != current_block->advance_speed) nextAdvanceISR = 0;
  1389. #endif
  1390. // Calculate the ticks_nominal for this nominal speed, if not done yet
  1391. if (ticks_nominal < 0) {
  1392. // step_rate to timer interval and loops for the nominal speed
  1393. ticks_nominal = calc_timer_interval(current_block->nominal_rate, oversampling_factor, &steps_per_isr);
  1394. }
  1395. // The timer interval is just the nominal value for the nominal speed
  1396. interval = ticks_nominal;
  1397. }
  1398. }
  1399. }
  1400. // If there is no current block at this point, attempt to pop one from the buffer
  1401. // and prepare its movement
  1402. if (!current_block) {
  1403. // Anything in the buffer?
  1404. if ((current_block = planner.get_current_block())) {
  1405. // Sync block? Sync the stepper counts and return
  1406. while (TEST(current_block->flag, BLOCK_BIT_SYNC_POSITION)) {
  1407. _set_position(
  1408. current_block->position[A_AXIS], current_block->position[B_AXIS],
  1409. current_block->position[C_AXIS], current_block->position[E_AXIS]
  1410. );
  1411. planner.discard_current_block();
  1412. // Try to get a new block
  1413. if (!(current_block = planner.get_current_block()))
  1414. return interval; // No more queued movements!
  1415. }
  1416. // Flag all moving axes for proper endstop handling
  1417. #if IS_CORE
  1418. // Define conditions for checking endstops
  1419. #define S_(N) current_block->steps[CORE_AXIS_##N]
  1420. #define D_(N) TEST(current_block->direction_bits, CORE_AXIS_##N)
  1421. #endif
  1422. #if CORE_IS_XY || CORE_IS_XZ
  1423. /**
  1424. * Head direction in -X axis for CoreXY and CoreXZ bots.
  1425. *
  1426. * If steps differ, both axes are moving.
  1427. * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Y or Z, handled below)
  1428. * If DeltaA == DeltaB, the movement is only in the 1st axis (X)
  1429. */
  1430. #if ENABLED(COREXY) || ENABLED(COREXZ)
  1431. #define X_CMP ==
  1432. #else
  1433. #define X_CMP !=
  1434. #endif
  1435. #define X_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && D_(1) X_CMP D_(2)) )
  1436. #else
  1437. #define X_MOVE_TEST !!current_block->steps[A_AXIS]
  1438. #endif
  1439. #if CORE_IS_XY || CORE_IS_YZ
  1440. /**
  1441. * Head direction in -Y axis for CoreXY / CoreYZ bots.
  1442. *
  1443. * If steps differ, both axes are moving
  1444. * If DeltaA == DeltaB, the movement is only in the 1st axis (X or Y)
  1445. * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Y or Z)
  1446. */
  1447. #if ENABLED(COREYX) || ENABLED(COREYZ)
  1448. #define Y_CMP ==
  1449. #else
  1450. #define Y_CMP !=
  1451. #endif
  1452. #define Y_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && D_(1) Y_CMP D_(2)) )
  1453. #else
  1454. #define Y_MOVE_TEST !!current_block->steps[B_AXIS]
  1455. #endif
  1456. #if CORE_IS_XZ || CORE_IS_YZ
  1457. /**
  1458. * Head direction in -Z axis for CoreXZ or CoreYZ bots.
  1459. *
  1460. * If steps differ, both axes are moving
  1461. * If DeltaA == DeltaB, the movement is only in the 1st axis (X or Y, already handled above)
  1462. * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Z)
  1463. */
  1464. #if ENABLED(COREZX) || ENABLED(COREZY)
  1465. #define Z_CMP ==
  1466. #else
  1467. #define Z_CMP !=
  1468. #endif
  1469. #define Z_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && D_(1) Z_CMP D_(2)) )
  1470. #else
  1471. #define Z_MOVE_TEST !!current_block->steps[C_AXIS]
  1472. #endif
  1473. uint8_t axis_bits = 0;
  1474. if (X_MOVE_TEST) SBI(axis_bits, A_AXIS);
  1475. if (Y_MOVE_TEST) SBI(axis_bits, B_AXIS);
  1476. if (Z_MOVE_TEST) SBI(axis_bits, C_AXIS);
  1477. //if (!!current_block->steps[E_AXIS]) SBI(axis_bits, E_AXIS);
  1478. //if (!!current_block->steps[A_AXIS]) SBI(axis_bits, X_HEAD);
  1479. //if (!!current_block->steps[B_AXIS]) SBI(axis_bits, Y_HEAD);
  1480. //if (!!current_block->steps[C_AXIS]) SBI(axis_bits, Z_HEAD);
  1481. axis_did_move = axis_bits;
  1482. // No acceleration / deceleration time elapsed so far
  1483. acceleration_time = deceleration_time = 0;
  1484. uint8_t oversampling = 0; // Assume we won't use it
  1485. #if ENABLED(ADAPTIVE_STEP_SMOOTHING)
  1486. // At this point, we must decide if we can use Stepper movement axis smoothing.
  1487. uint32_t max_rate = current_block->nominal_rate; // Get the maximum rate (maximum event speed)
  1488. while (max_rate < MIN_STEP_ISR_FREQUENCY) {
  1489. max_rate <<= 1;
  1490. if (max_rate >= MAX_1X_STEP_ISR_FREQUENCY) break;
  1491. ++oversampling;
  1492. }
  1493. oversampling_factor = oversampling;
  1494. #endif
  1495. // Based on the oversampling factor, do the calculations
  1496. step_event_count = current_block->step_event_count << oversampling;
  1497. // Initialize Bresenham delta errors to 1/2
  1498. delta_error[X_AXIS] = delta_error[Y_AXIS] = delta_error[Z_AXIS] = delta_error[E_AXIS] = -int32_t(step_event_count);
  1499. // Calculate Bresenham dividends
  1500. advance_dividend[X_AXIS] = current_block->steps[X_AXIS] << 1;
  1501. advance_dividend[Y_AXIS] = current_block->steps[Y_AXIS] << 1;
  1502. advance_dividend[Z_AXIS] = current_block->steps[Z_AXIS] << 1;
  1503. advance_dividend[E_AXIS] = current_block->steps[E_AXIS] << 1;
  1504. // Calculate Bresenham divisor
  1505. advance_divisor = step_event_count << 1;
  1506. // No step events completed so far
  1507. step_events_completed = 0;
  1508. // Compute the acceleration and deceleration points
  1509. accelerate_until = current_block->accelerate_until << oversampling;
  1510. decelerate_after = current_block->decelerate_after << oversampling;
  1511. #if ENABLED(MIXING_EXTRUDER)
  1512. const uint32_t e_steps = (
  1513. #if ENABLED(LIN_ADVANCE)
  1514. current_block->steps[E_AXIS]
  1515. #else
  1516. step_event_count
  1517. #endif
  1518. );
  1519. MIXING_STEPPERS_LOOP(i) {
  1520. delta_error_m[i] = -int32_t(e_steps);
  1521. advance_dividend_m[i] = current_block->mix_steps[i] << 1;
  1522. }
  1523. advance_divisor_m = e_steps << 1;
  1524. #else
  1525. active_extruder = current_block->active_extruder;
  1526. #endif
  1527. // Initialize the trapezoid generator from the current block.
  1528. #if ENABLED(LIN_ADVANCE)
  1529. #if DISABLED(MIXING_EXTRUDER) && E_STEPPERS > 1
  1530. // If the now active extruder wasn't in use during the last move, its pressure is most likely gone.
  1531. if (active_extruder != last_moved_extruder) LA_current_adv_steps = 0;
  1532. #endif
  1533. if ((LA_use_advance_lead = current_block->use_advance_lead)) {
  1534. LA_final_adv_steps = current_block->final_adv_steps;
  1535. LA_max_adv_steps = current_block->max_adv_steps;
  1536. }
  1537. #endif
  1538. if (current_block->direction_bits != last_direction_bits
  1539. #if DISABLED(MIXING_EXTRUDER)
  1540. || active_extruder != last_moved_extruder
  1541. #endif
  1542. ) {
  1543. last_direction_bits = current_block->direction_bits;
  1544. #if DISABLED(MIXING_EXTRUDER)
  1545. last_moved_extruder = active_extruder;
  1546. #endif
  1547. set_directions();
  1548. }
  1549. // At this point, we must ensure the movement about to execute isn't
  1550. // trying to force the head against a limit switch. If using interrupt-
  1551. // driven change detection, and already against a limit then no call to
  1552. // the endstop_triggered method will be done and the movement will be
  1553. // done against the endstop. So, check the limits here: If the movement
  1554. // is against the limits, the block will be marked as to be killed, and
  1555. // on the next call to this ISR, will be discarded.
  1556. endstops.check_possible_change();
  1557. #if ENABLED(Z_LATE_ENABLE)
  1558. // If delayed Z enable, enable it now. This option will severely interfere with
  1559. // timing between pulses when chaining motion between blocks, and it could lead
  1560. // to lost steps in both X and Y axis, so avoid using it unless strictly necessary!!
  1561. if (current_block->steps[Z_AXIS]) enable_Z();
  1562. #endif
  1563. // Mark the time_nominal as not calculated yet
  1564. ticks_nominal = -1;
  1565. #if DISABLED(S_CURVE_ACCELERATION)
  1566. // Set as deceleration point the initial rate of the block
  1567. acc_step_rate = current_block->initial_rate;
  1568. #endif
  1569. #if ENABLED(S_CURVE_ACCELERATION)
  1570. // Initialize the Bézier speed curve
  1571. _calc_bezier_curve_coeffs(current_block->initial_rate, current_block->cruise_rate, current_block->acceleration_time_inverse);
  1572. // We haven't started the 2nd half of the trapezoid
  1573. bezier_2nd_half = false;
  1574. #endif
  1575. // Calculate the initial timer interval
  1576. interval = calc_timer_interval(current_block->initial_rate, oversampling_factor, &steps_per_isr);
  1577. }
  1578. }
  1579. // Return the interval to wait
  1580. return interval;
  1581. }
  1582. #if ENABLED(LIN_ADVANCE)
  1583. // Timer interrupt for E. LA_steps is set in the main routine
  1584. uint32_t Stepper::advance_isr() {
  1585. uint32_t interval;
  1586. if (LA_use_advance_lead) {
  1587. if (step_events_completed > decelerate_after && LA_current_adv_steps > LA_final_adv_steps) {
  1588. LA_steps--;
  1589. LA_current_adv_steps--;
  1590. interval = LA_isr_rate;
  1591. }
  1592. else if (step_events_completed < decelerate_after && LA_current_adv_steps < LA_max_adv_steps) {
  1593. //step_events_completed <= (uint32_t)accelerate_until) {
  1594. LA_steps++;
  1595. LA_current_adv_steps++;
  1596. interval = LA_isr_rate;
  1597. }
  1598. else
  1599. interval = LA_isr_rate = LA_ADV_NEVER;
  1600. }
  1601. else
  1602. interval = LA_ADV_NEVER;
  1603. #if ENABLED(MIXING_EXTRUDER)
  1604. if (LA_steps >= 0)
  1605. MIXING_STEPPERS_LOOP(j) NORM_E_DIR(j);
  1606. else
  1607. MIXING_STEPPERS_LOOP(j) REV_E_DIR(j);
  1608. #else
  1609. if (LA_steps >= 0)
  1610. NORM_E_DIR(active_extruder);
  1611. else
  1612. REV_E_DIR(active_extruder);
  1613. #endif
  1614. // Step E stepper if we have steps
  1615. while (LA_steps) {
  1616. #if MINIMUM_STEPPER_PULSE
  1617. hal_timer_t pulse_end = HAL_timer_get_count(PULSE_TIMER_NUM) + hal_timer_t((HAL_TICKS_PER_US) * (MINIMUM_STEPPER_PULSE));
  1618. #endif
  1619. #if ENABLED(MIXING_EXTRUDER)
  1620. MIXING_STEPPERS_LOOP(j) {
  1621. // Step mixing steppers (proportionally)
  1622. delta_error_m[j] += advance_dividend_m[j];
  1623. // Step when the counter goes over zero
  1624. if (delta_error_m[j] >= 0) E_STEP_WRITE(j, !INVERT_E_STEP_PIN);
  1625. }
  1626. #else
  1627. E_STEP_WRITE(active_extruder, !INVERT_E_STEP_PIN);
  1628. #endif
  1629. #if MINIMUM_STEPPER_PULSE
  1630. // Just wait for the requested pulse duration
  1631. while (HAL_timer_get_count(PULSE_TIMER_NUM) < pulse_end) { /* nada */ }
  1632. // Get the timer count and estimate the end of the pulse for the OFF phase
  1633. pulse_end = HAL_timer_get_count(PULSE_TIMER_NUM) + hal_timer_t((HAL_TICKS_PER_US) * (MINIMUM_STEPPER_PULSE));
  1634. #endif
  1635. LA_steps < 0 ? ++LA_steps : --LA_steps;
  1636. #if ENABLED(MIXING_EXTRUDER)
  1637. MIXING_STEPPERS_LOOP(j) {
  1638. if (delta_error_m[j] >= 0) {
  1639. delta_error_m[j] -= advance_divisor_m;
  1640. E_STEP_WRITE(j, INVERT_E_STEP_PIN);
  1641. }
  1642. }
  1643. #else
  1644. E_STEP_WRITE(active_extruder, INVERT_E_STEP_PIN);
  1645. #endif
  1646. #if MINIMUM_STEPPER_PULSE
  1647. // For minimum pulse time wait before looping
  1648. // Just wait for the requested pulse duration
  1649. if (LA_steps) while (HAL_timer_get_count(PULSE_TIMER_NUM) < pulse_end) { /* nada */ }
  1650. #endif
  1651. } // LA_steps
  1652. return interval;
  1653. }
  1654. #endif // LIN_ADVANCE
  1655. void Stepper::init() {
  1656. // Init Digipot Motor Current
  1657. #if HAS_DIGIPOTSS || HAS_MOTOR_CURRENT_PWM
  1658. digipot_init();
  1659. #endif
  1660. #if MB(ALLIGATOR)
  1661. const float motor_current[] = MOTOR_CURRENT;
  1662. unsigned int digipot_motor = 0;
  1663. for (uint8_t i = 0; i < 3 + EXTRUDERS; i++) {
  1664. digipot_motor = 255 * (motor_current[i] / 2.5);
  1665. dac084s085::setValue(i, digipot_motor);
  1666. }
  1667. #endif//MB(ALLIGATOR)
  1668. // Init Microstepping Pins
  1669. #if HAS_MICROSTEPS
  1670. microstep_init();
  1671. #endif
  1672. // Init Dir Pins
  1673. #if HAS_X_DIR
  1674. X_DIR_INIT;
  1675. #endif
  1676. #if HAS_X2_DIR
  1677. X2_DIR_INIT;
  1678. #endif
  1679. #if HAS_Y_DIR
  1680. Y_DIR_INIT;
  1681. #if ENABLED(Y_DUAL_STEPPER_DRIVERS) && HAS_Y2_DIR
  1682. Y2_DIR_INIT;
  1683. #endif
  1684. #endif
  1685. #if HAS_Z_DIR
  1686. Z_DIR_INIT;
  1687. #if ENABLED(Z_DUAL_STEPPER_DRIVERS) && HAS_Z2_DIR
  1688. Z2_DIR_INIT;
  1689. #endif
  1690. #endif
  1691. #if HAS_E0_DIR
  1692. E0_DIR_INIT;
  1693. #endif
  1694. #if HAS_E1_DIR
  1695. E1_DIR_INIT;
  1696. #endif
  1697. #if HAS_E2_DIR
  1698. E2_DIR_INIT;
  1699. #endif
  1700. #if HAS_E3_DIR
  1701. E3_DIR_INIT;
  1702. #endif
  1703. #if HAS_E4_DIR
  1704. E4_DIR_INIT;
  1705. #endif
  1706. // Init Enable Pins - steppers default to disabled.
  1707. #if HAS_X_ENABLE
  1708. X_ENABLE_INIT;
  1709. if (!X_ENABLE_ON) X_ENABLE_WRITE(HIGH);
  1710. #if (ENABLED(DUAL_X_CARRIAGE) || ENABLED(X_DUAL_STEPPER_DRIVERS)) && HAS_X2_ENABLE
  1711. X2_ENABLE_INIT;
  1712. if (!X_ENABLE_ON) X2_ENABLE_WRITE(HIGH);
  1713. #endif
  1714. #endif
  1715. #if HAS_Y_ENABLE
  1716. Y_ENABLE_INIT;
  1717. if (!Y_ENABLE_ON) Y_ENABLE_WRITE(HIGH);
  1718. #if ENABLED(Y_DUAL_STEPPER_DRIVERS) && HAS_Y2_ENABLE
  1719. Y2_ENABLE_INIT;
  1720. if (!Y_ENABLE_ON) Y2_ENABLE_WRITE(HIGH);
  1721. #endif
  1722. #endif
  1723. #if HAS_Z_ENABLE
  1724. Z_ENABLE_INIT;
  1725. if (!Z_ENABLE_ON) Z_ENABLE_WRITE(HIGH);
  1726. #if ENABLED(Z_DUAL_STEPPER_DRIVERS) && HAS_Z2_ENABLE
  1727. Z2_ENABLE_INIT;
  1728. if (!Z_ENABLE_ON) Z2_ENABLE_WRITE(HIGH);
  1729. #endif
  1730. #endif
  1731. #if HAS_E0_ENABLE
  1732. E0_ENABLE_INIT;
  1733. if (!E_ENABLE_ON) E0_ENABLE_WRITE(HIGH);
  1734. #endif
  1735. #if HAS_E1_ENABLE
  1736. E1_ENABLE_INIT;
  1737. if (!E_ENABLE_ON) E1_ENABLE_WRITE(HIGH);
  1738. #endif
  1739. #if HAS_E2_ENABLE
  1740. E2_ENABLE_INIT;
  1741. if (!E_ENABLE_ON) E2_ENABLE_WRITE(HIGH);
  1742. #endif
  1743. #if HAS_E3_ENABLE
  1744. E3_ENABLE_INIT;
  1745. if (!E_ENABLE_ON) E3_ENABLE_WRITE(HIGH);
  1746. #endif
  1747. #if HAS_E4_ENABLE
  1748. E4_ENABLE_INIT;
  1749. if (!E_ENABLE_ON) E4_ENABLE_WRITE(HIGH);
  1750. #endif
  1751. #define _STEP_INIT(AXIS) AXIS ##_STEP_INIT
  1752. #define _WRITE_STEP(AXIS, HIGHLOW) AXIS ##_STEP_WRITE(HIGHLOW)
  1753. #define _DISABLE(AXIS) disable_## AXIS()
  1754. #define AXIS_INIT(AXIS, PIN) \
  1755. _STEP_INIT(AXIS); \
  1756. _WRITE_STEP(AXIS, _INVERT_STEP_PIN(PIN)); \
  1757. _DISABLE(AXIS)
  1758. #define E_AXIS_INIT(NUM) AXIS_INIT(E## NUM, E)
  1759. // Init Step Pins
  1760. #if HAS_X_STEP
  1761. #if ENABLED(X_DUAL_STEPPER_DRIVERS) || ENABLED(DUAL_X_CARRIAGE)
  1762. X2_STEP_INIT;
  1763. X2_STEP_WRITE(INVERT_X_STEP_PIN);
  1764. #endif
  1765. AXIS_INIT(X, X);
  1766. #endif
  1767. #if HAS_Y_STEP
  1768. #if ENABLED(Y_DUAL_STEPPER_DRIVERS)
  1769. Y2_STEP_INIT;
  1770. Y2_STEP_WRITE(INVERT_Y_STEP_PIN);
  1771. #endif
  1772. AXIS_INIT(Y, Y);
  1773. #endif
  1774. #if HAS_Z_STEP
  1775. #if ENABLED(Z_DUAL_STEPPER_DRIVERS)
  1776. Z2_STEP_INIT;
  1777. Z2_STEP_WRITE(INVERT_Z_STEP_PIN);
  1778. #endif
  1779. AXIS_INIT(Z, Z);
  1780. #endif
  1781. #if E_STEPPERS > 0 && HAS_E0_STEP
  1782. E_AXIS_INIT(0);
  1783. #endif
  1784. #if E_STEPPERS > 1 && HAS_E1_STEP
  1785. E_AXIS_INIT(1);
  1786. #endif
  1787. #if E_STEPPERS > 2 && HAS_E2_STEP
  1788. E_AXIS_INIT(2);
  1789. #endif
  1790. #if E_STEPPERS > 3 && HAS_E3_STEP
  1791. E_AXIS_INIT(3);
  1792. #endif
  1793. #if E_STEPPERS > 4 && HAS_E4_STEP
  1794. E_AXIS_INIT(4);
  1795. #endif
  1796. // Init Stepper ISR to 122 Hz for quick starting
  1797. HAL_timer_start(STEP_TIMER_NUM, 122);
  1798. ENABLE_STEPPER_DRIVER_INTERRUPT();
  1799. endstops.enable(true); // Start with endstops active. After homing they can be disabled
  1800. sei();
  1801. set_directions(); // Init directions to last_direction_bits = 0
  1802. }
  1803. /**
  1804. * Set the stepper positions directly in steps
  1805. *
  1806. * The input is based on the typical per-axis XYZ steps.
  1807. * For CORE machines XYZ needs to be translated to ABC.
  1808. *
  1809. * This allows get_axis_position_mm to correctly
  1810. * derive the current XYZ position later on.
  1811. */
  1812. void Stepper::_set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e) {
  1813. #if CORE_IS_XY
  1814. // corexy positioning
  1815. // these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
  1816. count_position[A_AXIS] = a + b;
  1817. count_position[B_AXIS] = CORESIGN(a - b);
  1818. count_position[Z_AXIS] = c;
  1819. #elif CORE_IS_XZ
  1820. // corexz planning
  1821. count_position[A_AXIS] = a + c;
  1822. count_position[Y_AXIS] = b;
  1823. count_position[C_AXIS] = CORESIGN(a - c);
  1824. #elif CORE_IS_YZ
  1825. // coreyz planning
  1826. count_position[X_AXIS] = a;
  1827. count_position[B_AXIS] = b + c;
  1828. count_position[C_AXIS] = CORESIGN(b - c);
  1829. #else
  1830. // default non-h-bot planning
  1831. count_position[X_AXIS] = a;
  1832. count_position[Y_AXIS] = b;
  1833. count_position[Z_AXIS] = c;
  1834. #endif
  1835. count_position[E_AXIS] = e;
  1836. }
  1837. /**
  1838. * Get a stepper's position in steps.
  1839. */
  1840. int32_t Stepper::position(const AxisEnum axis) {
  1841. #ifdef __AVR__
  1842. // Protect the access to the position. Only required for AVR, as
  1843. // any 32bit CPU offers atomic access to 32bit variables
  1844. const bool was_enabled = STEPPER_ISR_ENABLED();
  1845. if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
  1846. #endif
  1847. const int32_t v = count_position[axis];
  1848. #ifdef __AVR__
  1849. // Reenable Stepper ISR
  1850. if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
  1851. #endif
  1852. return v;
  1853. }
  1854. // Signal endstops were triggered - This function can be called from
  1855. // an ISR context (Temperature, Stepper or limits ISR), so we must
  1856. // be very careful here. If the interrupt being preempted was the
  1857. // Stepper ISR (this CAN happen with the endstop limits ISR) then
  1858. // when the stepper ISR resumes, we must be very sure that the movement
  1859. // is properly cancelled
  1860. void Stepper::endstop_triggered(const AxisEnum axis) {
  1861. const bool was_enabled = STEPPER_ISR_ENABLED();
  1862. if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
  1863. #if IS_CORE
  1864. endstops_trigsteps[axis] = 0.5f * (
  1865. axis == CORE_AXIS_2 ? CORESIGN(count_position[CORE_AXIS_1] - count_position[CORE_AXIS_2])
  1866. : count_position[CORE_AXIS_1] + count_position[CORE_AXIS_2]
  1867. );
  1868. #else // !COREXY && !COREXZ && !COREYZ
  1869. endstops_trigsteps[axis] = count_position[axis];
  1870. #endif // !COREXY && !COREXZ && !COREYZ
  1871. // Discard the rest of the move if there is a current block
  1872. quick_stop();
  1873. if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
  1874. }
  1875. int32_t Stepper::triggered_position(const AxisEnum axis) {
  1876. #ifdef __AVR__
  1877. // Protect the access to the position. Only required for AVR, as
  1878. // any 32bit CPU offers atomic access to 32bit variables
  1879. const bool was_enabled = STEPPER_ISR_ENABLED();
  1880. if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
  1881. #endif
  1882. const int32_t v = endstops_trigsteps[axis];
  1883. #ifdef __AVR__
  1884. // Reenable Stepper ISR
  1885. if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
  1886. #endif
  1887. return v;
  1888. }
  1889. void Stepper::report_positions() {
  1890. // Protect the access to the position.
  1891. const bool was_enabled = STEPPER_ISR_ENABLED();
  1892. if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
  1893. const int32_t xpos = count_position[X_AXIS],
  1894. ypos = count_position[Y_AXIS],
  1895. zpos = count_position[Z_AXIS];
  1896. if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
  1897. #if CORE_IS_XY || CORE_IS_XZ || IS_DELTA || IS_SCARA
  1898. SERIAL_PROTOCOLPGM(MSG_COUNT_A);
  1899. #else
  1900. SERIAL_PROTOCOLPGM(MSG_COUNT_X);
  1901. #endif
  1902. SERIAL_PROTOCOL(xpos);
  1903. #if CORE_IS_XY || CORE_IS_YZ || IS_DELTA || IS_SCARA
  1904. SERIAL_PROTOCOLPGM(" B:");
  1905. #else
  1906. SERIAL_PROTOCOLPGM(" Y:");
  1907. #endif
  1908. SERIAL_PROTOCOL(ypos);
  1909. #if CORE_IS_XZ || CORE_IS_YZ || IS_DELTA
  1910. SERIAL_PROTOCOLPGM(" C:");
  1911. #else
  1912. SERIAL_PROTOCOLPGM(" Z:");
  1913. #endif
  1914. SERIAL_PROTOCOL(zpos);
  1915. SERIAL_EOL();
  1916. }
  1917. #if ENABLED(BABYSTEPPING)
  1918. #if MINIMUM_STEPPER_PULSE
  1919. #define STEP_PULSE_CYCLES ((MINIMUM_STEPPER_PULSE) * CYCLES_PER_MICROSECOND)
  1920. #else
  1921. #define STEP_PULSE_CYCLES 0
  1922. #endif
  1923. #if ENABLED(DELTA)
  1924. #define CYCLES_EATEN_BABYSTEP (2 * 15)
  1925. #else
  1926. #define CYCLES_EATEN_BABYSTEP 0
  1927. #endif
  1928. #define EXTRA_CYCLES_BABYSTEP (STEP_PULSE_CYCLES - (CYCLES_EATEN_BABYSTEP))
  1929. #define _ENABLE(AXIS) enable_## AXIS()
  1930. #define _READ_DIR(AXIS) AXIS ##_DIR_READ
  1931. #define _INVERT_DIR(AXIS) INVERT_## AXIS ##_DIR
  1932. #define _APPLY_DIR(AXIS, INVERT) AXIS ##_APPLY_DIR(INVERT, true)
  1933. #if EXTRA_CYCLES_BABYSTEP > 20
  1934. #define _SAVE_START const hal_timer_t pulse_start = HAL_timer_get_count(PULSE_TIMER_NUM)
  1935. #define _PULSE_WAIT while (EXTRA_CYCLES_BABYSTEP > (uint32_t)(HAL_timer_get_count(PULSE_TIMER_NUM) - pulse_start) * (PULSE_TIMER_PRESCALE)) { /* nada */ }
  1936. #else
  1937. #define _SAVE_START NOOP
  1938. #if EXTRA_CYCLES_BABYSTEP > 0
  1939. #define _PULSE_WAIT DELAY_NS(EXTRA_CYCLES_BABYSTEP * NANOSECONDS_PER_CYCLE)
  1940. #elif STEP_PULSE_CYCLES > 0
  1941. #define _PULSE_WAIT NOOP
  1942. #elif ENABLED(DELTA)
  1943. #define _PULSE_WAIT DELAY_US(2);
  1944. #else
  1945. #define _PULSE_WAIT DELAY_US(4);
  1946. #endif
  1947. #endif
  1948. #define BABYSTEP_AXIS(AXIS, INVERT, DIR) { \
  1949. const uint8_t old_dir = _READ_DIR(AXIS); \
  1950. _ENABLE(AXIS); \
  1951. _APPLY_DIR(AXIS, _INVERT_DIR(AXIS)^DIR^INVERT); \
  1952. DELAY_NS(400); /* DRV8825 */ \
  1953. _SAVE_START; \
  1954. _APPLY_STEP(AXIS)(!_INVERT_STEP_PIN(AXIS), true); \
  1955. _PULSE_WAIT; \
  1956. _APPLY_STEP(AXIS)(_INVERT_STEP_PIN(AXIS), true); \
  1957. _APPLY_DIR(AXIS, old_dir); \
  1958. }
  1959. // MUST ONLY BE CALLED BY AN ISR,
  1960. // No other ISR should ever interrupt this!
  1961. void Stepper::babystep(const AxisEnum axis, const bool direction) {
  1962. cli();
  1963. switch (axis) {
  1964. #if ENABLED(BABYSTEP_XY)
  1965. case X_AXIS:
  1966. #if CORE_IS_XY
  1967. BABYSTEP_AXIS(X, false, direction);
  1968. BABYSTEP_AXIS(Y, false, direction);
  1969. #elif CORE_IS_XZ
  1970. BABYSTEP_AXIS(X, false, direction);
  1971. BABYSTEP_AXIS(Z, false, direction);
  1972. #else
  1973. BABYSTEP_AXIS(X, false, direction);
  1974. #endif
  1975. break;
  1976. case Y_AXIS:
  1977. #if CORE_IS_XY
  1978. BABYSTEP_AXIS(X, false, direction);
  1979. BABYSTEP_AXIS(Y, false, direction^(CORESIGN(1)<0));
  1980. #elif CORE_IS_YZ
  1981. BABYSTEP_AXIS(Y, false, direction);
  1982. BABYSTEP_AXIS(Z, false, direction^(CORESIGN(1)<0));
  1983. #else
  1984. BABYSTEP_AXIS(Y, false, direction);
  1985. #endif
  1986. break;
  1987. #endif
  1988. case Z_AXIS: {
  1989. #if CORE_IS_XZ
  1990. BABYSTEP_AXIS(X, BABYSTEP_INVERT_Z, direction);
  1991. BABYSTEP_AXIS(Z, BABYSTEP_INVERT_Z, direction^(CORESIGN(1)<0));
  1992. #elif CORE_IS_YZ
  1993. BABYSTEP_AXIS(Y, BABYSTEP_INVERT_Z, direction);
  1994. BABYSTEP_AXIS(Z, BABYSTEP_INVERT_Z, direction^(CORESIGN(1)<0));
  1995. #elif DISABLED(DELTA)
  1996. BABYSTEP_AXIS(Z, BABYSTEP_INVERT_Z, direction);
  1997. #else // DELTA
  1998. const bool z_direction = direction ^ BABYSTEP_INVERT_Z;
  1999. enable_X();
  2000. enable_Y();
  2001. enable_Z();
  2002. const uint8_t old_x_dir_pin = X_DIR_READ,
  2003. old_y_dir_pin = Y_DIR_READ,
  2004. old_z_dir_pin = Z_DIR_READ;
  2005. X_DIR_WRITE(INVERT_X_DIR ^ z_direction);
  2006. Y_DIR_WRITE(INVERT_Y_DIR ^ z_direction);
  2007. Z_DIR_WRITE(INVERT_Z_DIR ^ z_direction);
  2008. DELAY_NS(400); // DRV8825
  2009. _SAVE_START;
  2010. X_STEP_WRITE(!INVERT_X_STEP_PIN);
  2011. Y_STEP_WRITE(!INVERT_Y_STEP_PIN);
  2012. Z_STEP_WRITE(!INVERT_Z_STEP_PIN);
  2013. _PULSE_WAIT;
  2014. X_STEP_WRITE(INVERT_X_STEP_PIN);
  2015. Y_STEP_WRITE(INVERT_Y_STEP_PIN);
  2016. Z_STEP_WRITE(INVERT_Z_STEP_PIN);
  2017. // Restore direction bits
  2018. X_DIR_WRITE(old_x_dir_pin);
  2019. Y_DIR_WRITE(old_y_dir_pin);
  2020. Z_DIR_WRITE(old_z_dir_pin);
  2021. #endif
  2022. } break;
  2023. default: break;
  2024. }
  2025. sei();
  2026. }
  2027. #endif // BABYSTEPPING
  2028. /**
  2029. * Software-controlled Stepper Motor Current
  2030. */
  2031. #if HAS_DIGIPOTSS
  2032. // From Arduino DigitalPotControl example
  2033. void Stepper::digitalPotWrite(const int16_t address, const int16_t value) {
  2034. WRITE(DIGIPOTSS_PIN, LOW); // Take the SS pin low to select the chip
  2035. SPI.transfer(address); // Send the address and value via SPI
  2036. SPI.transfer(value);
  2037. WRITE(DIGIPOTSS_PIN, HIGH); // Take the SS pin high to de-select the chip
  2038. //delay(10);
  2039. }
  2040. #endif // HAS_DIGIPOTSS
  2041. #if HAS_MOTOR_CURRENT_PWM
  2042. void Stepper::refresh_motor_power() {
  2043. for (uint8_t i = 0; i < COUNT(motor_current_setting); ++i) {
  2044. switch (i) {
  2045. #if PIN_EXISTS(MOTOR_CURRENT_PWM_XY)
  2046. case 0:
  2047. #endif
  2048. #if PIN_EXISTS(MOTOR_CURRENT_PWM_Z)
  2049. case 1:
  2050. #endif
  2051. #if PIN_EXISTS(MOTOR_CURRENT_PWM_E)
  2052. case 2:
  2053. #endif
  2054. digipot_current(i, motor_current_setting[i]);
  2055. default: break;
  2056. }
  2057. }
  2058. }
  2059. #endif // HAS_MOTOR_CURRENT_PWM
  2060. #if HAS_DIGIPOTSS || HAS_MOTOR_CURRENT_PWM
  2061. void Stepper::digipot_current(const uint8_t driver, const int current) {
  2062. #if HAS_DIGIPOTSS
  2063. const uint8_t digipot_ch[] = DIGIPOT_CHANNELS;
  2064. digitalPotWrite(digipot_ch[driver], current);
  2065. #elif HAS_MOTOR_CURRENT_PWM
  2066. if (WITHIN(driver, 0, 2))
  2067. motor_current_setting[driver] = current; // update motor_current_setting
  2068. #define _WRITE_CURRENT_PWM(P) analogWrite(MOTOR_CURRENT_PWM_## P ##_PIN, 255L * current / (MOTOR_CURRENT_PWM_RANGE))
  2069. switch (driver) {
  2070. #if PIN_EXISTS(MOTOR_CURRENT_PWM_XY)
  2071. case 0: _WRITE_CURRENT_PWM(XY); break;
  2072. #endif
  2073. #if PIN_EXISTS(MOTOR_CURRENT_PWM_Z)
  2074. case 1: _WRITE_CURRENT_PWM(Z); break;
  2075. #endif
  2076. #if PIN_EXISTS(MOTOR_CURRENT_PWM_E)
  2077. case 2: _WRITE_CURRENT_PWM(E); break;
  2078. #endif
  2079. }
  2080. #endif
  2081. }
  2082. void Stepper::digipot_init() {
  2083. #if HAS_DIGIPOTSS
  2084. static const uint8_t digipot_motor_current[] = DIGIPOT_MOTOR_CURRENT;
  2085. SPI.begin();
  2086. SET_OUTPUT(DIGIPOTSS_PIN);
  2087. for (uint8_t i = 0; i < COUNT(digipot_motor_current); i++) {
  2088. //digitalPotWrite(digipot_ch[i], digipot_motor_current[i]);
  2089. digipot_current(i, digipot_motor_current[i]);
  2090. }
  2091. #elif HAS_MOTOR_CURRENT_PWM
  2092. #if PIN_EXISTS(MOTOR_CURRENT_PWM_XY)
  2093. SET_OUTPUT(MOTOR_CURRENT_PWM_XY_PIN);
  2094. #endif
  2095. #if PIN_EXISTS(MOTOR_CURRENT_PWM_Z)
  2096. SET_OUTPUT(MOTOR_CURRENT_PWM_Z_PIN);
  2097. #endif
  2098. #if PIN_EXISTS(MOTOR_CURRENT_PWM_E)
  2099. SET_OUTPUT(MOTOR_CURRENT_PWM_E_PIN);
  2100. #endif
  2101. refresh_motor_power();
  2102. // Set Timer5 to 31khz so the PWM of the motor power is as constant as possible. (removes a buzzing noise)
  2103. SET_CS5(PRESCALER_1);
  2104. #endif
  2105. }
  2106. #endif
  2107. #if HAS_MICROSTEPS
  2108. /**
  2109. * Software-controlled Microstepping
  2110. */
  2111. void Stepper::microstep_init() {
  2112. SET_OUTPUT(X_MS1_PIN);
  2113. SET_OUTPUT(X_MS2_PIN);
  2114. #if HAS_Y_MICROSTEPS
  2115. SET_OUTPUT(Y_MS1_PIN);
  2116. SET_OUTPUT(Y_MS2_PIN);
  2117. #endif
  2118. #if HAS_Z_MICROSTEPS
  2119. SET_OUTPUT(Z_MS1_PIN);
  2120. SET_OUTPUT(Z_MS2_PIN);
  2121. #endif
  2122. #if HAS_E0_MICROSTEPS
  2123. SET_OUTPUT(E0_MS1_PIN);
  2124. SET_OUTPUT(E0_MS2_PIN);
  2125. #endif
  2126. #if HAS_E1_MICROSTEPS
  2127. SET_OUTPUT(E1_MS1_PIN);
  2128. SET_OUTPUT(E1_MS2_PIN);
  2129. #endif
  2130. #if HAS_E2_MICROSTEPS
  2131. SET_OUTPUT(E2_MS1_PIN);
  2132. SET_OUTPUT(E2_MS2_PIN);
  2133. #endif
  2134. #if HAS_E3_MICROSTEPS
  2135. SET_OUTPUT(E3_MS1_PIN);
  2136. SET_OUTPUT(E3_MS2_PIN);
  2137. #endif
  2138. #if HAS_E4_MICROSTEPS
  2139. SET_OUTPUT(E4_MS1_PIN);
  2140. SET_OUTPUT(E4_MS2_PIN);
  2141. #endif
  2142. static const uint8_t microstep_modes[] = MICROSTEP_MODES;
  2143. for (uint16_t i = 0; i < COUNT(microstep_modes); i++)
  2144. microstep_mode(i, microstep_modes[i]);
  2145. }
  2146. void Stepper::microstep_ms(const uint8_t driver, const int8_t ms1, const int8_t ms2) {
  2147. if (ms1 >= 0) switch (driver) {
  2148. case 0: WRITE(X_MS1_PIN, ms1); break;
  2149. #if HAS_Y_MICROSTEPS
  2150. case 1: WRITE(Y_MS1_PIN, ms1); break;
  2151. #endif
  2152. #if HAS_Z_MICROSTEPS
  2153. case 2: WRITE(Z_MS1_PIN, ms1); break;
  2154. #endif
  2155. #if HAS_E0_MICROSTEPS
  2156. case 3: WRITE(E0_MS1_PIN, ms1); break;
  2157. #endif
  2158. #if HAS_E1_MICROSTEPS
  2159. case 4: WRITE(E1_MS1_PIN, ms1); break;
  2160. #endif
  2161. #if HAS_E2_MICROSTEPS
  2162. case 5: WRITE(E2_MS1_PIN, ms1); break;
  2163. #endif
  2164. #if HAS_E3_MICROSTEPS
  2165. case 6: WRITE(E3_MS1_PIN, ms1); break;
  2166. #endif
  2167. #if HAS_E4_MICROSTEPS
  2168. case 7: WRITE(E4_MS1_PIN, ms1); break;
  2169. #endif
  2170. }
  2171. if (ms2 >= 0) switch (driver) {
  2172. case 0: WRITE(X_MS2_PIN, ms2); break;
  2173. #if HAS_Y_MICROSTEPS
  2174. case 1: WRITE(Y_MS2_PIN, ms2); break;
  2175. #endif
  2176. #if HAS_Z_MICROSTEPS
  2177. case 2: WRITE(Z_MS2_PIN, ms2); break;
  2178. #endif
  2179. #if HAS_E0_MICROSTEPS
  2180. case 3: WRITE(E0_MS2_PIN, ms2); break;
  2181. #endif
  2182. #if HAS_E1_MICROSTEPS
  2183. case 4: WRITE(E1_MS2_PIN, ms2); break;
  2184. #endif
  2185. #if HAS_E2_MICROSTEPS
  2186. case 5: WRITE(E2_MS2_PIN, ms2); break;
  2187. #endif
  2188. #if HAS_E3_MICROSTEPS
  2189. case 6: WRITE(E3_MS2_PIN, ms2); break;
  2190. #endif
  2191. #if HAS_E4_MICROSTEPS
  2192. case 7: WRITE(E4_MS2_PIN, ms2); break;
  2193. #endif
  2194. }
  2195. }
  2196. void Stepper::microstep_mode(const uint8_t driver, const uint8_t stepping_mode) {
  2197. switch (stepping_mode) {
  2198. case 1: microstep_ms(driver, MICROSTEP1); break;
  2199. #if ENABLED(HEROIC_STEPPER_DRIVERS)
  2200. case 128: microstep_ms(driver, MICROSTEP128); break;
  2201. #else
  2202. case 2: microstep_ms(driver, MICROSTEP2); break;
  2203. case 4: microstep_ms(driver, MICROSTEP4); break;
  2204. #endif
  2205. case 8: microstep_ms(driver, MICROSTEP8); break;
  2206. case 16: microstep_ms(driver, MICROSTEP16); break;
  2207. #if MB(ALLIGATOR)
  2208. case 32: microstep_ms(driver, MICROSTEP32); break;
  2209. #endif
  2210. default: SERIAL_ERROR_START(); SERIAL_ERRORLNPGM("Microsteps unavailable"); break;
  2211. }
  2212. }
  2213. void Stepper::microstep_readings() {
  2214. SERIAL_PROTOCOLLNPGM("MS1,MS2 Pins");
  2215. SERIAL_PROTOCOLPGM("X: ");
  2216. SERIAL_PROTOCOL(READ(X_MS1_PIN));
  2217. SERIAL_PROTOCOLLN(READ(X_MS2_PIN));
  2218. #if HAS_Y_MICROSTEPS
  2219. SERIAL_PROTOCOLPGM("Y: ");
  2220. SERIAL_PROTOCOL(READ(Y_MS1_PIN));
  2221. SERIAL_PROTOCOLLN(READ(Y_MS2_PIN));
  2222. #endif
  2223. #if HAS_Z_MICROSTEPS
  2224. SERIAL_PROTOCOLPGM("Z: ");
  2225. SERIAL_PROTOCOL(READ(Z_MS1_PIN));
  2226. SERIAL_PROTOCOLLN(READ(Z_MS2_PIN));
  2227. #endif
  2228. #if HAS_E0_MICROSTEPS
  2229. SERIAL_PROTOCOLPGM("E0: ");
  2230. SERIAL_PROTOCOL(READ(E0_MS1_PIN));
  2231. SERIAL_PROTOCOLLN(READ(E0_MS2_PIN));
  2232. #endif
  2233. #if HAS_E1_MICROSTEPS
  2234. SERIAL_PROTOCOLPGM("E1: ");
  2235. SERIAL_PROTOCOL(READ(E1_MS1_PIN));
  2236. SERIAL_PROTOCOLLN(READ(E1_MS2_PIN));
  2237. #endif
  2238. #if HAS_E2_MICROSTEPS
  2239. SERIAL_PROTOCOLPGM("E2: ");
  2240. SERIAL_PROTOCOL(READ(E2_MS1_PIN));
  2241. SERIAL_PROTOCOLLN(READ(E2_MS2_PIN));
  2242. #endif
  2243. #if HAS_E3_MICROSTEPS
  2244. SERIAL_PROTOCOLPGM("E3: ");
  2245. SERIAL_PROTOCOL(READ(E3_MS1_PIN));
  2246. SERIAL_PROTOCOLLN(READ(E3_MS2_PIN));
  2247. #endif
  2248. #if HAS_E4_MICROSTEPS
  2249. SERIAL_PROTOCOLPGM("E4: ");
  2250. SERIAL_PROTOCOL(READ(E4_MS1_PIN));
  2251. SERIAL_PROTOCOLLN(READ(E4_MS2_PIN));
  2252. #endif
  2253. }
  2254. #endif // HAS_MICROSTEPS