My Marlin configs for Fabrikator Mini and CTC i3 Pro B
您最多选择25个主题 主题必须以字母或数字开头,可以包含连字符 (-),并且长度不得超过35个字符

planner.h 18KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535
  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 "types.h"
  33. #include "enum.h"
  34. #include "Marlin.h"
  35. #if HAS_ABL
  36. #include "vector_3.h"
  37. #endif
  38. enum BlockFlagBit {
  39. // Recalculate trapezoids on entry junction. For optimization.
  40. BLOCK_BIT_RECALCULATE,
  41. // Nominal speed always reached.
  42. // i.e., The segment is long enough, so the nominal speed is reachable if accelerating
  43. // from a safe speed (in consideration of jerking from zero speed).
  44. BLOCK_BIT_NOMINAL_LENGTH,
  45. // Start from a halt at the start of this block, respecting the maximum allowed jerk.
  46. BLOCK_BIT_START_FROM_FULL_HALT,
  47. // The block is busy
  48. BLOCK_BIT_BUSY
  49. };
  50. enum BlockFlag {
  51. BLOCK_FLAG_RECALCULATE = _BV(BLOCK_BIT_RECALCULATE),
  52. BLOCK_FLAG_NOMINAL_LENGTH = _BV(BLOCK_BIT_NOMINAL_LENGTH),
  53. BLOCK_FLAG_START_FROM_FULL_HALT = _BV(BLOCK_BIT_START_FROM_FULL_HALT),
  54. BLOCK_FLAG_BUSY = _BV(BLOCK_BIT_BUSY)
  55. };
  56. /**
  57. * struct block_t
  58. *
  59. * A single entry in the planner buffer.
  60. * Tracks linear movement over multiple axes.
  61. *
  62. * The "nominal" values are as-specified by gcode, and
  63. * may never actually be reached due to acceleration limits.
  64. */
  65. typedef struct {
  66. uint8_t flag; // Block flags (See BlockFlag enum above)
  67. unsigned char active_extruder; // The extruder to move (if E move)
  68. // Fields used by the Bresenham algorithm for tracing the line
  69. int32_t steps[NUM_AXIS]; // Step count along each axis
  70. uint32_t step_event_count; // The number of step events required to complete this block
  71. #if ENABLED(MIXING_EXTRUDER)
  72. uint32_t mix_event_count[MIXING_STEPPERS]; // Scaled step_event_count for the mixing steppers
  73. #endif
  74. int32_t accelerate_until, // The index of the step event on which to stop acceleration
  75. decelerate_after, // The index of the step event on which to start decelerating
  76. acceleration_rate; // The acceleration rate used for acceleration calculation
  77. uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
  78. // Advance extrusion
  79. #if ENABLED(LIN_ADVANCE)
  80. bool use_advance_lead;
  81. uint32_t abs_adv_steps_multiplier8; // Factorised by 2^8 to avoid float
  82. #endif
  83. // Fields used by the motion planner to manage acceleration
  84. float nominal_speed, // The nominal speed for this block in mm/sec
  85. entry_speed, // Entry speed at previous-current junction in mm/sec
  86. max_entry_speed, // Maximum allowable junction entry speed in mm/sec
  87. millimeters, // The total travel of this block in mm
  88. acceleration; // acceleration mm/sec^2
  89. // Settings for the trapezoid generator
  90. uint32_t nominal_rate, // The nominal step rate for this block in step_events/sec
  91. initial_rate, // The jerk-adjusted step rate at start of block
  92. final_rate, // The minimal rate at exit
  93. acceleration_steps_per_s2; // acceleration steps/sec^2
  94. #if FAN_COUNT > 0
  95. uint16_t fan_speed[FAN_COUNT];
  96. #endif
  97. #if ENABLED(BARICUDA)
  98. uint8_t valve_pressure, e_to_p_pressure;
  99. #endif
  100. uint32_t segment_time_us;
  101. } block_t;
  102. #define BLOCK_MOD(n) ((n)&(BLOCK_BUFFER_SIZE-1))
  103. class Planner {
  104. public:
  105. /**
  106. * A ring buffer of moves described in steps
  107. */
  108. static block_t block_buffer[BLOCK_BUFFER_SIZE];
  109. static volatile uint8_t block_buffer_head, // Index of the next block to be pushed
  110. block_buffer_tail;
  111. #if ENABLED(DISTINCT_E_FACTORS)
  112. static uint8_t last_extruder; // Respond to extruder change
  113. #endif
  114. static int16_t flow_percentage[EXTRUDERS]; // Extrusion factor for each extruder
  115. static float e_factor[EXTRUDERS], // The flow percentage and volumetric multiplier combine to scale E movement
  116. filament_size[EXTRUDERS], // diameter of filament (in millimeters), typically around 1.75 or 2.85, 0 disables the volumetric calculations for the extruder
  117. volumetric_area_nominal, // Nominal cross-sectional area
  118. volumetric_multiplier[EXTRUDERS]; // Reciprocal of cross-sectional area of filament (in mm^2). Pre-calculated to reduce computation in the planner
  119. // May be auto-adjusted by a filament width sensor
  120. static float max_feedrate_mm_s[XYZE_N], // Max speeds in mm per second
  121. axis_steps_per_mm[XYZE_N],
  122. steps_to_mm[XYZE_N];
  123. static uint32_t max_acceleration_steps_per_s2[XYZE_N],
  124. max_acceleration_mm_per_s2[XYZE_N]; // Use M201 to override
  125. static uint32_t min_segment_time_us; // Use 'M205 B<µs>' to override
  126. static float min_feedrate_mm_s,
  127. acceleration, // Normal acceleration mm/s^2 DEFAULT ACCELERATION for all printing moves. M204 SXXXX
  128. retract_acceleration, // Retract acceleration mm/s^2 filament pull-back and push-forward while standing still in the other axes M204 TXXXX
  129. travel_acceleration, // Travel acceleration mm/s^2 DEFAULT ACCELERATION for all NON printing moves. M204 MXXXX
  130. max_jerk[XYZE], // The largest speed change requiring no acceleration
  131. min_travel_feedrate_mm_s;
  132. #if HAS_LEVELING
  133. static bool leveling_active; // Flag that bed leveling is enabled
  134. #if ABL_PLANAR
  135. static matrix_3x3 bed_level_matrix; // Transform to compensate for bed level
  136. #endif
  137. #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
  138. static float z_fade_height, inverse_z_fade_height;
  139. #endif
  140. #endif
  141. #if ENABLED(LIN_ADVANCE)
  142. static float extruder_advance_k, advance_ed_ratio;
  143. #endif
  144. private:
  145. /**
  146. * The current position of the tool in absolute steps
  147. * Recalculated if any axis_steps_per_mm are changed by gcode
  148. */
  149. static long position[NUM_AXIS];
  150. /**
  151. * Speed of previous path line segment
  152. */
  153. static float previous_speed[NUM_AXIS];
  154. /**
  155. * Nominal speed of previous path line segment
  156. */
  157. static float previous_nominal_speed;
  158. /**
  159. * Limit where 64bit math is necessary for acceleration calculation
  160. */
  161. static uint32_t cutoff_long;
  162. #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
  163. static float last_fade_z;
  164. #endif
  165. #if ENABLED(DISABLE_INACTIVE_EXTRUDER)
  166. /**
  167. * Counters to manage disabling inactive extruders
  168. */
  169. static uint8_t g_uc_extruder_last_move[EXTRUDERS];
  170. #endif // DISABLE_INACTIVE_EXTRUDER
  171. #ifdef XY_FREQUENCY_LIMIT
  172. // Used for the frequency limit
  173. #define MAX_FREQ_TIME_US (uint32_t)(1000000.0 / XY_FREQUENCY_LIMIT)
  174. // Old direction bits. Used for speed calculations
  175. static unsigned char old_direction_bits;
  176. // Segment times (in µs). Used for speed calculations
  177. static uint32_t axis_segment_time_us[2][3];
  178. #endif
  179. #if ENABLED(LIN_ADVANCE)
  180. static float position_float[NUM_AXIS];
  181. #endif
  182. #if ENABLED(ULTRA_LCD)
  183. volatile static uint32_t block_buffer_runtime_us; //Theoretical block buffer runtime in µs
  184. #endif
  185. public:
  186. /**
  187. * Instance Methods
  188. */
  189. Planner();
  190. void init();
  191. /**
  192. * Static (class) Methods
  193. */
  194. static void reset_acceleration_rates();
  195. static void refresh_positioning();
  196. FORCE_INLINE static void refresh_e_factor(const uint8_t e) {
  197. e_factor[e] = volumetric_multiplier[e] * flow_percentage[e] * 0.01;
  198. }
  199. // Manage fans, paste pressure, etc.
  200. static void check_axes_activity();
  201. static void calculate_volumetric_multipliers();
  202. /**
  203. * Number of moves currently in the planner
  204. */
  205. static uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail + BLOCK_BUFFER_SIZE); }
  206. static bool is_full() { return (block_buffer_tail == BLOCK_MOD(block_buffer_head + 1)); }
  207. #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
  208. /**
  209. * Get the Z leveling fade factor based on the given Z height,
  210. * re-calculating only when needed.
  211. *
  212. * Returns 1.0 if planner.z_fade_height is 0.0.
  213. * Returns 0.0 if Z is past the specified 'Fade Height'.
  214. */
  215. inline static float fade_scaling_factor_for_z(const float &rz) {
  216. static float z_fade_factor = 1.0;
  217. if (z_fade_height) {
  218. if (rz >= z_fade_height) return 0.0;
  219. if (last_fade_z != rz) {
  220. last_fade_z = rz;
  221. z_fade_factor = 1.0 - rz * inverse_z_fade_height;
  222. }
  223. return z_fade_factor;
  224. }
  225. return 1.0;
  226. }
  227. FORCE_INLINE static void force_fade_recalc() { last_fade_z = -999.999; }
  228. FORCE_INLINE static void set_z_fade_height(const float &zfh) {
  229. z_fade_height = zfh > 0 ? zfh : 0;
  230. inverse_z_fade_height = RECIPROCAL(z_fade_height);
  231. force_fade_recalc();
  232. }
  233. FORCE_INLINE static bool leveling_active_at_z(const float &rz) {
  234. return !z_fade_height || rz < z_fade_height;
  235. }
  236. #else
  237. FORCE_INLINE static float fade_scaling_factor_for_z(const float &rz) {
  238. UNUSED(rz);
  239. return 1.0;
  240. }
  241. FORCE_INLINE static bool leveling_active_at_z(const float &rz) { UNUSED(rz); return true; }
  242. #endif
  243. #if PLANNER_LEVELING
  244. #define ARG_X float rx
  245. #define ARG_Y float ry
  246. #define ARG_Z float rz
  247. /**
  248. * Apply leveling to transform a cartesian position
  249. * as it will be given to the planner and steppers.
  250. */
  251. static void apply_leveling(float &rx, float &ry, float &rz);
  252. static void apply_leveling(float raw[XYZ]) { apply_leveling(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); }
  253. static void unapply_leveling(float raw[XYZ]);
  254. #else
  255. #define ARG_X const float &rx
  256. #define ARG_Y const float &ry
  257. #define ARG_Z const float &rz
  258. #endif
  259. /**
  260. * Planner::_buffer_line
  261. *
  262. * Add a new direct linear movement to the buffer.
  263. *
  264. * Leveling and kinematics should be applied ahead of this.
  265. *
  266. * a,b,c,e - target position in mm or degrees
  267. * fr_mm_s - (target) speed of the move (mm/s)
  268. * extruder - target extruder
  269. */
  270. static void _buffer_line(const float &a, const float &b, const float &c, const float &e, float fr_mm_s, const uint8_t extruder);
  271. static void _set_position_mm(const float &a, const float &b, const float &c, const float &e);
  272. /**
  273. * Add a new linear movement to the buffer.
  274. * The target is NOT translated to delta/scara
  275. *
  276. * Leveling will be applied to input on cartesians.
  277. * Kinematic machines should call buffer_line_kinematic (for leveled moves).
  278. * (Cartesians may also call buffer_line_kinematic.)
  279. *
  280. * rx,ry,rz,e - target position in mm or degrees
  281. * fr_mm_s - (target) speed of the move (mm/s)
  282. * extruder - target extruder
  283. */
  284. static FORCE_INLINE void buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, const float &fr_mm_s, const uint8_t extruder) {
  285. #if PLANNER_LEVELING && IS_CARTESIAN
  286. apply_leveling(rx, ry, rz);
  287. #endif
  288. _buffer_line(rx, ry, rz, e, fr_mm_s, extruder);
  289. }
  290. /**
  291. * Add a new linear movement to the buffer.
  292. * The target is cartesian, it's translated to delta/scara if
  293. * needed.
  294. *
  295. * cart - x,y,z,e CARTESIAN target in mm
  296. * fr_mm_s - (target) speed of the move (mm/s)
  297. * extruder - target extruder
  298. */
  299. static FORCE_INLINE void buffer_line_kinematic(const float cart[XYZE], const float &fr_mm_s, const uint8_t extruder) {
  300. #if PLANNER_LEVELING
  301. float raw[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] };
  302. apply_leveling(raw);
  303. #else
  304. const float * const raw = cart;
  305. #endif
  306. #if IS_KINEMATIC
  307. inverse_kinematics(raw);
  308. _buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS], fr_mm_s, extruder);
  309. #else
  310. _buffer_line(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS], fr_mm_s, extruder);
  311. #endif
  312. }
  313. /**
  314. * Set the planner.position and individual stepper positions.
  315. * Used by G92, G28, G29, and other procedures.
  316. *
  317. * Multiplies by axis_steps_per_mm[] and does necessary conversion
  318. * for COREXY / COREXZ / COREYZ to set the corresponding stepper positions.
  319. *
  320. * Clears previous speed values.
  321. */
  322. static FORCE_INLINE void set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) {
  323. #if PLANNER_LEVELING && IS_CARTESIAN
  324. apply_leveling(rx, ry, rz);
  325. #endif
  326. _set_position_mm(rx, ry, rz, e);
  327. }
  328. static void set_position_mm_kinematic(const float position[NUM_AXIS]);
  329. static void set_position_mm(const AxisEnum axis, const float &v);
  330. static FORCE_INLINE void set_z_position_mm(const float &z) { set_position_mm(Z_AXIS, z); }
  331. static FORCE_INLINE void set_e_position_mm(const float &e) { set_position_mm(AxisEnum(E_AXIS), e); }
  332. /**
  333. * Sync from the stepper positions. (e.g., after an interrupted move)
  334. */
  335. static void sync_from_steppers();
  336. /**
  337. * Does the buffer have any blocks queued?
  338. */
  339. static bool blocks_queued() { return (block_buffer_head != block_buffer_tail); }
  340. /**
  341. * "Discards" the block and "releases" the memory.
  342. * Called when the current block is no longer needed.
  343. */
  344. static void discard_current_block() {
  345. if (blocks_queued())
  346. block_buffer_tail = BLOCK_MOD(block_buffer_tail + 1);
  347. }
  348. /**
  349. * The current block. NULL if the buffer is empty.
  350. * This also marks the block as busy.
  351. */
  352. static block_t* get_current_block() {
  353. if (blocks_queued()) {
  354. block_t* block = &block_buffer[block_buffer_tail];
  355. #if ENABLED(ULTRA_LCD)
  356. 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.
  357. #endif
  358. SBI(block->flag, BLOCK_BIT_BUSY);
  359. return block;
  360. }
  361. else {
  362. #if ENABLED(ULTRA_LCD)
  363. clear_block_buffer_runtime(); // paranoia. Buffer is empty now - so reset accumulated time to zero.
  364. #endif
  365. return NULL;
  366. }
  367. }
  368. #if ENABLED(ULTRA_LCD)
  369. static uint16_t block_buffer_runtime() {
  370. CRITICAL_SECTION_START
  371. millis_t bbru = block_buffer_runtime_us;
  372. CRITICAL_SECTION_END
  373. // To translate µs to ms a division by 1000 would be required.
  374. // We introduce 2.4% error here by dividing by 1024.
  375. // Doesn't matter because block_buffer_runtime_us is already too small an estimation.
  376. bbru >>= 10;
  377. // limit to about a minute.
  378. NOMORE(bbru, 0xFFFFul);
  379. return bbru;
  380. }
  381. static void clear_block_buffer_runtime(){
  382. CRITICAL_SECTION_START
  383. block_buffer_runtime_us = 0;
  384. CRITICAL_SECTION_END
  385. }
  386. #endif
  387. #if ENABLED(AUTOTEMP)
  388. static float autotemp_min, autotemp_max, autotemp_factor;
  389. static bool autotemp_enabled;
  390. static void getHighESpeed();
  391. static void autotemp_M104_M109();
  392. #endif
  393. private:
  394. /**
  395. * Get the index of the next / previous block in the ring buffer
  396. */
  397. static int8_t next_block_index(int8_t block_index) { return BLOCK_MOD(block_index + 1); }
  398. static int8_t prev_block_index(int8_t block_index) { return BLOCK_MOD(block_index - 1); }
  399. /**
  400. * Calculate the distance (not time) it takes to accelerate
  401. * from initial_rate to target_rate using the given acceleration:
  402. */
  403. static float estimate_acceleration_distance(const float &initial_rate, const float &target_rate, const float &accel) {
  404. if (accel == 0) return 0; // accel was 0, set acceleration distance to 0
  405. return (sq(target_rate) - sq(initial_rate)) / (accel * 2);
  406. }
  407. /**
  408. * Return the point at which you must start braking (at the rate of -'acceleration') if
  409. * you start at 'initial_rate', accelerate (until reaching the point), and want to end at
  410. * 'final_rate' after traveling 'distance'.
  411. *
  412. * This is used to compute the intersection point between acceleration and deceleration
  413. * in cases where the "trapezoid" has no plateau (i.e., never reaches maximum speed)
  414. */
  415. static float intersection_distance(const float &initial_rate, const float &final_rate, const float &accel, const float &distance) {
  416. if (accel == 0) return 0; // accel was 0, set intersection distance to 0
  417. return (accel * 2 * distance - sq(initial_rate) + sq(final_rate)) / (accel * 4);
  418. }
  419. /**
  420. * Calculate the maximum allowable speed at this point, in order
  421. * to reach 'target_velocity' using 'acceleration' within a given
  422. * 'distance'.
  423. */
  424. static float max_allowable_speed(const float &accel, const float &target_velocity, const float &distance) {
  425. return SQRT(sq(target_velocity) - 2 * accel * distance);
  426. }
  427. static void calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor);
  428. static void reverse_pass_kernel(block_t* const current, const block_t *next);
  429. static void forward_pass_kernel(const block_t *previous, block_t* const current);
  430. static void reverse_pass();
  431. static void forward_pass();
  432. static void recalculate_trapezoids();
  433. static void recalculate();
  434. };
  435. #define PLANNER_XY_FEEDRATE() (min(planner.max_feedrate_mm_s[X_AXIS], planner.max_feedrate_mm_s[Y_AXIS]))
  436. extern Planner planner;
  437. #endif // PLANNER_H