My Marlin configs for Fabrikator Mini and CTC i3 Pro B
Você não pode selecionar mais de 25 tópicos Os tópicos devem começar com uma letra ou um número, podem incluir traços ('-') e podem ter até 35 caracteres.

planner.h 14KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415
  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 BlockFlag {
  39. // Recalculate trapezoids on entry junction. For optimization.
  40. BLOCK_FLAG_RECALCULATE = _BV(0),
  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_FLAG_NOMINAL_LENGTH = _BV(1),
  45. // Start from a halt at the start of this block, respecting the maximum allowed jerk.
  46. BLOCK_FLAG_START_FROM_FULL_HALT = _BV(2)
  47. };
  48. /**
  49. * struct block_t
  50. *
  51. * A single entry in the planner buffer.
  52. * Tracks linear movement over multiple axes.
  53. *
  54. * The "nominal" values are as-specified by gcode, and
  55. * may never actually be reached due to acceleration limits.
  56. */
  57. typedef struct {
  58. unsigned char active_extruder; // The extruder to move (if E move)
  59. // Fields used by the bresenham algorithm for tracing the line
  60. long steps[NUM_AXIS]; // Step count along each axis
  61. unsigned long step_event_count; // The number of step events required to complete this block
  62. #if ENABLED(MIXING_EXTRUDER)
  63. unsigned long mix_event_count[MIXING_STEPPERS]; // Scaled step_event_count for the mixing steppers
  64. #endif
  65. long accelerate_until, // The index of the step event on which to stop acceleration
  66. decelerate_after, // The index of the step event on which to start decelerating
  67. acceleration_rate; // The acceleration rate used for acceleration calculation
  68. unsigned char direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
  69. // Advance extrusion
  70. #if ENABLED(LIN_ADVANCE)
  71. bool use_advance_lead;
  72. int e_speed_multiplier8; // Factorised by 2^8 to avoid float
  73. #elif ENABLED(ADVANCE)
  74. long advance_rate;
  75. volatile long initial_advance;
  76. volatile long final_advance;
  77. float advance;
  78. #endif
  79. // Fields used by the motion planner to manage acceleration
  80. float nominal_speed, // The nominal speed for this block in mm/sec
  81. entry_speed, // Entry speed at previous-current junction in mm/sec
  82. max_entry_speed, // Maximum allowable junction entry speed in mm/sec
  83. millimeters, // The total travel of this block in mm
  84. acceleration; // acceleration mm/sec^2
  85. uint8_t flag; // Block flags (See BlockFlag enum above)
  86. // Settings for the trapezoid generator
  87. uint32_t nominal_rate, // The nominal step rate for this block in step_events/sec
  88. initial_rate, // The jerk-adjusted step rate at start of block
  89. final_rate, // The minimal rate at exit
  90. acceleration_steps_per_s2; // acceleration steps/sec^2
  91. #if FAN_COUNT > 0
  92. unsigned long fan_speed[FAN_COUNT];
  93. #endif
  94. #if ENABLED(BARICUDA)
  95. unsigned long valve_pressure, e_to_p_pressure;
  96. #endif
  97. volatile char busy;
  98. } block_t;
  99. #define BLOCK_MOD(n) ((n)&(BLOCK_BUFFER_SIZE-1))
  100. class Planner {
  101. public:
  102. /**
  103. * A ring buffer of moves described in steps
  104. */
  105. static block_t block_buffer[BLOCK_BUFFER_SIZE];
  106. static volatile uint8_t block_buffer_head; // Index of the next block to be pushed
  107. static volatile uint8_t block_buffer_tail;
  108. static float max_feedrate_mm_s[NUM_AXIS]; // Max speeds in mm per second
  109. static float axis_steps_per_mm[NUM_AXIS];
  110. static float steps_to_mm[NUM_AXIS];
  111. static unsigned long max_acceleration_steps_per_s2[NUM_AXIS];
  112. static unsigned long max_acceleration_mm_per_s2[NUM_AXIS]; // Use M201 to override by software
  113. static millis_t min_segment_time;
  114. static float min_feedrate_mm_s;
  115. static float acceleration; // Normal acceleration mm/s^2 DEFAULT ACCELERATION for all printing moves. M204 SXXXX
  116. static float retract_acceleration; // Retract acceleration mm/s^2 filament pull-back and push-forward while standing still in the other axes M204 TXXXX
  117. static float travel_acceleration; // Travel acceleration mm/s^2 DEFAULT ACCELERATION for all NON printing moves. M204 MXXXX
  118. static float max_jerk[XYZE]; // The largest speed change requiring no acceleration
  119. static float min_travel_feedrate_mm_s;
  120. #if HAS_ABL
  121. static bool abl_enabled; // Flag that bed leveling is enabled
  122. static matrix_3x3 bed_level_matrix; // Transform to compensate for bed level
  123. #endif
  124. private:
  125. /**
  126. * The current position of the tool in absolute steps
  127. * Recalculated if any axis_steps_per_mm are changed by gcode
  128. */
  129. static long position[NUM_AXIS];
  130. /**
  131. * Speed of previous path line segment
  132. */
  133. static float previous_speed[NUM_AXIS];
  134. /**
  135. * Nominal speed of previous path line segment
  136. */
  137. static float previous_nominal_speed;
  138. /**
  139. * Limit where 64bit math is necessary for acceleration calculation
  140. */
  141. static uint32_t cutoff_long;
  142. #if ENABLED(DISABLE_INACTIVE_EXTRUDER)
  143. /**
  144. * Counters to manage disabling inactive extruders
  145. */
  146. static uint8_t g_uc_extruder_last_move[EXTRUDERS];
  147. #endif // DISABLE_INACTIVE_EXTRUDER
  148. #ifdef XY_FREQUENCY_LIMIT
  149. // Used for the frequency limit
  150. #define MAX_FREQ_TIME long(1000000.0/XY_FREQUENCY_LIMIT)
  151. // Old direction bits. Used for speed calculations
  152. static unsigned char old_direction_bits;
  153. // Segment times (in µs). Used for speed calculations
  154. static long axis_segment_time[2][3];
  155. #endif
  156. public:
  157. /**
  158. * Instance Methods
  159. */
  160. Planner();
  161. void init();
  162. /**
  163. * Static (class) Methods
  164. */
  165. static void reset_acceleration_rates();
  166. static void refresh_positioning();
  167. // Manage fans, paste pressure, etc.
  168. static void check_axes_activity();
  169. /**
  170. * Number of moves currently in the planner
  171. */
  172. static uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail + BLOCK_BUFFER_SIZE); }
  173. static bool is_full() { return (block_buffer_tail == BLOCK_MOD(block_buffer_head + 1)); }
  174. #if PLANNER_LEVELING
  175. #define ARG_X float lx
  176. #define ARG_Y float ly
  177. #define ARG_Z float lz
  178. /**
  179. * Apply leveling to transform a cartesian position
  180. * as it will be given to the planner and steppers.
  181. */
  182. static void apply_leveling(float &lx, float &ly, float &lz);
  183. static void apply_leveling(float logical[XYZ]) { apply_leveling(logical[X_AXIS], logical[Y_AXIS], logical[Z_AXIS]); }
  184. static void unapply_leveling(float logical[XYZ]);
  185. #else
  186. #define ARG_X const float &lx
  187. #define ARG_Y const float &ly
  188. #define ARG_Z const float &lz
  189. #endif
  190. /**
  191. * Planner::_buffer_line
  192. *
  193. * Add a new direct linear movement to the buffer.
  194. *
  195. * Leveling and kinematics should be applied ahead of this.
  196. *
  197. * a,b,c,e - target position in mm or degrees
  198. * fr_mm_s - (target) speed of the move (mm/s)
  199. * extruder - target extruder
  200. */
  201. static void _buffer_line(const float &a, const float &b, const float &c, const float &e, float fr_mm_s, const uint8_t extruder);
  202. static void _set_position_mm(const float &a, const float &b, const float &c, const float &e);
  203. /**
  204. * Add a new linear movement to the buffer.
  205. * The target is NOT translated to delta/scara
  206. *
  207. * Leveling will be applied to input on cartesians.
  208. * Kinematic machines should call buffer_line_kinematic (for leveled moves).
  209. * (Cartesians may also call buffer_line_kinematic.)
  210. *
  211. * lx,ly,lz,e - target position in mm or degrees
  212. * fr_mm_s - (target) speed of the move (mm/s)
  213. * extruder - target extruder
  214. */
  215. static FORCE_INLINE void buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, const uint8_t extruder) {
  216. #if PLANNER_LEVELING && IS_CARTESIAN
  217. apply_leveling(lx, ly, lz);
  218. #endif
  219. _buffer_line(lx, ly, lz, e, fr_mm_s, extruder);
  220. }
  221. /**
  222. * Add a new linear movement to the buffer.
  223. * The target is cartesian, it's translated to delta/scara if
  224. * needed.
  225. *
  226. * target - x,y,z,e CARTESIAN target in mm
  227. * fr_mm_s - (target) speed of the move (mm/s)
  228. * extruder - target extruder
  229. */
  230. static FORCE_INLINE void buffer_line_kinematic(const float target[XYZE], float fr_mm_s, const uint8_t extruder) {
  231. #if PLANNER_LEVELING
  232. float pos[XYZ] = { target[X_AXIS], target[Y_AXIS], target[Z_AXIS] };
  233. apply_leveling(pos);
  234. #else
  235. const float * const pos = target;
  236. #endif
  237. #if IS_KINEMATIC
  238. inverse_kinematics(pos);
  239. _buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], target[E_AXIS], fr_mm_s, extruder);
  240. #else
  241. _buffer_line(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], target[E_AXIS], fr_mm_s, extruder);
  242. #endif
  243. }
  244. /**
  245. * Set the planner.position and individual stepper positions.
  246. * Used by G92, G28, G29, and other procedures.
  247. *
  248. * Multiplies by axis_steps_per_mm[] and does necessary conversion
  249. * for COREXY / COREXZ / COREYZ to set the corresponding stepper positions.
  250. *
  251. * Clears previous speed values.
  252. */
  253. static FORCE_INLINE void set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) {
  254. #if PLANNER_LEVELING && IS_CARTESIAN
  255. apply_leveling(lx, ly, lz);
  256. #endif
  257. _set_position_mm(lx, ly, lz, e);
  258. }
  259. static void set_position_mm_kinematic(const float position[NUM_AXIS]);
  260. static void set_position_mm(const AxisEnum axis, const float& v);
  261. static FORCE_INLINE void set_z_position_mm(const float& z) { set_position_mm(Z_AXIS, z); }
  262. static FORCE_INLINE void set_e_position_mm(const float& e) { set_position_mm(E_AXIS, e); }
  263. /**
  264. * Sync from the stepper positions. (e.g., after an interrupted move)
  265. */
  266. static void sync_from_steppers();
  267. /**
  268. * Does the buffer have any blocks queued?
  269. */
  270. static bool blocks_queued() { return (block_buffer_head != block_buffer_tail); }
  271. /**
  272. * "Discards" the block and "releases" the memory.
  273. * Called when the current block is no longer needed.
  274. */
  275. static void discard_current_block() {
  276. if (blocks_queued())
  277. block_buffer_tail = BLOCK_MOD(block_buffer_tail + 1);
  278. }
  279. /**
  280. * The current block. NULL if the buffer is empty.
  281. * This also marks the block as busy.
  282. */
  283. static block_t* get_current_block() {
  284. if (blocks_queued()) {
  285. block_t* block = &block_buffer[block_buffer_tail];
  286. block->busy = true;
  287. return block;
  288. }
  289. else
  290. return NULL;
  291. }
  292. #if ENABLED(AUTOTEMP)
  293. static float autotemp_max;
  294. static float autotemp_min;
  295. static float autotemp_factor;
  296. static bool autotemp_enabled;
  297. static void getHighESpeed();
  298. static void autotemp_M109();
  299. #endif
  300. private:
  301. /**
  302. * Get the index of the next / previous block in the ring buffer
  303. */
  304. static int8_t next_block_index(int8_t block_index) { return BLOCK_MOD(block_index + 1); }
  305. static int8_t prev_block_index(int8_t block_index) { return BLOCK_MOD(block_index - 1); }
  306. /**
  307. * Calculate the distance (not time) it takes to accelerate
  308. * from initial_rate to target_rate using the given acceleration:
  309. */
  310. static float estimate_acceleration_distance(float initial_rate, float target_rate, float accel) {
  311. if (accel == 0) return 0; // accel was 0, set acceleration distance to 0
  312. return (sq(target_rate) - sq(initial_rate)) / (accel * 2);
  313. }
  314. /**
  315. * Return the point at which you must start braking (at the rate of -'acceleration') if
  316. * you start at 'initial_rate', accelerate (until reaching the point), and want to end at
  317. * 'final_rate' after traveling 'distance'.
  318. *
  319. * This is used to compute the intersection point between acceleration and deceleration
  320. * in cases where the "trapezoid" has no plateau (i.e., never reaches maximum speed)
  321. */
  322. static float intersection_distance(float initial_rate, float final_rate, float accel, float distance) {
  323. if (accel == 0) return 0; // accel was 0, set intersection distance to 0
  324. return (accel * 2 * distance - sq(initial_rate) + sq(final_rate)) / (accel * 4);
  325. }
  326. /**
  327. * Calculate the maximum allowable speed at this point, in order
  328. * to reach 'target_velocity' using 'acceleration' within a given
  329. * 'distance'.
  330. */
  331. static float max_allowable_speed(float accel, float target_velocity, float distance) {
  332. return sqrt(sq(target_velocity) - 2 * accel * distance);
  333. }
  334. static void calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor);
  335. static void reverse_pass_kernel(block_t* const current, const block_t *next);
  336. static void forward_pass_kernel(const block_t *previous, block_t* const current);
  337. static void reverse_pass();
  338. static void forward_pass();
  339. static void recalculate_trapezoids();
  340. static void recalculate();
  341. };
  342. extern Planner planner;
  343. #endif // PLANNER_H