My Marlin configs for Fabrikator Mini and CTC i3 Pro B
Vous ne pouvez pas sélectionner plus de 25 sujets Les noms de sujets doivent commencer par une lettre ou un nombre, peuvent contenir des tirets ('-') et peuvent comporter jusqu'à 35 caractères.

planner.cpp 49KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348
  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.cpp
  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. * The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis.
  31. *
  32. *
  33. * Reasoning behind the mathematics in this module (in the key of 'Mathematica'):
  34. *
  35. * s == speed, a == acceleration, t == time, d == distance
  36. *
  37. * Basic definitions:
  38. * Speed[s_, a_, t_] := s + (a*t)
  39. * Travel[s_, a_, t_] := Integrate[Speed[s, a, t], t]
  40. *
  41. * Distance to reach a specific speed with a constant acceleration:
  42. * Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, d, t]
  43. * d -> (m^2 - s^2)/(2 a) --> estimate_acceleration_distance()
  44. *
  45. * Speed after a given distance of travel with constant acceleration:
  46. * Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, m, t]
  47. * m -> Sqrt[2 a d + s^2]
  48. *
  49. * DestinationSpeed[s_, a_, d_] := Sqrt[2 a d + s^2]
  50. *
  51. * When to start braking (di) to reach a specified destination speed (s2) after accelerating
  52. * from initial speed s1 without ever stopping at a plateau:
  53. * Solve[{DestinationSpeed[s1, a, di] == DestinationSpeed[s2, a, d - di]}, di]
  54. * di -> (2 a d - s1^2 + s2^2)/(4 a) --> intersection_distance()
  55. *
  56. * IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a)
  57. *
  58. */
  59. #include "planner.h"
  60. #include "stepper.h"
  61. #include "temperature.h"
  62. #include "ultralcd.h"
  63. #include "language.h"
  64. #include "Marlin.h"
  65. #if ENABLED(MESH_BED_LEVELING)
  66. #include "mesh_bed_leveling.h"
  67. #endif
  68. Planner planner;
  69. // public:
  70. /**
  71. * A ring buffer of moves described in steps
  72. */
  73. block_t Planner::block_buffer[BLOCK_BUFFER_SIZE];
  74. volatile uint8_t Planner::block_buffer_head = 0; // Index of the next block to be pushed
  75. volatile uint8_t Planner::block_buffer_tail = 0;
  76. float Planner::max_feedrate_mm_s[NUM_AXIS], // Max speeds in mm per second
  77. Planner::axis_steps_per_mm[NUM_AXIS],
  78. Planner::steps_to_mm[NUM_AXIS];
  79. uint32_t Planner::max_acceleration_steps_per_s2[NUM_AXIS],
  80. Planner::max_acceleration_mm_per_s2[NUM_AXIS]; // Use M201 to override by software
  81. millis_t Planner::min_segment_time;
  82. float Planner::min_feedrate_mm_s,
  83. Planner::acceleration, // Normal acceleration mm/s^2 DEFAULT ACCELERATION for all printing moves. M204 SXXXX
  84. Planner::retract_acceleration, // Retract acceleration mm/s^2 filament pull-back and push-forward while standing still in the other axes M204 TXXXX
  85. Planner::travel_acceleration, // Travel acceleration mm/s^2 DEFAULT ACCELERATION for all NON printing moves. M204 MXXXX
  86. Planner::max_jerk[XYZE], // The largest speed change requiring no acceleration
  87. Planner::min_travel_feedrate_mm_s;
  88. #if HAS_ABL
  89. bool Planner::abl_enabled = false; // Flag that auto bed leveling is enabled
  90. #endif
  91. #if ABL_PLANAR
  92. matrix_3x3 Planner::bed_level_matrix; // Transform to compensate for bed level
  93. #endif
  94. #if ENABLED(AUTOTEMP)
  95. float Planner::autotemp_max = 250,
  96. Planner::autotemp_min = 210,
  97. Planner::autotemp_factor = 0.1;
  98. bool Planner::autotemp_enabled = false;
  99. #endif
  100. // private:
  101. long Planner::position[NUM_AXIS] = { 0 };
  102. uint32_t Planner::cutoff_long;
  103. float Planner::previous_speed[NUM_AXIS],
  104. Planner::previous_nominal_speed;
  105. #if ENABLED(DISABLE_INACTIVE_EXTRUDER)
  106. uint8_t Planner::g_uc_extruder_last_move[EXTRUDERS] = { 0 };
  107. #endif // DISABLE_INACTIVE_EXTRUDER
  108. #ifdef XY_FREQUENCY_LIMIT
  109. // Old direction bits. Used for speed calculations
  110. unsigned char Planner::old_direction_bits = 0;
  111. // Segment times (in µs). Used for speed calculations
  112. long Planner::axis_segment_time[2][3] = { {MAX_FREQ_TIME + 1, 0, 0}, {MAX_FREQ_TIME + 1, 0, 0} };
  113. #endif
  114. /**
  115. * Class and Instance Methods
  116. */
  117. Planner::Planner() { init(); }
  118. void Planner::init() {
  119. block_buffer_head = block_buffer_tail = 0;
  120. ZERO(position);
  121. ZERO(previous_speed);
  122. previous_nominal_speed = 0.0;
  123. #if ABL_PLANAR
  124. bed_level_matrix.set_to_identity();
  125. #endif
  126. }
  127. #define MINIMAL_STEP_RATE 120
  128. /**
  129. * Calculate trapezoid parameters, multiplying the entry- and exit-speeds
  130. * by the provided factors.
  131. */
  132. void Planner::calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor) {
  133. uint32_t initial_rate = ceil(block->nominal_rate * entry_factor),
  134. final_rate = ceil(block->nominal_rate * exit_factor); // (steps per second)
  135. // Limit minimal step rate (Otherwise the timer will overflow.)
  136. NOLESS(initial_rate, MINIMAL_STEP_RATE);
  137. NOLESS(final_rate, MINIMAL_STEP_RATE);
  138. int32_t accel = block->acceleration_steps_per_s2,
  139. accelerate_steps = ceil(estimate_acceleration_distance(initial_rate, block->nominal_rate, accel)),
  140. decelerate_steps = floor(estimate_acceleration_distance(block->nominal_rate, final_rate, -accel)),
  141. plateau_steps = block->step_event_count - accelerate_steps - decelerate_steps;
  142. // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
  143. // have to use intersection_distance() to calculate when to abort accel and start braking
  144. // in order to reach the final_rate exactly at the end of this block.
  145. if (plateau_steps < 0) {
  146. accelerate_steps = ceil(intersection_distance(initial_rate, final_rate, accel, block->step_event_count));
  147. NOLESS(accelerate_steps, 0); // Check limits due to numerical round-off
  148. accelerate_steps = min((uint32_t)accelerate_steps, block->step_event_count);//(We can cast here to unsigned, because the above line ensures that we are above zero)
  149. plateau_steps = 0;
  150. }
  151. // block->accelerate_until = accelerate_steps;
  152. // block->decelerate_after = accelerate_steps+plateau_steps;
  153. CRITICAL_SECTION_START; // Fill variables used by the stepper in a critical section
  154. if (!block->busy) { // Don't update variables if block is busy.
  155. block->accelerate_until = accelerate_steps;
  156. block->decelerate_after = accelerate_steps + plateau_steps;
  157. block->initial_rate = initial_rate;
  158. block->final_rate = final_rate;
  159. #if ENABLED(ADVANCE)
  160. block->initial_advance = block->advance * sq(entry_factor);
  161. block->final_advance = block->advance * sq(exit_factor);
  162. #endif
  163. }
  164. CRITICAL_SECTION_END;
  165. }
  166. // "Junction jerk" in this context is the immediate change in speed at the junction of two blocks.
  167. // This method will calculate the junction jerk as the euclidean distance between the nominal
  168. // velocities of the respective blocks.
  169. //inline float junction_jerk(block_t *before, block_t *after) {
  170. // return sqrt(
  171. // pow((before->speed_x-after->speed_x), 2)+pow((before->speed_y-after->speed_y), 2));
  172. //}
  173. // The kernel called by recalculate() when scanning the plan from last to first entry.
  174. void Planner::reverse_pass_kernel(block_t* const current, const block_t *next) {
  175. if (!current || !next) return;
  176. // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
  177. // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
  178. // check for maximum allowable speed reductions to ensure maximum possible planned speed.
  179. float max_entry_speed = current->max_entry_speed;
  180. if (current->entry_speed != max_entry_speed) {
  181. // If nominal length true, max junction speed is guaranteed to be reached. Only compute
  182. // for max allowable speed if block is decelerating and nominal length is false.
  183. current->entry_speed = ((current->flag & BLOCK_FLAG_NOMINAL_LENGTH) || max_entry_speed <= next->entry_speed)
  184. ? max_entry_speed
  185. : min(max_entry_speed, max_allowable_speed(-current->acceleration, next->entry_speed, current->millimeters));
  186. current->flag |= BLOCK_FLAG_RECALCULATE;
  187. }
  188. }
  189. /**
  190. * recalculate() needs to go over the current plan twice.
  191. * Once in reverse and once forward. This implements the reverse pass.
  192. */
  193. void Planner::reverse_pass() {
  194. if (movesplanned() > 3) {
  195. block_t* block[3] = { NULL, NULL, NULL };
  196. // Make a local copy of block_buffer_tail, because the interrupt can alter it
  197. // Is a critical section REALLY needed for a single byte change?
  198. //CRITICAL_SECTION_START;
  199. uint8_t tail = block_buffer_tail;
  200. //CRITICAL_SECTION_END
  201. uint8_t b = BLOCK_MOD(block_buffer_head - 3);
  202. while (b != tail) {
  203. if (block[0] && (block[0]->flag & BLOCK_FLAG_START_FROM_FULL_HALT)) break;
  204. b = prev_block_index(b);
  205. block[2] = block[1];
  206. block[1] = block[0];
  207. block[0] = &block_buffer[b];
  208. reverse_pass_kernel(block[1], block[2]);
  209. }
  210. }
  211. }
  212. // The kernel called by recalculate() when scanning the plan from first to last entry.
  213. void Planner::forward_pass_kernel(const block_t* previous, block_t* const current) {
  214. if (!previous) return;
  215. // If the previous block is an acceleration block, but it is not long enough to complete the
  216. // full speed change within the block, we need to adjust the entry speed accordingly. Entry
  217. // speeds have already been reset, maximized, and reverse planned by reverse planner.
  218. // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
  219. if (!(previous->flag & BLOCK_FLAG_NOMINAL_LENGTH)) {
  220. if (previous->entry_speed < current->entry_speed) {
  221. float entry_speed = min(current->entry_speed,
  222. max_allowable_speed(-previous->acceleration, previous->entry_speed, previous->millimeters));
  223. // Check for junction speed change
  224. if (current->entry_speed != entry_speed) {
  225. current->entry_speed = entry_speed;
  226. current->flag |= BLOCK_FLAG_RECALCULATE;
  227. }
  228. }
  229. }
  230. }
  231. /**
  232. * recalculate() needs to go over the current plan twice.
  233. * Once in reverse and once forward. This implements the forward pass.
  234. */
  235. void Planner::forward_pass() {
  236. block_t* block[3] = { NULL, NULL, NULL };
  237. for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) {
  238. block[0] = block[1];
  239. block[1] = block[2];
  240. block[2] = &block_buffer[b];
  241. forward_pass_kernel(block[0], block[1]);
  242. }
  243. forward_pass_kernel(block[1], block[2]);
  244. }
  245. /**
  246. * Recalculate the trapezoid speed profiles for all blocks in the plan
  247. * according to the entry_factor for each junction. Must be called by
  248. * recalculate() after updating the blocks.
  249. */
  250. void Planner::recalculate_trapezoids() {
  251. int8_t block_index = block_buffer_tail;
  252. block_t *current, *next = NULL;
  253. while (block_index != block_buffer_head) {
  254. current = next;
  255. next = &block_buffer[block_index];
  256. if (current) {
  257. // Recalculate if current block entry or exit junction speed has changed.
  258. if ((current->flag & BLOCK_FLAG_RECALCULATE) || (next->flag & BLOCK_FLAG_RECALCULATE)) {
  259. // NOTE: Entry and exit factors always > 0 by all previous logic operations.
  260. float nom = current->nominal_speed;
  261. calculate_trapezoid_for_block(current, current->entry_speed / nom, next->entry_speed / nom);
  262. current->flag &= ~BLOCK_FLAG_RECALCULATE; // Reset current only to ensure next trapezoid is computed
  263. }
  264. }
  265. block_index = next_block_index(block_index);
  266. }
  267. // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
  268. if (next) {
  269. float nom = next->nominal_speed;
  270. calculate_trapezoid_for_block(next, next->entry_speed / nom, (MINIMUM_PLANNER_SPEED) / nom);
  271. next->flag &= ~BLOCK_FLAG_RECALCULATE;
  272. }
  273. }
  274. /*
  275. * Recalculate the motion plan according to the following algorithm:
  276. *
  277. * 1. Go over every block in reverse order...
  278. *
  279. * Calculate a junction speed reduction (block_t.entry_factor) so:
  280. *
  281. * a. The junction jerk is within the set limit, and
  282. *
  283. * b. No speed reduction within one block requires faster
  284. * deceleration than the one, true constant acceleration.
  285. *
  286. * 2. Go over every block in chronological order...
  287. *
  288. * Dial down junction speed reduction values if:
  289. * a. The speed increase within one block would require faster
  290. * acceleration than the one, true constant acceleration.
  291. *
  292. * After that, all blocks will have an entry_factor allowing all speed changes to
  293. * be performed using only the one, true constant acceleration, and where no junction
  294. * jerk is jerkier than the set limit, Jerky. Finally it will:
  295. *
  296. * 3. Recalculate "trapezoids" for all blocks.
  297. */
  298. void Planner::recalculate() {
  299. reverse_pass();
  300. forward_pass();
  301. recalculate_trapezoids();
  302. }
  303. #if ENABLED(AUTOTEMP)
  304. void Planner::getHighESpeed() {
  305. static float oldt = 0;
  306. if (!autotemp_enabled) return;
  307. if (thermalManager.degTargetHotend(0) + 2 < autotemp_min) return; // probably temperature set to zero.
  308. float high = 0.0;
  309. for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) {
  310. block_t* block = &block_buffer[b];
  311. if (block->steps[X_AXIS] || block->steps[Y_AXIS] || block->steps[Z_AXIS]) {
  312. float se = (float)block->steps[E_AXIS] / block->step_event_count * block->nominal_speed; // mm/sec;
  313. NOLESS(high, se);
  314. }
  315. }
  316. float t = autotemp_min + high * autotemp_factor;
  317. t = constrain(t, autotemp_min, autotemp_max);
  318. if (oldt > t) {
  319. t *= (1 - (AUTOTEMP_OLDWEIGHT));
  320. t += (AUTOTEMP_OLDWEIGHT) * oldt;
  321. }
  322. oldt = t;
  323. thermalManager.setTargetHotend(t, 0);
  324. }
  325. #endif //AUTOTEMP
  326. /**
  327. * Maintain fans, paste extruder pressure,
  328. */
  329. void Planner::check_axes_activity() {
  330. unsigned char axis_active[NUM_AXIS] = { 0 },
  331. tail_fan_speed[FAN_COUNT];
  332. #if FAN_COUNT > 0
  333. for (uint8_t i = 0; i < FAN_COUNT; i++) tail_fan_speed[i] = fanSpeeds[i];
  334. #endif
  335. #if ENABLED(BARICUDA)
  336. #if HAS_HEATER_1
  337. unsigned char tail_valve_pressure = baricuda_valve_pressure;
  338. #endif
  339. #if HAS_HEATER_2
  340. unsigned char tail_e_to_p_pressure = baricuda_e_to_p_pressure;
  341. #endif
  342. #endif
  343. if (blocks_queued()) {
  344. #if FAN_COUNT > 0
  345. for (uint8_t i = 0; i < FAN_COUNT; i++) tail_fan_speed[i] = block_buffer[block_buffer_tail].fan_speed[i];
  346. #endif
  347. block_t* block;
  348. #if ENABLED(BARICUDA)
  349. block = &block_buffer[block_buffer_tail];
  350. #if HAS_HEATER_1
  351. tail_valve_pressure = block->valve_pressure;
  352. #endif
  353. #if HAS_HEATER_2
  354. tail_e_to_p_pressure = block->e_to_p_pressure;
  355. #endif
  356. #endif
  357. for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) {
  358. block = &block_buffer[b];
  359. LOOP_XYZE(i) if (block->steps[i]) axis_active[i]++;
  360. }
  361. }
  362. #if ENABLED(DISABLE_X)
  363. if (!axis_active[X_AXIS]) disable_x();
  364. #endif
  365. #if ENABLED(DISABLE_Y)
  366. if (!axis_active[Y_AXIS]) disable_y();
  367. #endif
  368. #if ENABLED(DISABLE_Z)
  369. if (!axis_active[Z_AXIS]) disable_z();
  370. #endif
  371. #if ENABLED(DISABLE_E)
  372. if (!axis_active[E_AXIS]) {
  373. disable_e0();
  374. disable_e1();
  375. disable_e2();
  376. disable_e3();
  377. }
  378. #endif
  379. #if FAN_COUNT > 0
  380. #if defined(FAN_MIN_PWM)
  381. #define CALC_FAN_SPEED(f) (tail_fan_speed[f] ? ( FAN_MIN_PWM + (tail_fan_speed[f] * (255 - FAN_MIN_PWM)) / 255 ) : 0)
  382. #else
  383. #define CALC_FAN_SPEED(f) tail_fan_speed[f]
  384. #endif
  385. #ifdef FAN_KICKSTART_TIME
  386. static millis_t fan_kick_end[FAN_COUNT] = { 0 };
  387. #define KICKSTART_FAN(f) \
  388. if (tail_fan_speed[f]) { \
  389. millis_t ms = millis(); \
  390. if (fan_kick_end[f] == 0) { \
  391. fan_kick_end[f] = ms + FAN_KICKSTART_TIME; \
  392. tail_fan_speed[f] = 255; \
  393. } else { \
  394. if (PENDING(ms, fan_kick_end[f])) { \
  395. tail_fan_speed[f] = 255; \
  396. } \
  397. } \
  398. } else { \
  399. fan_kick_end[f] = 0; \
  400. }
  401. #if HAS_FAN0
  402. KICKSTART_FAN(0);
  403. #endif
  404. #if HAS_FAN1
  405. KICKSTART_FAN(1);
  406. #endif
  407. #if HAS_FAN2
  408. KICKSTART_FAN(2);
  409. #endif
  410. #endif //FAN_KICKSTART_TIME
  411. #if ENABLED(FAN_SOFT_PWM)
  412. #if HAS_FAN0
  413. thermalManager.fanSpeedSoftPwm[0] = CALC_FAN_SPEED(0);
  414. #endif
  415. #if HAS_FAN1
  416. thermalManager.fanSpeedSoftPwm[1] = CALC_FAN_SPEED(1);
  417. #endif
  418. #if HAS_FAN2
  419. thermalManager.fanSpeedSoftPwm[2] = CALC_FAN_SPEED(2);
  420. #endif
  421. #else
  422. #if HAS_FAN0
  423. analogWrite(FAN_PIN, CALC_FAN_SPEED(0));
  424. #endif
  425. #if HAS_FAN1
  426. analogWrite(FAN1_PIN, CALC_FAN_SPEED(1));
  427. #endif
  428. #if HAS_FAN2
  429. analogWrite(FAN2_PIN, CALC_FAN_SPEED(2));
  430. #endif
  431. #endif
  432. #endif // FAN_COUNT > 0
  433. #if ENABLED(AUTOTEMP)
  434. getHighESpeed();
  435. #endif
  436. #if ENABLED(BARICUDA)
  437. #if HAS_HEATER_1
  438. analogWrite(HEATER_1_PIN, tail_valve_pressure);
  439. #endif
  440. #if HAS_HEATER_2
  441. analogWrite(HEATER_2_PIN, tail_e_to_p_pressure);
  442. #endif
  443. #endif
  444. }
  445. #if PLANNER_LEVELING
  446. /**
  447. * lx, ly, lz - logical (cartesian, not delta) positions in mm
  448. */
  449. void Planner::apply_leveling(float &lx, float &ly, float &lz) {
  450. #if HAS_ABL
  451. if (!abl_enabled) return;
  452. #endif
  453. #if ENABLED(MESH_BED_LEVELING)
  454. if (mbl.active())
  455. lz += mbl.get_z(RAW_X_POSITION(lx), RAW_Y_POSITION(ly));
  456. #elif ABL_PLANAR
  457. float dx = RAW_X_POSITION(lx) - (X_TILT_FULCRUM),
  458. dy = RAW_Y_POSITION(ly) - (Y_TILT_FULCRUM),
  459. dz = RAW_Z_POSITION(lz);
  460. apply_rotation_xyz(bed_level_matrix, dx, dy, dz);
  461. lx = LOGICAL_X_POSITION(dx + X_TILT_FULCRUM);
  462. ly = LOGICAL_Y_POSITION(dy + Y_TILT_FULCRUM);
  463. lz = LOGICAL_Z_POSITION(dz);
  464. #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
  465. float tmp[XYZ] = { lx, ly, 0 };
  466. lz += bilinear_z_offset(tmp);
  467. #endif
  468. }
  469. void Planner::unapply_leveling(float logical[XYZ]) {
  470. #if HAS_ABL
  471. if (!abl_enabled) return;
  472. #endif
  473. #if ENABLED(MESH_BED_LEVELING)
  474. if (mbl.active())
  475. logical[Z_AXIS] -= mbl.get_z(RAW_X_POSITION(logical[X_AXIS]), RAW_Y_POSITION(logical[Y_AXIS]));
  476. #elif ABL_PLANAR
  477. matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix);
  478. float dx = RAW_X_POSITION(logical[X_AXIS]) - (X_TILT_FULCRUM),
  479. dy = RAW_Y_POSITION(logical[Y_AXIS]) - (Y_TILT_FULCRUM),
  480. dz = RAW_Z_POSITION(logical[Z_AXIS]);
  481. apply_rotation_xyz(inverse, dx, dy, dz);
  482. logical[X_AXIS] = LOGICAL_X_POSITION(dx + X_TILT_FULCRUM);
  483. logical[Y_AXIS] = LOGICAL_Y_POSITION(dy + Y_TILT_FULCRUM);
  484. logical[Z_AXIS] = LOGICAL_Z_POSITION(dz);
  485. #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
  486. logical[Z_AXIS] -= bilinear_z_offset(logical);
  487. #endif
  488. }
  489. #endif // PLANNER_LEVELING
  490. /**
  491. * Planner::_buffer_line
  492. *
  493. * Add a new linear movement to the buffer.
  494. *
  495. * Leveling and kinematics should be applied ahead of calling this.
  496. *
  497. * a,b,c,e - target positions in mm or degrees
  498. * fr_mm_s - (target) speed of the move
  499. * extruder - target extruder
  500. */
  501. void Planner::_buffer_line(const float &a, const float &b, const float &c, const float &e, float fr_mm_s, const uint8_t extruder) {
  502. // Calculate the buffer head after we push this byte
  503. int next_buffer_head = next_block_index(block_buffer_head);
  504. // If the buffer is full: good! That means we are well ahead of the robot.
  505. // Rest here until there is room in the buffer.
  506. while (block_buffer_tail == next_buffer_head) idle();
  507. // The target position of the tool in absolute steps
  508. // Calculate target position in absolute steps
  509. //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
  510. long target[XYZE] = {
  511. lround(a * axis_steps_per_mm[X_AXIS]),
  512. lround(b * axis_steps_per_mm[Y_AXIS]),
  513. lround(c * axis_steps_per_mm[Z_AXIS]),
  514. lround(e * axis_steps_per_mm[E_AXIS])
  515. };
  516. long da = target[X_AXIS] - position[X_AXIS],
  517. db = target[Y_AXIS] - position[Y_AXIS],
  518. dc = target[Z_AXIS] - position[Z_AXIS];
  519. /*
  520. SERIAL_ECHOPAIR(" Planner FR:", fr_mm_s);
  521. SERIAL_CHAR(' ');
  522. #if IS_KINEMATIC
  523. SERIAL_ECHOPAIR("A:", a);
  524. SERIAL_ECHOPAIR(" (", da);
  525. SERIAL_ECHOPAIR(") B:", b);
  526. #else
  527. SERIAL_ECHOPAIR("X:", a);
  528. SERIAL_ECHOPAIR(" (", da);
  529. SERIAL_ECHOPAIR(") Y:", b);
  530. #endif
  531. SERIAL_ECHOPAIR(" (", db);
  532. #if ENABLED(DELTA)
  533. SERIAL_ECHOPAIR(") C:", c);
  534. #else
  535. SERIAL_ECHOPAIR(") Z:", c);
  536. #endif
  537. SERIAL_ECHOPAIR(" (", dc);
  538. SERIAL_CHAR(')');
  539. SERIAL_EOL;
  540. //*/
  541. // DRYRUN ignores all temperature constraints and assures that the extruder is instantly satisfied
  542. if (DEBUGGING(DRYRUN)) position[E_AXIS] = target[E_AXIS];
  543. long de = target[E_AXIS] - position[E_AXIS];
  544. #if ENABLED(PREVENT_COLD_EXTRUSION)
  545. if (de) {
  546. if (thermalManager.tooColdToExtrude(extruder)) {
  547. position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
  548. de = 0; // no difference
  549. SERIAL_ECHO_START;
  550. SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
  551. }
  552. #if ENABLED(PREVENT_LENGTHY_EXTRUDE)
  553. if (labs(de) > axis_steps_per_mm[E_AXIS] * (EXTRUDE_MAXLENGTH)) {
  554. position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
  555. de = 0; // no difference
  556. SERIAL_ECHO_START;
  557. SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
  558. }
  559. #endif
  560. }
  561. #endif
  562. // Prepare to set up new block
  563. block_t* block = &block_buffer[block_buffer_head];
  564. // Mark block as not busy (Not executed by the stepper interrupt)
  565. block->busy = false;
  566. // Number of steps for each axis
  567. #if ENABLED(COREXY)
  568. // corexy planning
  569. // these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
  570. block->steps[A_AXIS] = labs(da + db);
  571. block->steps[B_AXIS] = labs(da - db);
  572. block->steps[Z_AXIS] = labs(dc);
  573. #elif ENABLED(COREXZ)
  574. // corexz planning
  575. block->steps[A_AXIS] = labs(da + dc);
  576. block->steps[Y_AXIS] = labs(db);
  577. block->steps[C_AXIS] = labs(da - dc);
  578. #elif ENABLED(COREYZ)
  579. // coreyz planning
  580. block->steps[X_AXIS] = labs(da);
  581. block->steps[B_AXIS] = labs(db + dc);
  582. block->steps[C_AXIS] = labs(db - dc);
  583. #else
  584. // default non-h-bot planning
  585. block->steps[X_AXIS] = labs(da);
  586. block->steps[Y_AXIS] = labs(db);
  587. block->steps[Z_AXIS] = labs(dc);
  588. #endif
  589. block->steps[E_AXIS] = labs(de) * volumetric_multiplier[extruder] * flow_percentage[extruder] * 0.01 + 0.5;
  590. block->step_event_count = MAX4(block->steps[X_AXIS], block->steps[Y_AXIS], block->steps[Z_AXIS], block->steps[E_AXIS]);
  591. // Bail if this is a zero-length block
  592. if (block->step_event_count < MIN_STEPS_PER_SEGMENT) return;
  593. // Clear the block flags
  594. block->flag = 0;
  595. // For a mixing extruder, get a magnified step_event_count for each
  596. #if ENABLED(MIXING_EXTRUDER)
  597. for (uint8_t i = 0; i < MIXING_STEPPERS; i++)
  598. block->mix_event_count[i] = UNEAR_ZERO(mixing_factor[i]) ? 0 : block->step_event_count / mixing_factor[i];
  599. #endif
  600. #if FAN_COUNT > 0
  601. for (uint8_t i = 0; i < FAN_COUNT; i++) block->fan_speed[i] = fanSpeeds[i];
  602. #endif
  603. #if ENABLED(BARICUDA)
  604. block->valve_pressure = baricuda_valve_pressure;
  605. block->e_to_p_pressure = baricuda_e_to_p_pressure;
  606. #endif
  607. // Compute direction bit-mask for this block
  608. uint8_t dm = 0;
  609. #if ENABLED(COREXY)
  610. if (da < 0) SBI(dm, X_HEAD); // Save the real Extruder (head) direction in X Axis
  611. if (db < 0) SBI(dm, Y_HEAD); // ...and Y
  612. if (dc < 0) SBI(dm, Z_AXIS);
  613. if (da + db < 0) SBI(dm, A_AXIS); // Motor A direction
  614. if (da - db < 0) SBI(dm, B_AXIS); // Motor B direction
  615. #elif ENABLED(COREXZ)
  616. if (da < 0) SBI(dm, X_HEAD); // Save the real Extruder (head) direction in X Axis
  617. if (db < 0) SBI(dm, Y_AXIS);
  618. if (dc < 0) SBI(dm, Z_HEAD); // ...and Z
  619. if (da + dc < 0) SBI(dm, A_AXIS); // Motor A direction
  620. if (da - dc < 0) SBI(dm, C_AXIS); // Motor C direction
  621. #elif ENABLED(COREYZ)
  622. if (da < 0) SBI(dm, X_AXIS);
  623. if (db < 0) SBI(dm, Y_HEAD); // Save the real Extruder (head) direction in Y Axis
  624. if (dc < 0) SBI(dm, Z_HEAD); // ...and Z
  625. if (db + dc < 0) SBI(dm, B_AXIS); // Motor B direction
  626. if (db - dc < 0) SBI(dm, C_AXIS); // Motor C direction
  627. #else
  628. if (da < 0) SBI(dm, X_AXIS);
  629. if (db < 0) SBI(dm, Y_AXIS);
  630. if (dc < 0) SBI(dm, Z_AXIS);
  631. #endif
  632. if (de < 0) SBI(dm, E_AXIS);
  633. block->direction_bits = dm;
  634. block->active_extruder = extruder;
  635. //enable active axes
  636. #if ENABLED(COREXY)
  637. if (block->steps[A_AXIS] || block->steps[B_AXIS]) {
  638. enable_x();
  639. enable_y();
  640. }
  641. #if DISABLED(Z_LATE_ENABLE)
  642. if (block->steps[Z_AXIS]) enable_z();
  643. #endif
  644. #elif ENABLED(COREXZ)
  645. if (block->steps[A_AXIS] || block->steps[C_AXIS]) {
  646. enable_x();
  647. enable_z();
  648. }
  649. if (block->steps[Y_AXIS]) enable_y();
  650. #else
  651. if (block->steps[X_AXIS]) enable_x();
  652. if (block->steps[Y_AXIS]) enable_y();
  653. #if DISABLED(Z_LATE_ENABLE)
  654. if (block->steps[Z_AXIS]) enable_z();
  655. #endif
  656. #endif
  657. // Enable extruder(s)
  658. if (block->steps[E_AXIS]) {
  659. #if ENABLED(DISABLE_INACTIVE_EXTRUDER) // Enable only the selected extruder
  660. for (int i = 0; i < EXTRUDERS; i++)
  661. if (g_uc_extruder_last_move[i] > 0) g_uc_extruder_last_move[i]--;
  662. switch(extruder) {
  663. case 0:
  664. enable_e0();
  665. #if ENABLED(DUAL_X_CARRIAGE)
  666. if (extruder_duplication_enabled) {
  667. enable_e1();
  668. g_uc_extruder_last_move[1] = (BLOCK_BUFFER_SIZE) * 2;
  669. }
  670. #endif
  671. g_uc_extruder_last_move[0] = (BLOCK_BUFFER_SIZE) * 2;
  672. #if EXTRUDERS > 1
  673. if (g_uc_extruder_last_move[1] == 0) disable_e1();
  674. #if EXTRUDERS > 2
  675. if (g_uc_extruder_last_move[2] == 0) disable_e2();
  676. #if EXTRUDERS > 3
  677. if (g_uc_extruder_last_move[3] == 0) disable_e3();
  678. #endif
  679. #endif
  680. #endif
  681. break;
  682. #if EXTRUDERS > 1
  683. case 1:
  684. enable_e1();
  685. g_uc_extruder_last_move[1] = (BLOCK_BUFFER_SIZE) * 2;
  686. if (g_uc_extruder_last_move[0] == 0) disable_e0();
  687. #if EXTRUDERS > 2
  688. if (g_uc_extruder_last_move[2] == 0) disable_e2();
  689. #if EXTRUDERS > 3
  690. if (g_uc_extruder_last_move[3] == 0) disable_e3();
  691. #endif
  692. #endif
  693. break;
  694. #if EXTRUDERS > 2
  695. case 2:
  696. enable_e2();
  697. g_uc_extruder_last_move[2] = (BLOCK_BUFFER_SIZE) * 2;
  698. if (g_uc_extruder_last_move[0] == 0) disable_e0();
  699. if (g_uc_extruder_last_move[1] == 0) disable_e1();
  700. #if EXTRUDERS > 3
  701. if (g_uc_extruder_last_move[3] == 0) disable_e3();
  702. #endif
  703. break;
  704. #if EXTRUDERS > 3
  705. case 3:
  706. enable_e3();
  707. g_uc_extruder_last_move[3] = (BLOCK_BUFFER_SIZE) * 2;
  708. if (g_uc_extruder_last_move[0] == 0) disable_e0();
  709. if (g_uc_extruder_last_move[1] == 0) disable_e1();
  710. if (g_uc_extruder_last_move[2] == 0) disable_e2();
  711. break;
  712. #endif // EXTRUDERS > 3
  713. #endif // EXTRUDERS > 2
  714. #endif // EXTRUDERS > 1
  715. }
  716. #else
  717. enable_e0();
  718. enable_e1();
  719. enable_e2();
  720. enable_e3();
  721. #endif
  722. }
  723. if (block->steps[E_AXIS])
  724. NOLESS(fr_mm_s, min_feedrate_mm_s);
  725. else
  726. NOLESS(fr_mm_s, min_travel_feedrate_mm_s);
  727. /**
  728. * This part of the code calculates the total length of the movement.
  729. * For cartesian bots, the X_AXIS is the real X movement and same for Y_AXIS.
  730. * But for corexy bots, that is not true. The "X_AXIS" and "Y_AXIS" motors (that should be named to A_AXIS
  731. * and B_AXIS) cannot be used for X and Y length, because A=X+Y and B=X-Y.
  732. * So we need to create other 2 "AXIS", named X_HEAD and Y_HEAD, meaning the real displacement of the Head.
  733. * Having the real displacement of the head, we can calculate the total movement length and apply the desired speed.
  734. */
  735. #if ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ)
  736. float delta_mm[7];
  737. #if ENABLED(COREXY)
  738. delta_mm[X_HEAD] = da * steps_to_mm[A_AXIS];
  739. delta_mm[Y_HEAD] = db * steps_to_mm[B_AXIS];
  740. delta_mm[Z_AXIS] = dc * steps_to_mm[Z_AXIS];
  741. delta_mm[A_AXIS] = (da + db) * steps_to_mm[A_AXIS];
  742. delta_mm[B_AXIS] = (da - db) * steps_to_mm[B_AXIS];
  743. #elif ENABLED(COREXZ)
  744. delta_mm[X_HEAD] = da * steps_to_mm[A_AXIS];
  745. delta_mm[Y_AXIS] = db * steps_to_mm[Y_AXIS];
  746. delta_mm[Z_HEAD] = dc * steps_to_mm[C_AXIS];
  747. delta_mm[A_AXIS] = (da + dc) * steps_to_mm[A_AXIS];
  748. delta_mm[C_AXIS] = (da - dc) * steps_to_mm[C_AXIS];
  749. #elif ENABLED(COREYZ)
  750. delta_mm[X_AXIS] = da * steps_to_mm[X_AXIS];
  751. delta_mm[Y_HEAD] = db * steps_to_mm[B_AXIS];
  752. delta_mm[Z_HEAD] = dc * steps_to_mm[C_AXIS];
  753. delta_mm[B_AXIS] = (db + dc) * steps_to_mm[B_AXIS];
  754. delta_mm[C_AXIS] = (db - dc) * steps_to_mm[C_AXIS];
  755. #endif
  756. #else
  757. float delta_mm[4];
  758. delta_mm[X_AXIS] = da * steps_to_mm[X_AXIS];
  759. delta_mm[Y_AXIS] = db * steps_to_mm[Y_AXIS];
  760. delta_mm[Z_AXIS] = dc * steps_to_mm[Z_AXIS];
  761. #endif
  762. delta_mm[E_AXIS] = 0.01 * (de * steps_to_mm[E_AXIS]) * volumetric_multiplier[extruder] * flow_percentage[extruder];
  763. if (block->steps[X_AXIS] < MIN_STEPS_PER_SEGMENT && block->steps[Y_AXIS] < MIN_STEPS_PER_SEGMENT && block->steps[Z_AXIS] < MIN_STEPS_PER_SEGMENT) {
  764. block->millimeters = fabs(delta_mm[E_AXIS]);
  765. }
  766. else {
  767. block->millimeters = sqrt(
  768. #if ENABLED(COREXY)
  769. sq(delta_mm[X_HEAD]) + sq(delta_mm[Y_HEAD]) + sq(delta_mm[Z_AXIS])
  770. #elif ENABLED(COREXZ)
  771. sq(delta_mm[X_HEAD]) + sq(delta_mm[Y_AXIS]) + sq(delta_mm[Z_HEAD])
  772. #elif ENABLED(COREYZ)
  773. sq(delta_mm[X_AXIS]) + sq(delta_mm[Y_HEAD]) + sq(delta_mm[Z_HEAD])
  774. #else
  775. sq(delta_mm[X_AXIS]) + sq(delta_mm[Y_AXIS]) + sq(delta_mm[Z_AXIS])
  776. #endif
  777. );
  778. }
  779. float inverse_millimeters = 1.0 / block->millimeters; // Inverse millimeters to remove multiple divides
  780. // Calculate moves/second for this move. No divide by zero due to previous checks.
  781. float inverse_mm_s = fr_mm_s * inverse_millimeters;
  782. int moves_queued = movesplanned();
  783. // Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
  784. #if ENABLED(OLD_SLOWDOWN) || ENABLED(SLOWDOWN)
  785. bool mq = moves_queued > 1 && moves_queued < (BLOCK_BUFFER_SIZE) / 2;
  786. #if ENABLED(OLD_SLOWDOWN)
  787. if (mq) fr_mm_s *= 2.0 * moves_queued / (BLOCK_BUFFER_SIZE);
  788. #endif
  789. #if ENABLED(SLOWDOWN)
  790. // segment time im micro seconds
  791. unsigned long segment_time = lround(1000000.0/inverse_mm_s);
  792. if (mq) {
  793. if (segment_time < min_segment_time) {
  794. // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
  795. inverse_mm_s = 1000000.0 / (segment_time + lround(2 * (min_segment_time - segment_time) / moves_queued));
  796. #ifdef XY_FREQUENCY_LIMIT
  797. segment_time = lround(1000000.0 / inverse_mm_s);
  798. #endif
  799. }
  800. }
  801. #endif
  802. #endif
  803. block->nominal_speed = block->millimeters * inverse_mm_s; // (mm/sec) Always > 0
  804. block->nominal_rate = ceil(block->step_event_count * inverse_mm_s); // (step/sec) Always > 0
  805. #if ENABLED(FILAMENT_WIDTH_SENSOR)
  806. static float filwidth_e_count = 0, filwidth_delay_dist = 0;
  807. //FMM update ring buffer used for delay with filament measurements
  808. if (extruder == FILAMENT_SENSOR_EXTRUDER_NUM && filwidth_delay_index[1] >= 0) { //only for extruder with filament sensor and if ring buffer is initialized
  809. const int MMD_CM = MAX_MEASUREMENT_DELAY + 1, MMD_MM = MMD_CM * 10;
  810. // increment counters with next move in e axis
  811. filwidth_e_count += delta_mm[E_AXIS];
  812. filwidth_delay_dist += delta_mm[E_AXIS];
  813. // Only get new measurements on forward E movement
  814. if (filwidth_e_count > 0.0001) {
  815. // Loop the delay distance counter (modulus by the mm length)
  816. while (filwidth_delay_dist >= MMD_MM) filwidth_delay_dist -= MMD_MM;
  817. // Convert into an index into the measurement array
  818. filwidth_delay_index[0] = (int)(filwidth_delay_dist * 0.1 + 0.0001);
  819. // If the index has changed (must have gone forward)...
  820. if (filwidth_delay_index[0] != filwidth_delay_index[1]) {
  821. filwidth_e_count = 0; // Reset the E movement counter
  822. int8_t meas_sample = thermalManager.widthFil_to_size_ratio() - 100; // Subtract 100 to reduce magnitude - to store in a signed char
  823. do {
  824. filwidth_delay_index[1] = (filwidth_delay_index[1] + 1) % MMD_CM; // The next unused slot
  825. measurement_delay[filwidth_delay_index[1]] = meas_sample; // Store the measurement
  826. } while (filwidth_delay_index[0] != filwidth_delay_index[1]); // More slots to fill?
  827. }
  828. }
  829. }
  830. #endif
  831. // Calculate and limit speed in mm/sec for each axis
  832. float current_speed[NUM_AXIS], speed_factor = 1.0; // factor <1 decreases speed
  833. LOOP_XYZE(i) {
  834. float cs = fabs(current_speed[i] = delta_mm[i] * inverse_mm_s);
  835. if (cs > max_feedrate_mm_s[i]) NOMORE(speed_factor, max_feedrate_mm_s[i] / cs);
  836. }
  837. // Max segment time in µs.
  838. #ifdef XY_FREQUENCY_LIMIT
  839. // Check and limit the xy direction change frequency
  840. unsigned char direction_change = block->direction_bits ^ old_direction_bits;
  841. old_direction_bits = block->direction_bits;
  842. segment_time = lround((float)segment_time / speed_factor);
  843. long xs0 = axis_segment_time[X_AXIS][0],
  844. xs1 = axis_segment_time[X_AXIS][1],
  845. xs2 = axis_segment_time[X_AXIS][2],
  846. ys0 = axis_segment_time[Y_AXIS][0],
  847. ys1 = axis_segment_time[Y_AXIS][1],
  848. ys2 = axis_segment_time[Y_AXIS][2];
  849. if (TEST(direction_change, X_AXIS)) {
  850. xs2 = axis_segment_time[X_AXIS][2] = xs1;
  851. xs1 = axis_segment_time[X_AXIS][1] = xs0;
  852. xs0 = 0;
  853. }
  854. xs0 = axis_segment_time[X_AXIS][0] = xs0 + segment_time;
  855. if (TEST(direction_change, Y_AXIS)) {
  856. ys2 = axis_segment_time[Y_AXIS][2] = axis_segment_time[Y_AXIS][1];
  857. ys1 = axis_segment_time[Y_AXIS][1] = axis_segment_time[Y_AXIS][0];
  858. ys0 = 0;
  859. }
  860. ys0 = axis_segment_time[Y_AXIS][0] = ys0 + segment_time;
  861. long max_x_segment_time = MAX3(xs0, xs1, xs2),
  862. max_y_segment_time = MAX3(ys0, ys1, ys2),
  863. min_xy_segment_time = min(max_x_segment_time, max_y_segment_time);
  864. if (min_xy_segment_time < MAX_FREQ_TIME) {
  865. float low_sf = speed_factor * min_xy_segment_time / (MAX_FREQ_TIME);
  866. NOMORE(speed_factor, low_sf);
  867. }
  868. #endif // XY_FREQUENCY_LIMIT
  869. // Correct the speed
  870. if (speed_factor < 1.0) {
  871. LOOP_XYZE(i) current_speed[i] *= speed_factor;
  872. block->nominal_speed *= speed_factor;
  873. block->nominal_rate *= speed_factor;
  874. }
  875. // Compute and limit the acceleration rate for the trapezoid generator.
  876. float steps_per_mm = block->step_event_count * inverse_millimeters;
  877. uint32_t accel;
  878. if (!block->steps[X_AXIS] && !block->steps[Y_AXIS] && !block->steps[Z_AXIS]) {
  879. // convert to: acceleration steps/sec^2
  880. accel = ceil(retract_acceleration * steps_per_mm);
  881. }
  882. else {
  883. #define LIMIT_ACCEL_LONG(AXIS) do{ \
  884. if (block->steps[AXIS] && max_acceleration_steps_per_s2[AXIS] < accel) { \
  885. const uint32_t comp = max_acceleration_steps_per_s2[AXIS] * block->step_event_count; \
  886. if (accel * block->steps[AXIS] > comp) accel = comp / block->steps[AXIS]; \
  887. } \
  888. }while(0)
  889. #define LIMIT_ACCEL_FLOAT(AXIS) do{ \
  890. if (block->steps[AXIS] && max_acceleration_steps_per_s2[AXIS] < accel) { \
  891. const float comp = (float)max_acceleration_steps_per_s2[AXIS] * (float)block->step_event_count; \
  892. if ((float)accel * (float)block->steps[AXIS] > comp) accel = comp / (float)block->steps[AXIS]; \
  893. } \
  894. }while(0)
  895. // Start with print or travel acceleration
  896. accel = ceil((block->steps[E_AXIS] ? acceleration : travel_acceleration) * steps_per_mm);
  897. // Limit acceleration per axis
  898. if (block->step_event_count <= cutoff_long){
  899. LIMIT_ACCEL_LONG(X_AXIS);
  900. LIMIT_ACCEL_LONG(Y_AXIS);
  901. LIMIT_ACCEL_LONG(Z_AXIS);
  902. LIMIT_ACCEL_LONG(E_AXIS);
  903. } else {
  904. LIMIT_ACCEL_FLOAT(X_AXIS);
  905. LIMIT_ACCEL_FLOAT(Y_AXIS);
  906. LIMIT_ACCEL_FLOAT(Z_AXIS);
  907. LIMIT_ACCEL_FLOAT(E_AXIS);
  908. }
  909. }
  910. block->acceleration_steps_per_s2 = accel;
  911. block->acceleration = accel / steps_per_mm;
  912. block->acceleration_rate = (long)(accel * 16777216.0 / ((F_CPU) * 0.125)); // * 8.388608
  913. // Initial limit on the segment entry velocity
  914. float vmax_junction;
  915. #if 0 // Use old jerk for now
  916. float junction_deviation = 0.1;
  917. // Compute path unit vector
  918. double unit_vec[XYZ] = {
  919. delta_mm[X_AXIS] * inverse_millimeters,
  920. delta_mm[Y_AXIS] * inverse_millimeters,
  921. delta_mm[Z_AXIS] * inverse_millimeters
  922. };
  923. /*
  924. Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
  925. Let a circle be tangent to both previous and current path line segments, where the junction
  926. deviation is defined as the distance from the junction to the closest edge of the circle,
  927. collinear with the circle center.
  928. The circular segment joining the two paths represents the path of centripetal acceleration.
  929. Solve for max velocity based on max acceleration about the radius of the circle, defined
  930. indirectly by junction deviation.
  931. This may be also viewed as path width or max_jerk in the previous grbl version. This approach
  932. does not actually deviate from path, but used as a robust way to compute cornering speeds, as
  933. it takes into account the nonlinearities of both the junction angle and junction velocity.
  934. */
  935. vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
  936. // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
  937. if (block_buffer_head != block_buffer_tail && previous_nominal_speed > 0.0) {
  938. // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
  939. // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
  940. float cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
  941. - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
  942. - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
  943. // Skip and use default max junction speed for 0 degree acute junction.
  944. if (cos_theta < 0.95) {
  945. vmax_junction = min(previous_nominal_speed, block->nominal_speed);
  946. // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
  947. if (cos_theta > -0.95) {
  948. // Compute maximum junction velocity based on maximum acceleration and junction deviation
  949. float sin_theta_d2 = sqrt(0.5 * (1.0 - cos_theta)); // Trig half angle identity. Always positive.
  950. NOMORE(vmax_junction, sqrt(block->acceleration * junction_deviation * sin_theta_d2 / (1.0 - sin_theta_d2)));
  951. }
  952. }
  953. }
  954. #endif
  955. /**
  956. * Adapted from Prusa MKS firmware
  957. *
  958. * Start with a safe speed (from which the machine may halt to stop immediately).
  959. */
  960. // Exit speed limited by a jerk to full halt of a previous last segment
  961. static float previous_safe_speed;
  962. float safe_speed = block->nominal_speed;
  963. bool limited = false;
  964. LOOP_XYZE(i) {
  965. float jerk = fabs(current_speed[i]);
  966. if (jerk > max_jerk[i]) {
  967. // The actual jerk is lower if it has been limited by the XY jerk.
  968. if (limited) {
  969. // Spare one division by a following gymnastics:
  970. // Instead of jerk *= safe_speed / block->nominal_speed,
  971. // multiply max_jerk[i] by the divisor.
  972. jerk *= safe_speed;
  973. float mjerk = max_jerk[i] * block->nominal_speed;
  974. if (jerk > mjerk) safe_speed *= mjerk / jerk;
  975. }
  976. else {
  977. safe_speed = max_jerk[i];
  978. limited = true;
  979. }
  980. }
  981. }
  982. if (moves_queued > 1 && previous_nominal_speed > 0.0001) {
  983. // Estimate a maximum velocity allowed at a joint of two successive segments.
  984. // If this maximum velocity allowed is lower than the minimum of the entry / exit safe velocities,
  985. // then the machine is not coasting anymore and the safe entry / exit velocities shall be used.
  986. // The junction velocity will be shared between successive segments. Limit the junction velocity to their minimum.
  987. bool prev_speed_larger = previous_nominal_speed > block->nominal_speed;
  988. float smaller_speed_factor = prev_speed_larger ? (block->nominal_speed / previous_nominal_speed) : (previous_nominal_speed / block->nominal_speed);
  989. // Pick the smaller of the nominal speeds. Higher speed shall not be achieved at the junction during coasting.
  990. vmax_junction = prev_speed_larger ? block->nominal_speed : previous_nominal_speed;
  991. // Factor to multiply the previous / current nominal velocities to get componentwise limited velocities.
  992. float v_factor = 1.f;
  993. limited = false;
  994. // Now limit the jerk in all axes.
  995. LOOP_XYZE(axis) {
  996. // Limit an axis. We have to differentiate: coasting, reversal of an axis, full stop.
  997. float v_exit = previous_speed[axis], v_entry = current_speed[axis];
  998. if (prev_speed_larger) v_exit *= smaller_speed_factor;
  999. if (limited) {
  1000. v_exit *= v_factor;
  1001. v_entry *= v_factor;
  1002. }
  1003. // Calculate jerk depending on whether the axis is coasting in the same direction or reversing.
  1004. float jerk =
  1005. (v_exit > v_entry) ?
  1006. ((v_entry > 0.f || v_exit < 0.f) ?
  1007. // coasting
  1008. (v_exit - v_entry) :
  1009. // axis reversal
  1010. max(v_exit, -v_entry)) :
  1011. // v_exit <= v_entry
  1012. ((v_entry < 0.f || v_exit > 0.f) ?
  1013. // coasting
  1014. (v_entry - v_exit) :
  1015. // axis reversal
  1016. max(-v_exit, v_entry));
  1017. if (jerk > max_jerk[axis]) {
  1018. v_factor *= max_jerk[axis] / jerk;
  1019. limited = true;
  1020. }
  1021. }
  1022. if (limited) vmax_junction *= v_factor;
  1023. // Now the transition velocity is known, which maximizes the shared exit / entry velocity while
  1024. // respecting the jerk factors, it may be possible, that applying separate safe exit / entry velocities will achieve faster prints.
  1025. float vmax_junction_threshold = vmax_junction * 0.99f;
  1026. if (previous_safe_speed > vmax_junction_threshold && safe_speed > vmax_junction_threshold) {
  1027. // Not coasting. The machine will stop and start the movements anyway,
  1028. // better to start the segment from start.
  1029. block->flag |= BLOCK_FLAG_START_FROM_FULL_HALT;
  1030. vmax_junction = safe_speed;
  1031. }
  1032. }
  1033. else {
  1034. block->flag |= BLOCK_FLAG_START_FROM_FULL_HALT;
  1035. vmax_junction = safe_speed;
  1036. }
  1037. // Max entry speed of this block equals the max exit speed of the previous block.
  1038. block->max_entry_speed = vmax_junction;
  1039. // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
  1040. float v_allowable = max_allowable_speed(-block->acceleration, MINIMUM_PLANNER_SPEED, block->millimeters);
  1041. block->entry_speed = min(vmax_junction, v_allowable);
  1042. // Initialize planner efficiency flags
  1043. // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
  1044. // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
  1045. // the current block and next block junction speeds are guaranteed to always be at their maximum
  1046. // junction speeds in deceleration and acceleration, respectively. This is due to how the current
  1047. // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
  1048. // the reverse and forward planners, the corresponding block junction speed will always be at the
  1049. // the maximum junction speed and may always be ignored for any speed reduction checks.
  1050. block->flag |= BLOCK_FLAG_RECALCULATE | (block->nominal_speed <= v_allowable ? BLOCK_FLAG_NOMINAL_LENGTH : 0);
  1051. // Update previous path unit_vector and nominal speed
  1052. memcpy(previous_speed, current_speed, sizeof(previous_speed));
  1053. previous_nominal_speed = block->nominal_speed;
  1054. previous_safe_speed = safe_speed;
  1055. #if ENABLED(LIN_ADVANCE)
  1056. // block->steps[E_AXIS] == block->step_event_count: A problem occurs when there's a very tiny move before a retract.
  1057. // In this case, the retract and the move will be executed together.
  1058. // This leads to an enormous number of advance steps due to a huge e_acceleration.
  1059. // The math is correct, but you don't want a retract move done with advance!
  1060. // So this situation is filtered out here.
  1061. if (!block->steps[E_AXIS] || (!block->steps[X_AXIS] && !block->steps[Y_AXIS]) || stepper.get_advance_k() == 0 || (uint32_t) block->steps[E_AXIS] == block->step_event_count) {
  1062. block->use_advance_lead = false;
  1063. }
  1064. else {
  1065. block->use_advance_lead = true;
  1066. block->e_speed_multiplier8 = (block->steps[E_AXIS] << 8) / block->step_event_count;
  1067. }
  1068. #elif ENABLED(ADVANCE)
  1069. // Calculate advance rate
  1070. if (!block->steps[E_AXIS] || (!block->steps[X_AXIS] && !block->steps[Y_AXIS] && !block->steps[Z_AXIS])) {
  1071. block->advance_rate = 0;
  1072. block->advance = 0;
  1073. }
  1074. else {
  1075. long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_steps_per_s2);
  1076. float advance = ((STEPS_PER_CUBIC_MM_E) * (EXTRUDER_ADVANCE_K)) * HYPOT(current_speed[E_AXIS], EXTRUSION_AREA) * 256;
  1077. block->advance = advance;
  1078. block->advance_rate = acc_dist ? advance / (float)acc_dist : 0;
  1079. }
  1080. /**
  1081. SERIAL_ECHO_START;
  1082. SERIAL_ECHOPGM("advance :");
  1083. SERIAL_ECHO(block->advance/256.0);
  1084. SERIAL_ECHOPGM("advance rate :");
  1085. SERIAL_ECHOLN(block->advance_rate/256.0);
  1086. */
  1087. #endif // ADVANCE or LIN_ADVANCE
  1088. calculate_trapezoid_for_block(block, block->entry_speed / block->nominal_speed, safe_speed / block->nominal_speed);
  1089. // Move buffer head
  1090. block_buffer_head = next_buffer_head;
  1091. // Update the position (only when a move was queued)
  1092. memcpy(position, target, sizeof(position));
  1093. recalculate();
  1094. stepper.wake_up();
  1095. } // buffer_line()
  1096. /**
  1097. * Directly set the planner XYZ position (and stepper positions)
  1098. * converting mm (or angles for SCARA) into steps.
  1099. *
  1100. * On CORE machines stepper ABC will be translated from the given XYZ.
  1101. */
  1102. void Planner::_set_position_mm(const float &a, const float &b, const float &c, const float &e) {
  1103. long na = position[X_AXIS] = lround(a * axis_steps_per_mm[X_AXIS]),
  1104. nb = position[Y_AXIS] = lround(b * axis_steps_per_mm[Y_AXIS]),
  1105. nc = position[Z_AXIS] = lround(c * axis_steps_per_mm[Z_AXIS]),
  1106. ne = position[E_AXIS] = lround(e * axis_steps_per_mm[E_AXIS]);
  1107. stepper.set_position(na, nb, nc, ne);
  1108. previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
  1109. ZERO(previous_speed);
  1110. }
  1111. void Planner::set_position_mm_kinematic(const float position[NUM_AXIS]) {
  1112. #if PLANNER_LEVELING
  1113. float pos[XYZ] = { position[X_AXIS], position[Y_AXIS], position[Z_AXIS] };
  1114. apply_leveling(pos);
  1115. #else
  1116. const float * const pos = position;
  1117. #endif
  1118. #if IS_KINEMATIC
  1119. inverse_kinematics(pos);
  1120. _set_position_mm(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], position[E_AXIS]);
  1121. #else
  1122. _set_position_mm(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], position[E_AXIS]);
  1123. #endif
  1124. }
  1125. /**
  1126. * Sync from the stepper positions. (e.g., after an interrupted move)
  1127. */
  1128. void Planner::sync_from_steppers() {
  1129. LOOP_XYZE(i) position[i] = stepper.position((AxisEnum)i);
  1130. }
  1131. /**
  1132. * Setters for planner position (also setting stepper position).
  1133. */
  1134. void Planner::set_position_mm(const AxisEnum axis, const float& v) {
  1135. position[axis] = lround(v * axis_steps_per_mm[axis]);
  1136. stepper.set_position(axis, v);
  1137. previous_speed[axis] = 0.0;
  1138. }
  1139. // Recalculate the steps/s^2 acceleration rates, based on the mm/s^2
  1140. void Planner::reset_acceleration_rates() {
  1141. uint32_t highest_rate = 1;
  1142. LOOP_XYZE(i) {
  1143. max_acceleration_steps_per_s2[i] = max_acceleration_mm_per_s2[i] * axis_steps_per_mm[i];
  1144. NOLESS(highest_rate, max_acceleration_steps_per_s2[i]);
  1145. }
  1146. cutoff_long = 4294967295UL / highest_rate;
  1147. }
  1148. // Recalculate position, steps_to_mm if axis_steps_per_mm changes!
  1149. void Planner::refresh_positioning() {
  1150. LOOP_XYZE(i) steps_to_mm[i] = 1.0 / axis_steps_per_mm[i];
  1151. set_position_mm_kinematic(current_position);
  1152. reset_acceleration_rates();
  1153. }
  1154. #if ENABLED(AUTOTEMP)
  1155. void Planner::autotemp_M109() {
  1156. autotemp_enabled = code_seen('F');
  1157. if (autotemp_enabled) autotemp_factor = code_value_temp_diff();
  1158. if (code_seen('S')) autotemp_min = code_value_temp_abs();
  1159. if (code_seen('B')) autotemp_max = code_value_temp_abs();
  1160. }
  1161. #endif