My Marlin configs for Fabrikator Mini and CTC i3 Pro B
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

planner.h 34KB

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