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

planner.cpp 55KB

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