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.
 
 
 
 
 

835 lines
30 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. * planner.h
  24. *
  25. * Buffer movement commands and manage the acceleration profile plan
  26. *
  27. * Derived from Grbl
  28. * Copyright (c) 2009-2011 Simen Svale Skogsrud
  29. */
  30. #ifndef PLANNER_H
  31. #define PLANNER_H
  32. #include "../Marlin.h"
  33. #include "motion.h"
  34. #include "../gcode/queue.h"
  35. #if ENABLED(DELTA)
  36. #include "delta.h"
  37. #endif
  38. #if ABL_PLANAR
  39. #include "../libs/vector_3.h"
  40. #endif
  41. enum BlockFlagBit : char {
  42. // Recalculate trapezoids on entry junction. For optimization.
  43. BLOCK_BIT_RECALCULATE,
  44. // Nominal speed always reached.
  45. // i.e., The segment is long enough, so the nominal speed is reachable if accelerating
  46. // from a safe speed (in consideration of jerking from zero speed).
  47. BLOCK_BIT_NOMINAL_LENGTH,
  48. // The block is busy, being interpreted by the stepper ISR
  49. BLOCK_BIT_BUSY,
  50. // The block is segment 2+ of a longer move
  51. BLOCK_BIT_CONTINUED,
  52. // Sync the stepper counts from the block
  53. BLOCK_BIT_SYNC_POSITION
  54. };
  55. enum BlockFlag : char {
  56. BLOCK_FLAG_RECALCULATE = _BV(BLOCK_BIT_RECALCULATE),
  57. BLOCK_FLAG_NOMINAL_LENGTH = _BV(BLOCK_BIT_NOMINAL_LENGTH),
  58. BLOCK_FLAG_BUSY = _BV(BLOCK_BIT_BUSY),
  59. BLOCK_FLAG_CONTINUED = _BV(BLOCK_BIT_CONTINUED),
  60. BLOCK_FLAG_SYNC_POSITION = _BV(BLOCK_BIT_SYNC_POSITION)
  61. };
  62. /**
  63. * struct block_t
  64. *
  65. * A single entry in the planner buffer.
  66. * Tracks linear movement over multiple axes.
  67. *
  68. * The "nominal" values are as-specified by gcode, and
  69. * may never actually be reached due to acceleration limits.
  70. */
  71. typedef struct {
  72. uint8_t flag; // Block flags (See BlockFlag enum above)
  73. // Fields used by the motion planner to manage acceleration
  74. float nominal_speed_sqr, // The nominal speed for this block in (mm/sec)^2
  75. entry_speed_sqr, // Entry speed at previous-current junction in (mm/sec)^2
  76. max_entry_speed_sqr, // Maximum allowable junction entry speed in (mm/sec)^2
  77. millimeters, // The total travel of this block in mm
  78. acceleration; // acceleration mm/sec^2
  79. union {
  80. // Data used by all move blocks
  81. struct {
  82. // Fields used by the Bresenham algorithm for tracing the line
  83. uint32_t steps[NUM_AXIS]; // Step count along each axis
  84. };
  85. // Data used by all sync blocks
  86. struct {
  87. int32_t position[NUM_AXIS]; // New position to force when this sync block is executed
  88. };
  89. };
  90. uint32_t step_event_count; // The number of step events required to complete this block
  91. uint8_t active_extruder; // The extruder to move (if E move)
  92. #if ENABLED(MIXING_EXTRUDER)
  93. uint32_t mix_steps[MIXING_STEPPERS]; // Scaled steps[E_AXIS] for the mixing steppers
  94. #endif
  95. // Settings for the trapezoid generator
  96. uint32_t accelerate_until, // The index of the step event on which to stop acceleration
  97. decelerate_after; // The index of the step event on which to start decelerating
  98. #if ENABLED(S_CURVE_ACCELERATION)
  99. uint32_t cruise_rate; // The actual cruise rate to use, between end of the acceleration phase and start of deceleration phase
  100. uint32_t acceleration_time, // Acceleration time and deceleration time in STEP timer counts
  101. deceleration_time;
  102. uint32_t acceleration_time_inverse, // Inverse of acceleration and deceleration periods, expressed as integer. Scale depends on CPU being used
  103. deceleration_time_inverse;
  104. #else
  105. uint32_t acceleration_rate; // The acceleration rate used for acceleration calculation
  106. #endif
  107. uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
  108. // Advance extrusion
  109. #if ENABLED(LIN_ADVANCE)
  110. bool use_advance_lead;
  111. uint16_t advance_speed, // STEP timer value for extruder speed offset ISR
  112. max_adv_steps, // max. advance steps to get cruising speed pressure (not always nominal_speed!)
  113. final_adv_steps; // advance steps due to exit speed
  114. float e_D_ratio;
  115. #endif
  116. uint32_t nominal_rate, // The nominal step rate for this block in step_events/sec
  117. initial_rate, // The jerk-adjusted step rate at start of block
  118. final_rate, // The minimal rate at exit
  119. acceleration_steps_per_s2; // acceleration steps/sec^2
  120. #if FAN_COUNT > 0
  121. uint16_t fan_speed[FAN_COUNT];
  122. #endif
  123. #if ENABLED(BARICUDA)
  124. uint8_t valve_pressure, e_to_p_pressure;
  125. #endif
  126. uint32_t segment_time_us;
  127. } block_t;
  128. #define HAS_POSITION_FLOAT (ENABLED(LIN_ADVANCE) || ENABLED(SCARA_FEEDRATE_SCALING))
  129. #define BLOCK_MOD(n) ((n)&(BLOCK_BUFFER_SIZE-1))
  130. class Planner {
  131. public:
  132. /**
  133. * The move buffer, calculated in stepper steps
  134. *
  135. * block_buffer is a ring buffer...
  136. *
  137. * head,tail : indexes for write,read
  138. * head==tail : the buffer is empty
  139. * head!=tail : blocks are in the buffer
  140. * head==(tail-1)%size : the buffer is full
  141. *
  142. * Writer of head is Planner::buffer_segment().
  143. * Reader of tail is Stepper::isr(). Always consider tail busy / read-only
  144. */
  145. static block_t block_buffer[BLOCK_BUFFER_SIZE];
  146. static volatile uint8_t block_buffer_head, // Index of the next block to be pushed
  147. block_buffer_tail; // Index of the busy block, if any
  148. static uint16_t cleaning_buffer_counter; // A counter to disable queuing of blocks
  149. static uint8_t delay_before_delivering, // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks
  150. block_buffer_planned; // Index of the optimally planned block
  151. #if ENABLED(DISTINCT_E_FACTORS)
  152. static uint8_t last_extruder; // Respond to extruder change
  153. #endif
  154. static int16_t flow_percentage[EXTRUDERS]; // Extrusion factor for each extruder
  155. static float e_factor[EXTRUDERS]; // The flow percentage and volumetric multiplier combine to scale E movement
  156. #if DISABLED(NO_VOLUMETRICS)
  157. static float filament_size[EXTRUDERS], // diameter of filament (in millimeters), typically around 1.75 or 2.85, 0 disables the volumetric calculations for the extruder
  158. volumetric_area_nominal, // Nominal cross-sectional area
  159. volumetric_multiplier[EXTRUDERS]; // Reciprocal of cross-sectional area of filament (in mm^2). Pre-calculated to reduce computation in the planner
  160. // May be auto-adjusted by a filament width sensor
  161. #endif
  162. static float max_feedrate_mm_s[XYZE_N], // Max speeds in mm per second
  163. axis_steps_per_mm[XYZE_N],
  164. steps_to_mm[XYZE_N];
  165. static uint32_t max_acceleration_steps_per_s2[XYZE_N],
  166. max_acceleration_mm_per_s2[XYZE_N]; // Use M201 to override
  167. static uint32_t min_segment_time_us; // Use 'M205 B<µs>' to override
  168. static float min_feedrate_mm_s,
  169. acceleration, // Normal acceleration mm/s^2 DEFAULT ACCELERATION for all printing moves. M204 SXXXX
  170. retract_acceleration, // Retract acceleration mm/s^2 filament pull-back and push-forward while standing still in the other axes M204 TXXXX
  171. travel_acceleration, // Travel acceleration mm/s^2 DEFAULT ACCELERATION for all NON printing moves. M204 MXXXX
  172. max_jerk[XYZE], // The largest speed change requiring no acceleration
  173. min_travel_feedrate_mm_s;
  174. #if HAS_LEVELING
  175. static bool leveling_active; // Flag that bed leveling is enabled
  176. #if ABL_PLANAR
  177. static matrix_3x3 bed_level_matrix; // Transform to compensate for bed level
  178. #endif
  179. #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
  180. static float z_fade_height, inverse_z_fade_height;
  181. #endif
  182. #else
  183. static constexpr bool leveling_active = false;
  184. #endif
  185. #if ENABLED(LIN_ADVANCE)
  186. static float extruder_advance_K;
  187. #endif
  188. #if HAS_POSITION_FLOAT
  189. static float position_float[XYZE];
  190. #endif
  191. #if ENABLED(SKEW_CORRECTION)
  192. #if ENABLED(SKEW_CORRECTION_GCODE)
  193. static float xy_skew_factor;
  194. #else
  195. static constexpr float xy_skew_factor = XY_SKEW_FACTOR;
  196. #endif
  197. #if ENABLED(SKEW_CORRECTION_FOR_Z)
  198. #if ENABLED(SKEW_CORRECTION_GCODE)
  199. static float xz_skew_factor, yz_skew_factor;
  200. #else
  201. static constexpr float xz_skew_factor = XZ_SKEW_FACTOR, yz_skew_factor = YZ_SKEW_FACTOR;
  202. #endif
  203. #else
  204. static constexpr float xz_skew_factor = 0, yz_skew_factor = 0;
  205. #endif
  206. #endif
  207. #if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
  208. static bool abort_on_endstop_hit;
  209. #endif
  210. private:
  211. /**
  212. * The current position of the tool in absolute steps
  213. * Recalculated if any axis_steps_per_mm are changed by gcode
  214. */
  215. static int32_t position[NUM_AXIS];
  216. /**
  217. * Speed of previous path line segment
  218. */
  219. static float previous_speed[NUM_AXIS];
  220. /**
  221. * Nominal speed of previous path line segment (mm/s)^2
  222. */
  223. static float previous_nominal_speed_sqr;
  224. /**
  225. * Limit where 64bit math is necessary for acceleration calculation
  226. */
  227. static uint32_t cutoff_long;
  228. #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
  229. static float last_fade_z;
  230. #endif
  231. #if ENABLED(DISABLE_INACTIVE_EXTRUDER)
  232. /**
  233. * Counters to manage disabling inactive extruders
  234. */
  235. static uint8_t g_uc_extruder_last_move[EXTRUDERS];
  236. #endif // DISABLE_INACTIVE_EXTRUDER
  237. #ifdef XY_FREQUENCY_LIMIT
  238. // Used for the frequency limit
  239. #define MAX_FREQ_TIME_US (uint32_t)(1000000.0 / XY_FREQUENCY_LIMIT)
  240. // Old direction bits. Used for speed calculations
  241. static unsigned char old_direction_bits;
  242. // Segment times (in µs). Used for speed calculations
  243. static uint32_t axis_segment_time_us[2][3];
  244. #endif
  245. #if ENABLED(ULTRA_LCD)
  246. volatile static uint32_t block_buffer_runtime_us; //Theoretical block buffer runtime in µs
  247. #endif
  248. public:
  249. /**
  250. * Instance Methods
  251. */
  252. Planner();
  253. void init();
  254. /**
  255. * Static (class) Methods
  256. */
  257. static void reset_acceleration_rates();
  258. static void refresh_positioning();
  259. FORCE_INLINE static void refresh_e_factor(const uint8_t e) {
  260. e_factor[e] = (flow_percentage[e] * 0.01
  261. #if DISABLED(NO_VOLUMETRICS)
  262. * volumetric_multiplier[e]
  263. #endif
  264. );
  265. }
  266. // Manage fans, paste pressure, etc.
  267. static void check_axes_activity();
  268. // Update multipliers based on new diameter measurements
  269. static void calculate_volumetric_multipliers();
  270. #if ENABLED(FILAMENT_WIDTH_SENSOR)
  271. void calculate_volumetric_for_width_sensor(const int8_t encoded_ratio);
  272. #endif
  273. #if DISABLED(NO_VOLUMETRICS)
  274. FORCE_INLINE static void set_filament_size(const uint8_t e, const float &v) {
  275. filament_size[e] = v;
  276. // make sure all extruders have some sane value for the filament size
  277. for (uint8_t i = 0; i < COUNT(filament_size); i++)
  278. if (!filament_size[i]) filament_size[i] = DEFAULT_NOMINAL_FILAMENT_DIA;
  279. }
  280. #endif
  281. #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
  282. /**
  283. * Get the Z leveling fade factor based on the given Z height,
  284. * re-calculating only when needed.
  285. *
  286. * Returns 1.0 if planner.z_fade_height is 0.0.
  287. * Returns 0.0 if Z is past the specified 'Fade Height'.
  288. */
  289. inline static float fade_scaling_factor_for_z(const float &rz) {
  290. static float z_fade_factor = 1.0;
  291. if (z_fade_height) {
  292. if (rz >= z_fade_height) return 0.0;
  293. if (last_fade_z != rz) {
  294. last_fade_z = rz;
  295. z_fade_factor = 1.0 - rz * inverse_z_fade_height;
  296. }
  297. return z_fade_factor;
  298. }
  299. return 1.0;
  300. }
  301. FORCE_INLINE static void force_fade_recalc() { last_fade_z = -999.999; }
  302. FORCE_INLINE static void set_z_fade_height(const float &zfh) {
  303. z_fade_height = zfh > 0 ? zfh : 0;
  304. inverse_z_fade_height = RECIPROCAL(z_fade_height);
  305. force_fade_recalc();
  306. }
  307. FORCE_INLINE static bool leveling_active_at_z(const float &rz) {
  308. return !z_fade_height || rz < z_fade_height;
  309. }
  310. #else
  311. FORCE_INLINE static float fade_scaling_factor_for_z(const float &rz) {
  312. UNUSED(rz);
  313. return 1.0;
  314. }
  315. FORCE_INLINE static bool leveling_active_at_z(const float &rz) { UNUSED(rz); return true; }
  316. #endif
  317. #if ENABLED(SKEW_CORRECTION)
  318. FORCE_INLINE static void skew(float &cx, float &cy, const float &cz) {
  319. if (WITHIN(cx, X_MIN_POS + 1, X_MAX_POS) && WITHIN(cy, Y_MIN_POS + 1, Y_MAX_POS)) {
  320. const float sx = cx - cy * xy_skew_factor - cz * (xz_skew_factor - (xy_skew_factor * yz_skew_factor)),
  321. sy = cy - cz * yz_skew_factor;
  322. if (WITHIN(sx, X_MIN_POS, X_MAX_POS) && WITHIN(sy, Y_MIN_POS, Y_MAX_POS)) {
  323. cx = sx; cy = sy;
  324. }
  325. }
  326. }
  327. FORCE_INLINE static void unskew(float &cx, float &cy, const float &cz) {
  328. if (WITHIN(cx, X_MIN_POS, X_MAX_POS) && WITHIN(cy, Y_MIN_POS, Y_MAX_POS)) {
  329. const float sx = cx + cy * xy_skew_factor + cz * xz_skew_factor,
  330. sy = cy + cz * yz_skew_factor;
  331. if (WITHIN(sx, X_MIN_POS, X_MAX_POS) && WITHIN(sy, Y_MIN_POS, Y_MAX_POS)) {
  332. cx = sx; cy = sy;
  333. }
  334. }
  335. }
  336. #endif // SKEW_CORRECTION
  337. #if PLANNER_LEVELING || HAS_UBL_AND_CURVES
  338. /**
  339. * Apply leveling to transform a cartesian position
  340. * as it will be given to the planner and steppers.
  341. */
  342. static void apply_leveling(float &rx, float &ry, float &rz);
  343. FORCE_INLINE static void apply_leveling(float (&raw)[XYZ]) { apply_leveling(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); }
  344. #endif
  345. #if PLANNER_LEVELING
  346. #define ARG_X float rx
  347. #define ARG_Y float ry
  348. #define ARG_Z float rz
  349. static void unapply_leveling(float raw[XYZ]);
  350. #else
  351. #define ARG_X const float &rx
  352. #define ARG_Y const float &ry
  353. #define ARG_Z const float &rz
  354. #endif
  355. // Number of moves currently in the planner
  356. FORCE_INLINE static uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail); }
  357. // Remove all blocks from the buffer
  358. FORCE_INLINE static void clear_block_buffer() { block_buffer_head = block_buffer_tail = 0; }
  359. // Check if movement queue is full
  360. FORCE_INLINE static bool is_full() { return block_buffer_tail == next_block_index(block_buffer_head); }
  361. // Get count of movement slots free
  362. FORCE_INLINE static uint8_t moves_free() { return BLOCK_BUFFER_SIZE - 1 - movesplanned(); }
  363. /**
  364. * Planner::get_next_free_block
  365. *
  366. * - Get the next head indices (passed by reference)
  367. * - Wait for the number of spaces to open up in the planner
  368. * - Return the first head block
  369. */
  370. FORCE_INLINE static block_t* get_next_free_block(uint8_t &next_buffer_head, const uint8_t count=1) {
  371. // Wait until there are enough slots free
  372. while (moves_free() < count) { idle(); }
  373. // Return the first available block
  374. next_buffer_head = next_block_index(block_buffer_head);
  375. return &block_buffer[block_buffer_head];
  376. }
  377. /**
  378. * Planner::_buffer_steps
  379. *
  380. * Add a new linear movement to the buffer (in terms of steps).
  381. *
  382. * target - target position in steps units
  383. * fr_mm_s - (target) speed of the move
  384. * extruder - target extruder
  385. * millimeters - the length of the movement, if known
  386. *
  387. * Returns true if movement was buffered, false otherwise
  388. */
  389. static bool _buffer_steps(const int32_t (&target)[XYZE]
  390. #if HAS_POSITION_FLOAT
  391. , const float (&target_float)[XYZE]
  392. #endif
  393. , float fr_mm_s, const uint8_t extruder, const float &millimeters=0.0
  394. );
  395. /**
  396. * Planner::_populate_block
  397. *
  398. * Fills a new linear movement in the block (in terms of steps).
  399. *
  400. * target - target position in steps units
  401. * fr_mm_s - (target) speed of the move
  402. * extruder - target extruder
  403. * millimeters - the length of the movement, if known
  404. *
  405. * Returns true is movement is acceptable, false otherwise
  406. */
  407. static bool _populate_block(block_t * const block, bool split_move,
  408. const int32_t (&target)[XYZE]
  409. #if HAS_POSITION_FLOAT
  410. , const float (&target_float)[XYZE]
  411. #endif
  412. , float fr_mm_s, const uint8_t extruder, const float &millimeters=0.0
  413. );
  414. /**
  415. * Planner::buffer_sync_block
  416. * Add a block to the buffer that just updates the position
  417. */
  418. static void buffer_sync_block();
  419. /**
  420. * Planner::buffer_segment
  421. *
  422. * Add a new linear movement to the buffer in axis units.
  423. *
  424. * Leveling and kinematics should be applied ahead of calling this.
  425. *
  426. * a,b,c,e - target positions in mm and/or degrees
  427. * fr_mm_s - (target) speed of the move
  428. * extruder - target extruder
  429. * millimeters - the length of the movement, if known
  430. */
  431. static bool buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder, const float &millimeters=0.0);
  432. static void _set_position_mm(const float &a, const float &b, const float &c, const float &e);
  433. /**
  434. * Add a new linear movement to the buffer.
  435. * The target is NOT translated to delta/scara
  436. *
  437. * Leveling will be applied to input on cartesians.
  438. * Kinematic machines should call buffer_line_kinematic (for leveled moves).
  439. * (Cartesians may also call buffer_line_kinematic.)
  440. *
  441. * rx,ry,rz,e - target position in mm or degrees
  442. * fr_mm_s - (target) speed of the move (mm/s)
  443. * extruder - target extruder
  444. * millimeters - the length of the movement, if known
  445. */
  446. FORCE_INLINE static bool buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, const float &fr_mm_s, const uint8_t extruder, const float millimeters = 0.0) {
  447. #if PLANNER_LEVELING && IS_CARTESIAN
  448. apply_leveling(rx, ry, rz);
  449. #endif
  450. return buffer_segment(rx, ry, rz, e, fr_mm_s, extruder, millimeters);
  451. }
  452. /**
  453. * Add a new linear movement to the buffer.
  454. * The target is cartesian, it's translated to delta/scara if
  455. * needed.
  456. *
  457. * cart - x,y,z,e CARTESIAN target in mm
  458. * fr_mm_s - (target) speed of the move (mm/s)
  459. * extruder - target extruder
  460. * millimeters - the length of the movement, if known
  461. */
  462. FORCE_INLINE static bool buffer_line_kinematic(const float (&cart)[XYZE], const float &fr_mm_s, const uint8_t extruder, const float millimeters = 0.0) {
  463. #if PLANNER_LEVELING
  464. float raw[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] };
  465. apply_leveling(raw);
  466. #else
  467. const float (&raw)[XYZE] = cart;
  468. #endif
  469. #if IS_KINEMATIC
  470. inverse_kinematics(raw);
  471. return buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters);
  472. #else
  473. return buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters);
  474. #endif
  475. }
  476. /**
  477. * Set the planner.position and individual stepper positions.
  478. * Used by G92, G28, G29, and other procedures.
  479. *
  480. * Multiplies by axis_steps_per_mm[] and does necessary conversion
  481. * for COREXY / COREXZ / COREYZ to set the corresponding stepper positions.
  482. *
  483. * Clears previous speed values.
  484. */
  485. FORCE_INLINE static void set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) {
  486. #if PLANNER_LEVELING && IS_CARTESIAN
  487. apply_leveling(rx, ry, rz);
  488. #endif
  489. _set_position_mm(rx, ry, rz, e);
  490. }
  491. static void set_position_mm_kinematic(const float (&cart)[XYZE]);
  492. static void set_position_mm(const AxisEnum axis, const float &v);
  493. FORCE_INLINE static void set_z_position_mm(const float &z) { set_position_mm(Z_AXIS, z); }
  494. FORCE_INLINE static void set_e_position_mm(const float &e) { set_position_mm(E_AXIS, e); }
  495. /**
  496. * Get an axis position according to stepper position(s)
  497. * For CORE machines apply translation from ABC to XYZ.
  498. */
  499. static float get_axis_position_mm(const AxisEnum axis);
  500. // SCARA AB axes are in degrees, not mm
  501. #if IS_SCARA
  502. FORCE_INLINE static float get_axis_position_degrees(const AxisEnum axis) { return get_axis_position_mm(axis); }
  503. #endif
  504. // Called to force a quick stop of the machine (for example, when an emergency
  505. // stop is required, or when endstops are hit)
  506. static void quick_stop();
  507. // Called when an endstop is triggered. Causes the machine to stop inmediately
  508. static void endstop_triggered(const AxisEnum axis);
  509. // Triggered position of an axis in mm (not core-savvy)
  510. static float triggered_position_mm(const AxisEnum axis);
  511. // Block until all buffered steps are executed / cleaned
  512. static void synchronize();
  513. // Wait for moves to finish and disable all steppers
  514. static void finish_and_disable();
  515. // Periodic tick to handle cleaning timeouts
  516. // Called from the Temperature ISR at ~1kHz
  517. static void tick() {
  518. if (cleaning_buffer_counter) {
  519. --cleaning_buffer_counter;
  520. #if ENABLED(SD_FINISHED_STEPPERRELEASE) && defined(SD_FINISHED_RELEASECOMMAND)
  521. if (!cleaning_buffer_counter) enqueue_and_echo_commands_P(PSTR(SD_FINISHED_RELEASECOMMAND));
  522. #endif
  523. }
  524. }
  525. /**
  526. * Does the buffer have any blocks queued?
  527. */
  528. FORCE_INLINE static bool has_blocks_queued() { return (block_buffer_head != block_buffer_tail); }
  529. /**
  530. * The current block. NULL if the buffer is empty.
  531. * This also marks the block as busy.
  532. * WARNING: Called from Stepper ISR context!
  533. */
  534. static block_t* get_current_block() {
  535. // Get the number of moves in the planner queue so far
  536. uint8_t nr_moves = movesplanned();
  537. // If there are any moves queued ...
  538. if (nr_moves) {
  539. // If there is still delay of delivery of blocks running, decrement it
  540. if (delay_before_delivering) {
  541. --delay_before_delivering;
  542. // If the number of movements queued is less than 3, and there is still time
  543. // to wait, do not deliver anything
  544. if (nr_moves < 3 && delay_before_delivering) return NULL;
  545. delay_before_delivering = 0;
  546. }
  547. // If we are here, there is no excuse to deliver the block
  548. block_t * const block = &block_buffer[block_buffer_tail];
  549. // No trapezoid calculated? Don't execute yet.
  550. if (TEST(block->flag, BLOCK_BIT_RECALCULATE)) return NULL;
  551. #if ENABLED(ULTRA_LCD)
  552. block_buffer_runtime_us -= block->segment_time_us; // We can't be sure how long an active block will take, so don't count it.
  553. #endif
  554. // Mark the block as busy, so the planner does not attempt to replan it
  555. SBI(block->flag, BLOCK_BIT_BUSY);
  556. return block;
  557. }
  558. // The queue became empty
  559. #if ENABLED(ULTRA_LCD)
  560. clear_block_buffer_runtime(); // paranoia. Buffer is empty now - so reset accumulated time to zero.
  561. #endif
  562. return NULL;
  563. }
  564. /**
  565. * "Discard" the block and "release" the memory.
  566. * Called when the current block is no longer needed.
  567. * NB: There MUST be a current block to call this function!!
  568. */
  569. FORCE_INLINE static void discard_current_block() {
  570. if (has_blocks_queued()) { // Discard non-empty buffer.
  571. uint8_t block_index = next_block_index( block_buffer_tail );
  572. // Push block_buffer_planned pointer, if encountered.
  573. if (!has_blocks_queued()) block_buffer_planned = block_index;
  574. block_buffer_tail = block_index;
  575. }
  576. }
  577. #if ENABLED(ULTRA_LCD)
  578. static uint16_t block_buffer_runtime() {
  579. #ifdef __AVR__
  580. // Protect the access to the variable. Only required for AVR, as
  581. // any 32bit CPU offers atomic access to 32bit variables
  582. bool was_enabled = STEPPER_ISR_ENABLED();
  583. if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
  584. #endif
  585. millis_t bbru = block_buffer_runtime_us;
  586. #ifdef __AVR__
  587. // Reenable Stepper ISR
  588. if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
  589. #endif
  590. // To translate µs to ms a division by 1000 would be required.
  591. // We introduce 2.4% error here by dividing by 1024.
  592. // Doesn't matter because block_buffer_runtime_us is already too small an estimation.
  593. bbru >>= 10;
  594. // limit to about a minute.
  595. NOMORE(bbru, 0xFFFFul);
  596. return bbru;
  597. }
  598. static void clear_block_buffer_runtime() {
  599. #ifdef __AVR__
  600. // Protect the access to the variable. Only required for AVR, as
  601. // any 32bit CPU offers atomic access to 32bit variables
  602. bool was_enabled = STEPPER_ISR_ENABLED();
  603. if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
  604. #endif
  605. block_buffer_runtime_us = 0;
  606. #ifdef __AVR__
  607. // Reenable Stepper ISR
  608. if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
  609. #endif
  610. }
  611. #endif
  612. #if ENABLED(AUTOTEMP)
  613. static float autotemp_min, autotemp_max, autotemp_factor;
  614. static bool autotemp_enabled;
  615. static void getHighESpeed();
  616. static void autotemp_M104_M109();
  617. #endif
  618. private:
  619. /**
  620. * Get the index of the next / previous block in the ring buffer
  621. */
  622. static constexpr uint8_t next_block_index(const uint8_t block_index) { return BLOCK_MOD(block_index + 1); }
  623. static constexpr uint8_t prev_block_index(const uint8_t block_index) { return BLOCK_MOD(block_index - 1); }
  624. /**
  625. * Calculate the distance (not time) it takes to accelerate
  626. * from initial_rate to target_rate using the given acceleration:
  627. */
  628. static float estimate_acceleration_distance(const float &initial_rate, const float &target_rate, const float &accel) {
  629. if (accel == 0) return 0; // accel was 0, set acceleration distance to 0
  630. return (sq(target_rate) - sq(initial_rate)) / (accel * 2);
  631. }
  632. /**
  633. * Return the point at which you must start braking (at the rate of -'accel') if
  634. * you start at 'initial_rate', accelerate (until reaching the point), and want to end at
  635. * 'final_rate' after traveling 'distance'.
  636. *
  637. * This is used to compute the intersection point between acceleration and deceleration
  638. * in cases where the "trapezoid" has no plateau (i.e., never reaches maximum speed)
  639. */
  640. static float intersection_distance(const float &initial_rate, const float &final_rate, const float &accel, const float &distance) {
  641. if (accel == 0) return 0; // accel was 0, set intersection distance to 0
  642. return (accel * 2 * distance - sq(initial_rate) + sq(final_rate)) / (accel * 4);
  643. }
  644. /**
  645. * Calculate the maximum allowable speed squared at this point, in order
  646. * to reach 'target_velocity_sqr' using 'acceleration' within a given
  647. * 'distance'.
  648. */
  649. static float max_allowable_speed_sqr(const float &accel, const float &target_velocity_sqr, const float &distance) {
  650. return target_velocity_sqr - 2 * accel * distance;
  651. }
  652. #if ENABLED(S_CURVE_ACCELERATION)
  653. /**
  654. * Calculate the speed reached given initial speed, acceleration and distance
  655. */
  656. static float final_speed(const float &initial_velocity, const float &accel, const float &distance) {
  657. return SQRT(sq(initial_velocity) + 2 * accel * distance);
  658. }
  659. #endif
  660. static void calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor);
  661. static void reverse_pass_kernel(block_t* const current, const block_t * const next);
  662. static void forward_pass_kernel(const block_t * const previous, block_t* const current, uint8_t block_index);
  663. static void reverse_pass();
  664. static void forward_pass();
  665. static void recalculate_trapezoids();
  666. static void recalculate();
  667. #if ENABLED(JUNCTION_DEVIATION)
  668. #if ENABLED(JUNCTION_DEVIATION_INCLUDE_E)
  669. #define JD_AXES XYZE
  670. #else
  671. #define JD_AXES XYZ
  672. #endif
  673. FORCE_INLINE static void normalize_junction_vector(float (&vector)[JD_AXES]) {
  674. float magnitude_sq = 0.0;
  675. for (uint8_t idx = 0; idx < JD_AXES; idx++) if (vector[idx]) magnitude_sq += sq(vector[idx]);
  676. const float inv_magnitude = 1.0 / SQRT(magnitude_sq);
  677. for (uint8_t idx = 0; idx < JD_AXES; idx++) vector[idx] *= inv_magnitude;
  678. }
  679. FORCE_INLINE static float limit_value_by_axis_maximum(const float &max_value, float (&unit_vec)[JD_AXES]) {
  680. float limit_value = max_value;
  681. for (uint8_t idx = 0; idx < JD_AXES; idx++) if (unit_vec[idx]) // Avoid divide by zero
  682. NOMORE(limit_value, ABS(max_acceleration_mm_per_s2[idx] / unit_vec[idx]));
  683. return limit_value;
  684. }
  685. #endif // JUNCTION_DEVIATION
  686. };
  687. #define PLANNER_XY_FEEDRATE() (MIN(planner.max_feedrate_mm_s[X_AXIS], planner.max_feedrate_mm_s[Y_AXIS]))
  688. extern Planner planner;
  689. #endif // PLANNER_H