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 100KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448144914501451145214531454145514561457145814591460146114621463146414651466146714681469147014711472147314741475147614771478147914801481148214831484148514861487148814891490149114921493149414951496149714981499150015011502150315041505150615071508150915101511151215131514151515161517151815191520152115221523152415251526152715281529153015311532153315341535153615371538153915401541154215431544154515461547154815491550155115521553155415551556155715581559156015611562156315641565156615671568156915701571157215731574157515761577157815791580158115821583158415851586158715881589159015911592159315941595159615971598159916001601160216031604160516061607160816091610161116121613161416151616161716181619162016211622162316241625162616271628162916301631163216331634163516361637163816391640164116421643164416451646164716481649165016511652165316541655165616571658165916601661166216631664166516661667166816691670167116721673167416751676167716781679168016811682168316841685168616871688168916901691169216931694169516961697169816991700170117021703170417051706170717081709171017111712171317141715171617171718171917201721172217231724172517261727172817291730173117321733173417351736173717381739174017411742174317441745174617471748174917501751175217531754175517561757175817591760176117621763176417651766176717681769177017711772177317741775177617771778177917801781178217831784178517861787178817891790179117921793179417951796179717981799180018011802180318041805180618071808180918101811181218131814181518161817181818191820182118221823182418251826182718281829183018311832183318341835183618371838183918401841184218431844184518461847184818491850185118521853185418551856185718581859186018611862186318641865186618671868186918701871187218731874187518761877187818791880188118821883188418851886188718881889189018911892189318941895189618971898189919001901190219031904190519061907190819091910191119121913191419151916191719181919192019211922192319241925192619271928192919301931193219331934193519361937193819391940194119421943194419451946194719481949195019511952195319541955195619571958195919601961196219631964196519661967196819691970197119721973197419751976197719781979198019811982198319841985198619871988198919901991199219931994199519961997199819992000200120022003200420052006200720082009201020112012201320142015201620172018201920202021202220232024202520262027202820292030203120322033203420352036203720382039204020412042204320442045204620472048204920502051205220532054205520562057205820592060206120622063206420652066206720682069207020712072207320742075207620772078207920802081208220832084208520862087208820892090209120922093209420952096209720982099210021012102210321042105210621072108210921102111211221132114211521162117211821192120212121222123212421252126212721282129213021312132213321342135213621372138213921402141214221432144214521462147214821492150215121522153215421552156215721582159216021612162216321642165216621672168216921702171217221732174217521762177217821792180218121822183218421852186218721882189219021912192219321942195219621972198219922002201220222032204220522062207220822092210221122122213221422152216221722182219222022212222222322242225222622272228222922302231223222332234223522362237223822392240224122422243224422452246224722482249225022512252225322542255225622572258225922602261226222632264226522662267226822692270227122722273227422752276227722782279228022812282228322842285228622872288228922902291229222932294229522962297229822992300230123022303230423052306230723082309231023112312231323142315231623172318231923202321232223232324232523262327232823292330233123322333233423352336233723382339234023412342234323442345234623472348234923502351235223532354235523562357235823592360236123622363236423652366236723682369237023712372237323742375237623772378237923802381238223832384238523862387238823892390239123922393239423952396239723982399240024012402240324042405240624072408240924102411241224132414241524162417241824192420242124222423242424252426242724282429243024312432243324342435243624372438243924402441244224432444244524462447244824492450245124522453245424552456245724582459246024612462246324642465246624672468246924702471247224732474247524762477247824792480248124822483248424852486248724882489249024912492249324942495249624972498249925002501250225032504250525062507250825092510251125122513251425152516251725182519252025212522252325242525252625272528252925302531253225332534253525362537253825392540254125422543254425452546254725482549255025512552
  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. *
  60. * The fast inverse function needed for Bézier interpolation for AVR
  61. * was designed, written and tested by Eduardo José Tagle on April/2018
  62. */
  63. #include "planner.h"
  64. #include "stepper.h"
  65. #include "motion.h"
  66. #include "../module/temperature.h"
  67. #include "../lcd/ultralcd.h"
  68. #include "../core/language.h"
  69. #include "../gcode/parser.h"
  70. #include "../Marlin.h"
  71. #if HAS_LEVELING
  72. #include "../feature/bedlevel/bedlevel.h"
  73. #endif
  74. #if ENABLED(FILAMENT_WIDTH_SENSOR)
  75. #include "../feature/filwidth.h"
  76. #endif
  77. #if ENABLED(BARICUDA)
  78. #include "../feature/baricuda.h"
  79. #endif
  80. #if ENABLED(MIXING_EXTRUDER)
  81. #include "../feature/mixing.h"
  82. #endif
  83. #if ENABLED(AUTO_POWER_CONTROL)
  84. #include "../feature/power.h"
  85. #endif
  86. // Delay for delivery of first block to the stepper ISR, if the queue contains 2 or
  87. // fewer movements. The delay is measured in milliseconds, and must be less than 250ms
  88. #define BLOCK_DELAY_FOR_1ST_MOVE 100
  89. Planner planner;
  90. // public:
  91. /**
  92. * A ring buffer of moves described in steps
  93. */
  94. block_t Planner::block_buffer[BLOCK_BUFFER_SIZE];
  95. volatile uint8_t Planner::block_buffer_head, // Index of the next block to be pushed
  96. Planner::block_buffer_tail; // Index of the busy block, if any
  97. uint16_t Planner::cleaning_buffer_counter; // A counter to disable queuing of blocks
  98. uint8_t Planner::delay_before_delivering, // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks
  99. Planner::block_buffer_planned; // Index of the optimally planned block
  100. float Planner::max_feedrate_mm_s[XYZE_N], // Max speeds in mm per second
  101. Planner::axis_steps_per_mm[XYZE_N],
  102. Planner::steps_to_mm[XYZE_N];
  103. #if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
  104. bool Planner::abort_on_endstop_hit = false;
  105. #endif
  106. #if ENABLED(DISTINCT_E_FACTORS)
  107. uint8_t Planner::last_extruder = 0; // Respond to extruder change
  108. #endif
  109. int16_t Planner::flow_percentage[EXTRUDERS] = ARRAY_BY_EXTRUDERS1(100); // Extrusion factor for each extruder
  110. float Planner::e_factor[EXTRUDERS] = ARRAY_BY_EXTRUDERS1(1.0); // The flow percentage and volumetric multiplier combine to scale E movement
  111. #if DISABLED(NO_VOLUMETRICS)
  112. float Planner::filament_size[EXTRUDERS], // diameter of filament (in millimeters), typically around 1.75 or 2.85, 0 disables the volumetric calculations for the extruder
  113. Planner::volumetric_area_nominal = CIRCLE_AREA((DEFAULT_NOMINAL_FILAMENT_DIA) * 0.5), // Nominal cross-sectional area
  114. Planner::volumetric_multiplier[EXTRUDERS]; // Reciprocal of cross-sectional area of filament (in mm^2). Pre-calculated to reduce computation in the planner
  115. #endif
  116. uint32_t Planner::max_acceleration_steps_per_s2[XYZE_N],
  117. Planner::max_acceleration_mm_per_s2[XYZE_N]; // Use M201 to override by software
  118. uint32_t Planner::min_segment_time_us;
  119. // Initialized by settings.load()
  120. float Planner::min_feedrate_mm_s,
  121. Planner::acceleration, // Normal acceleration mm/s^2 DEFAULT ACCELERATION for all printing moves. M204 SXXXX
  122. Planner::retract_acceleration, // Retract acceleration mm/s^2 filament pull-back and push-forward while standing still in the other axes M204 TXXXX
  123. Planner::travel_acceleration, // Travel acceleration mm/s^2 DEFAULT ACCELERATION for all NON printing moves. M204 MXXXX
  124. Planner::max_jerk[XYZE], // The largest speed change requiring no acceleration
  125. Planner::min_travel_feedrate_mm_s;
  126. #if HAS_LEVELING
  127. bool Planner::leveling_active = false; // Flag that auto bed leveling is enabled
  128. #if ABL_PLANAR
  129. matrix_3x3 Planner::bed_level_matrix; // Transform to compensate for bed level
  130. #endif
  131. #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
  132. float Planner::z_fade_height, // Initialized by settings.load()
  133. Planner::inverse_z_fade_height,
  134. Planner::last_fade_z;
  135. #endif
  136. #else
  137. constexpr bool Planner::leveling_active;
  138. #endif
  139. #if ENABLED(SKEW_CORRECTION)
  140. #if ENABLED(SKEW_CORRECTION_GCODE)
  141. float Planner::xy_skew_factor;
  142. #else
  143. constexpr float Planner::xy_skew_factor;
  144. #endif
  145. #if ENABLED(SKEW_CORRECTION_FOR_Z) && ENABLED(SKEW_CORRECTION_GCODE)
  146. float Planner::xz_skew_factor, Planner::yz_skew_factor;
  147. #else
  148. constexpr float Planner::xz_skew_factor, Planner::yz_skew_factor;
  149. #endif
  150. #endif
  151. #if ENABLED(AUTOTEMP)
  152. float Planner::autotemp_max = 250,
  153. Planner::autotemp_min = 210,
  154. Planner::autotemp_factor = 0.1;
  155. bool Planner::autotemp_enabled = false;
  156. #endif
  157. // private:
  158. int32_t Planner::position[NUM_AXIS] = { 0 };
  159. uint32_t Planner::cutoff_long;
  160. float Planner::previous_speed[NUM_AXIS],
  161. Planner::previous_nominal_speed_sqr;
  162. #if ENABLED(DISABLE_INACTIVE_EXTRUDER)
  163. uint8_t Planner::g_uc_extruder_last_move[EXTRUDERS] = { 0 };
  164. #endif
  165. #ifdef XY_FREQUENCY_LIMIT
  166. // Old direction bits. Used for speed calculations
  167. unsigned char Planner::old_direction_bits = 0;
  168. // Segment times (in µs). Used for speed calculations
  169. uint32_t Planner::axis_segment_time_us[2][3] = { { MAX_FREQ_TIME_US + 1, 0, 0 }, { MAX_FREQ_TIME_US + 1, 0, 0 } };
  170. #endif
  171. #if ENABLED(LIN_ADVANCE)
  172. float Planner::extruder_advance_K; // Initialized by settings.load()
  173. #endif
  174. #if HAS_POSITION_FLOAT
  175. float Planner::position_float[XYZE]; // Needed for accurate maths. Steps cannot be used!
  176. #endif
  177. #if ENABLED(ULTRA_LCD)
  178. volatile uint32_t Planner::block_buffer_runtime_us = 0;
  179. #endif
  180. /**
  181. * Class and Instance Methods
  182. */
  183. Planner::Planner() { init(); }
  184. void Planner::init() {
  185. ZERO(position);
  186. #if HAS_POSITION_FLOAT
  187. ZERO(position_float);
  188. #endif
  189. ZERO(previous_speed);
  190. previous_nominal_speed_sqr = 0.0;
  191. #if ABL_PLANAR
  192. bed_level_matrix.set_to_identity();
  193. #endif
  194. clear_block_buffer();
  195. block_buffer_planned = 0;
  196. delay_before_delivering = 0;
  197. }
  198. #if ENABLED(S_CURVE_ACCELERATION)
  199. #ifdef __AVR__
  200. /**
  201. * This routine returns 0x1000000 / d, getting the inverse as fast as possible.
  202. * A fast-converging iterative Newton-Raphson method can reach full precision in
  203. * just 1 iteration, and takes 211 cycles (worst case; the mean case is less, up
  204. * to 30 cycles for small divisors), instead of the 500 cycles a normal division
  205. * would take.
  206. *
  207. * Inspired by the following page:
  208. * https://stackoverflow.com/questions/27801397/newton-raphson-division-with-big-integers
  209. *
  210. * Suppose we want to calculate floor(2 ^ k / B) where B is a positive integer
  211. * Then, B must be <= 2^k, otherwise, the quotient is 0.
  212. *
  213. * The Newton - Raphson iteration for x = B / 2 ^ k yields:
  214. * q[n + 1] = q[n] * (2 - q[n] * B / 2 ^ k)
  215. *
  216. * This can be rearranged to:
  217. * q[n + 1] = q[n] * (2 ^ (k + 1) - q[n] * B) >> k
  218. *
  219. * Each iteration requires only integer multiplications and bit shifts.
  220. * It doesn't necessarily converge to floor(2 ^ k / B) but in the worst case
  221. * it eventually alternates between floor(2 ^ k / B) and ceil(2 ^ k / B).
  222. * So it checks for this case and extracts floor(2 ^ k / B).
  223. *
  224. * A simple but important optimization for this approach is to truncate
  225. * multiplications (i.e., calculate only the higher bits of the product) in the
  226. * early iterations of the Newton - Raphson method. This is done so the results
  227. * of the early iterations are far from the quotient. Then it doesn't matter if
  228. * they are done inaccurately.
  229. * It's important to pick a good starting value for x. Knowing how many
  230. * digits the divisor has, it can be estimated:
  231. *
  232. * 2^k / x = 2 ^ log2(2^k / x)
  233. * 2^k / x = 2 ^(log2(2^k)-log2(x))
  234. * 2^k / x = 2 ^(k*log2(2)-log2(x))
  235. * 2^k / x = 2 ^ (k-log2(x))
  236. * 2^k / x >= 2 ^ (k-floor(log2(x)))
  237. * floor(log2(x)) is simply the index of the most significant bit set.
  238. *
  239. * If this estimation can be improved even further the number of iterations can be
  240. * reduced a lot, saving valuable execution time.
  241. * The paper "Software Integer Division" by Thomas L.Rodeheffer, Microsoft
  242. * Research, Silicon Valley,August 26, 2008, available at
  243. * https://www.microsoft.com/en-us/research/wp-content/uploads/2008/08/tr-2008-141.pdf
  244. * suggests, for its integer division algorithm, using a table to supply the first
  245. * 8 bits of precision, then, due to the quadratic convergence nature of the
  246. * Newton-Raphon iteration, just 2 iterations should be enough to get maximum
  247. * precision of the division.
  248. * By precomputing values of inverses for small denominator values, just one
  249. * Newton-Raphson iteration is enough to reach full precision.
  250. * This code uses the top 9 bits of the denominator as index.
  251. *
  252. * The AVR assembly function implements this C code using the data below:
  253. *
  254. * // For small divisors, it is best to directly retrieve the results
  255. * if (d <= 110) return pgm_read_dword(&small_inv_tab[d]);
  256. *
  257. * // Compute initial estimation of 0x1000000/x -
  258. * // Get most significant bit set on divider
  259. * uint8_t idx = 0;
  260. * uint32_t nr = d;
  261. * if (!(nr & 0xFF0000)) {
  262. * nr <<= 8; idx += 8;
  263. * if (!(nr & 0xFF0000)) { nr <<= 8; idx += 8; }
  264. * }
  265. * if (!(nr & 0xF00000)) { nr <<= 4; idx += 4; }
  266. * if (!(nr & 0xC00000)) { nr <<= 2; idx += 2; }
  267. * if (!(nr & 0x800000)) { nr <<= 1; idx += 1; }
  268. *
  269. * // Isolate top 9 bits of the denominator, to be used as index into the initial estimation table
  270. * uint32_t tidx = nr >> 15, // top 9 bits. bit8 is always set
  271. * ie = inv_tab[tidx & 0xFF] + 256, // Get the table value. bit9 is always set
  272. * x = idx <= 8 ? (ie >> (8 - idx)) : (ie << (idx - 8)); // Position the estimation at the proper place
  273. *
  274. * x = uint32_t((x * uint64_t(_BV(25) - x * d)) >> 24); // Refine estimation by newton-raphson. 1 iteration is enough
  275. * const uint32_t r = _BV(24) - x * d; // Estimate remainder
  276. * if (r >= d) x++; // Check whether to adjust result
  277. * return uint32_t(x); // x holds the proper estimation
  278. *
  279. */
  280. static uint32_t get_period_inverse(uint32_t d) {
  281. static const uint8_t inv_tab[256] PROGMEM = {
  282. 255,253,252,250,248,246,244,242,240,238,236,234,233,231,229,227,
  283. 225,224,222,220,218,217,215,213,212,210,208,207,205,203,202,200,
  284. 199,197,195,194,192,191,189,188,186,185,183,182,180,179,178,176,
  285. 175,173,172,170,169,168,166,165,164,162,161,160,158,157,156,154,
  286. 153,152,151,149,148,147,146,144,143,142,141,139,138,137,136,135,
  287. 134,132,131,130,129,128,127,126,125,123,122,121,120,119,118,117,
  288. 116,115,114,113,112,111,110,109,108,107,106,105,104,103,102,101,
  289. 100,99,98,97,96,95,94,93,92,91,90,89,88,88,87,86,
  290. 85,84,83,82,81,80,80,79,78,77,76,75,74,74,73,72,
  291. 71,70,70,69,68,67,66,66,65,64,63,62,62,61,60,59,
  292. 59,58,57,56,56,55,54,53,53,52,51,50,50,49,48,48,
  293. 47,46,46,45,44,43,43,42,41,41,40,39,39,38,37,37,
  294. 36,35,35,34,33,33,32,32,31,30,30,29,28,28,27,27,
  295. 26,25,25,24,24,23,22,22,21,21,20,19,19,18,18,17,
  296. 17,16,15,15,14,14,13,13,12,12,11,10,10,9,9,8,
  297. 8,7,7,6,6,5,5,4,4,3,3,2,2,1,0,0
  298. };
  299. // For small denominators, it is cheaper to directly store the result.
  300. // For bigger ones, just ONE Newton-Raphson iteration is enough to get
  301. // maximum precision we need
  302. static const uint32_t small_inv_tab[111] PROGMEM = {
  303. 16777216,16777216,8388608,5592405,4194304,3355443,2796202,2396745,2097152,1864135,1677721,1525201,1398101,1290555,1198372,1118481,
  304. 1048576,986895,932067,883011,838860,798915,762600,729444,699050,671088,645277,621378,599186,578524,559240,541200,
  305. 524288,508400,493447,479349,466033,453438,441505,430185,419430,409200,399457,390167,381300,372827,364722,356962,
  306. 349525,342392,335544,328965,322638,316551,310689,305040,299593,294337,289262,284359,279620,275036,270600,266305,
  307. 262144,258111,254200,250406,246723,243148,239674,236298,233016,229824,226719,223696,220752,217885,215092,212369,
  308. 209715,207126,204600,202135,199728,197379,195083,192841,190650,188508,186413,184365,182361,180400,178481,176602,
  309. 174762,172960,171196,169466,167772,166111,164482,162885,161319,159783,158275,156796,155344,153919,152520
  310. };
  311. // For small divisors, it is best to directly retrieve the results
  312. if (d <= 110) return pgm_read_dword(&small_inv_tab[d]);
  313. register uint8_t r8 = d & 0xFF,
  314. r9 = (d >> 8) & 0xFF,
  315. r10 = (d >> 16) & 0xFF,
  316. r2,r3,r4,r5,r6,r7,r11,r12,r13,r14,r15,r16,r17,r18;
  317. register const uint8_t* ptab = inv_tab;
  318. __asm__ __volatile__(
  319. // %8:%7:%6 = interval
  320. // r31:r30: MUST be those registers, and they must point to the inv_tab
  321. A("clr %13") // %13 = 0
  322. // Now we must compute
  323. // result = 0xFFFFFF / d
  324. // %8:%7:%6 = interval
  325. // %16:%15:%14 = nr
  326. // %13 = 0
  327. // A plain division of 24x24 bits should take 388 cycles to complete. We will
  328. // use Newton-Raphson for the calculation, and will strive to get way less cycles
  329. // for the same result - Using C division, it takes 500cycles to complete .
  330. A("clr %3") // idx = 0
  331. A("mov %14,%6")
  332. A("mov %15,%7")
  333. A("mov %16,%8") // nr = interval
  334. A("tst %16") // nr & 0xFF0000 == 0 ?
  335. A("brne 2f") // No, skip this
  336. A("mov %16,%15")
  337. A("mov %15,%14") // nr <<= 8, %14 not needed
  338. A("subi %3,-8") // idx += 8
  339. A("tst %16") // nr & 0xFF0000 == 0 ?
  340. A("brne 2f") // No, skip this
  341. A("mov %16,%15") // nr <<= 8, %14 not needed
  342. A("clr %15") // We clear %14
  343. A("subi %3,-8") // idx += 8
  344. // here %16 != 0 and %16:%15 contains at least 9 MSBits, or both %16:%15 are 0
  345. L("2")
  346. A("cpi %16,0x10") // (nr & 0xF00000) == 0 ?
  347. A("brcc 3f") // No, skip this
  348. A("swap %15") // Swap nibbles
  349. A("swap %16") // Swap nibbles. Low nibble is 0
  350. A("mov %14, %15")
  351. A("andi %14,0x0F") // Isolate low nibble
  352. A("andi %15,0xF0") // Keep proper nibble in %15
  353. A("or %16, %14") // %16:%15 <<= 4
  354. A("subi %3,-4") // idx += 4
  355. L("3")
  356. A("cpi %16,0x40") // (nr & 0xC00000) == 0 ?
  357. A("brcc 4f") // No, skip this
  358. A("add %15,%15")
  359. A("adc %16,%16")
  360. A("add %15,%15")
  361. A("adc %16,%16") // %16:%15 <<= 2
  362. A("subi %3,-2") // idx += 2
  363. L("4")
  364. A("cpi %16,0x80") // (nr & 0x800000) == 0 ?
  365. A("brcc 5f") // No, skip this
  366. A("add %15,%15")
  367. A("adc %16,%16") // %16:%15 <<= 1
  368. A("inc %3") // idx += 1
  369. // Now %16:%15 contains its MSBit set to 1, or %16:%15 is == 0. We are now absolutely sure
  370. // we have at least 9 MSBits available to enter the initial estimation table
  371. L("5")
  372. A("add %15,%15")
  373. A("adc %16,%16") // %16:%15 = tidx = (nr <<= 1), we lose the top MSBit (always set to 1, %16 is the index into the inverse table)
  374. A("add r30,%16") // Only use top 8 bits
  375. A("adc r31,%13") // r31:r30 = inv_tab + (tidx)
  376. A("lpm %14, Z") // %14 = inv_tab[tidx]
  377. A("ldi %15, 1") // %15 = 1 %15:%14 = inv_tab[tidx] + 256
  378. // We must scale the approximation to the proper place
  379. A("clr %16") // %16 will always be 0 here
  380. A("subi %3,8") // idx == 8 ?
  381. A("breq 6f") // yes, no need to scale
  382. A("brcs 7f") // If C=1, means idx < 8, result was negative!
  383. // idx > 8, now %3 = idx - 8. We must perform a left shift. idx range:[1-8]
  384. A("sbrs %3,0") // shift by 1bit position?
  385. A("rjmp 8f") // No
  386. A("add %14,%14")
  387. A("adc %15,%15") // %15:16 <<= 1
  388. L("8")
  389. A("sbrs %3,1") // shift by 2bit position?
  390. A("rjmp 9f") // No
  391. A("add %14,%14")
  392. A("adc %15,%15")
  393. A("add %14,%14")
  394. A("adc %15,%15") // %15:16 <<= 1
  395. L("9")
  396. A("sbrs %3,2") // shift by 4bits position?
  397. A("rjmp 16f") // No
  398. A("swap %15") // Swap nibbles. lo nibble of %15 will always be 0
  399. A("swap %14") // Swap nibbles
  400. A("mov %12,%14")
  401. A("andi %12,0x0F") // isolate low nibble
  402. A("andi %14,0xF0") // and clear it
  403. A("or %15,%12") // %15:%16 <<= 4
  404. L("16")
  405. A("sbrs %3,3") // shift by 8bits position?
  406. A("rjmp 6f") // No, we are done
  407. A("mov %16,%15")
  408. A("mov %15,%14")
  409. A("clr %14")
  410. A("jmp 6f")
  411. // idx < 8, now %3 = idx - 8. Get the count of bits
  412. L("7")
  413. A("neg %3") // %3 = -idx = count of bits to move right. idx range:[1...8]
  414. A("sbrs %3,0") // shift by 1 bit position ?
  415. A("rjmp 10f") // No, skip it
  416. A("asr %15") // (bit7 is always 0 here)
  417. A("ror %14")
  418. L("10")
  419. A("sbrs %3,1") // shift by 2 bit position ?
  420. A("rjmp 11f") // No, skip it
  421. A("asr %15") // (bit7 is always 0 here)
  422. A("ror %14")
  423. A("asr %15") // (bit7 is always 0 here)
  424. A("ror %14")
  425. L("11")
  426. A("sbrs %3,2") // shift by 4 bit position ?
  427. A("rjmp 12f") // No, skip it
  428. A("swap %15") // Swap nibbles
  429. A("andi %14, 0xF0") // Lose the lowest nibble
  430. A("swap %14") // Swap nibbles. Upper nibble is 0
  431. A("or %14,%15") // Pass nibble from upper byte
  432. A("andi %15, 0x0F") // And get rid of that nibble
  433. L("12")
  434. A("sbrs %3,3") // shift by 8 bit position ?
  435. A("rjmp 6f") // No, skip it
  436. A("mov %14,%15")
  437. A("clr %15")
  438. L("6") // %16:%15:%14 = initial estimation of 0x1000000 / d
  439. // Now, we must refine the estimation present on %16:%15:%14 using 1 iteration
  440. // of Newton-Raphson. As it has a quadratic convergence, 1 iteration is enough
  441. // to get more than 18bits of precision (the initial table lookup gives 9 bits of
  442. // precision to start from). 18bits of precision is all what is needed here for result
  443. // %8:%7:%6 = d = interval
  444. // %16:%15:%14 = x = initial estimation of 0x1000000 / d
  445. // %13 = 0
  446. // %3:%2:%1:%0 = working accumulator
  447. // Compute 1<<25 - x*d. Result should never exceed 25 bits and should always be positive
  448. A("clr %0")
  449. A("clr %1")
  450. A("clr %2")
  451. A("ldi %3,2") // %3:%2:%1:%0 = 0x2000000
  452. A("mul %6,%14") // r1:r0 = LO(d) * LO(x)
  453. A("sub %0,r0")
  454. A("sbc %1,r1")
  455. A("sbc %2,%13")
  456. A("sbc %3,%13") // %3:%2:%1:%0 -= LO(d) * LO(x)
  457. A("mul %7,%14") // r1:r0 = MI(d) * LO(x)
  458. A("sub %1,r0")
  459. A("sbc %2,r1" )
  460. A("sbc %3,%13") // %3:%2:%1:%0 -= MI(d) * LO(x) << 8
  461. A("mul %8,%14") // r1:r0 = HI(d) * LO(x)
  462. A("sub %2,r0")
  463. A("sbc %3,r1") // %3:%2:%1:%0 -= MIL(d) * LO(x) << 16
  464. A("mul %6,%15") // r1:r0 = LO(d) * MI(x)
  465. A("sub %1,r0")
  466. A("sbc %2,r1")
  467. A("sbc %3,%13") // %3:%2:%1:%0 -= LO(d) * MI(x) << 8
  468. A("mul %7,%15") // r1:r0 = MI(d) * MI(x)
  469. A("sub %2,r0")
  470. A("sbc %3,r1") // %3:%2:%1:%0 -= MI(d) * MI(x) << 16
  471. A("mul %8,%15") // r1:r0 = HI(d) * MI(x)
  472. A("sub %3,r0") // %3:%2:%1:%0 -= MIL(d) * MI(x) << 24
  473. A("mul %6,%16") // r1:r0 = LO(d) * HI(x)
  474. A("sub %2,r0")
  475. A("sbc %3,r1") // %3:%2:%1:%0 -= LO(d) * HI(x) << 16
  476. A("mul %7,%16") // r1:r0 = MI(d) * HI(x)
  477. A("sub %3,r0") // %3:%2:%1:%0 -= MI(d) * HI(x) << 24
  478. // %3:%2:%1:%0 = (1<<25) - x*d [169]
  479. // We need to multiply that result by x, and we are only interested in the top 24bits of that multiply
  480. // %16:%15:%14 = x = initial estimation of 0x1000000 / d
  481. // %3:%2:%1:%0 = (1<<25) - x*d = acc
  482. // %13 = 0
  483. // result = %11:%10:%9:%5:%4
  484. A("mul %14,%0") // r1:r0 = LO(x) * LO(acc)
  485. A("mov %4,r1")
  486. A("clr %5")
  487. A("clr %9")
  488. A("clr %10")
  489. A("clr %11") // %11:%10:%9:%5:%4 = LO(x) * LO(acc) >> 8
  490. A("mul %15,%0") // r1:r0 = MI(x) * LO(acc)
  491. A("add %4,r0")
  492. A("adc %5,r1")
  493. A("adc %9,%13")
  494. A("adc %10,%13")
  495. A("adc %11,%13") // %11:%10:%9:%5:%4 += MI(x) * LO(acc)
  496. A("mul %16,%0") // r1:r0 = HI(x) * LO(acc)
  497. A("add %5,r0")
  498. A("adc %9,r1")
  499. A("adc %10,%13")
  500. A("adc %11,%13") // %11:%10:%9:%5:%4 += MI(x) * LO(acc) << 8
  501. A("mul %14,%1") // r1:r0 = LO(x) * MIL(acc)
  502. A("add %4,r0")
  503. A("adc %5,r1")
  504. A("adc %9,%13")
  505. A("adc %10,%13")
  506. A("adc %11,%13") // %11:%10:%9:%5:%4 = LO(x) * MIL(acc)
  507. A("mul %15,%1") // r1:r0 = MI(x) * MIL(acc)
  508. A("add %5,r0")
  509. A("adc %9,r1")
  510. A("adc %10,%13")
  511. A("adc %11,%13") // %11:%10:%9:%5:%4 += MI(x) * MIL(acc) << 8
  512. A("mul %16,%1") // r1:r0 = HI(x) * MIL(acc)
  513. A("add %9,r0")
  514. A("adc %10,r1")
  515. A("adc %11,%13") // %11:%10:%9:%5:%4 += MI(x) * MIL(acc) << 16
  516. A("mul %14,%2") // r1:r0 = LO(x) * MIH(acc)
  517. A("add %5,r0")
  518. A("adc %9,r1")
  519. A("adc %10,%13")
  520. A("adc %11,%13") // %11:%10:%9:%5:%4 = LO(x) * MIH(acc) << 8
  521. A("mul %15,%2") // r1:r0 = MI(x) * MIH(acc)
  522. A("add %9,r0")
  523. A("adc %10,r1")
  524. A("adc %11,%13") // %11:%10:%9:%5:%4 += MI(x) * MIH(acc) << 16
  525. A("mul %16,%2") // r1:r0 = HI(x) * MIH(acc)
  526. A("add %10,r0")
  527. A("adc %11,r1") // %11:%10:%9:%5:%4 += MI(x) * MIH(acc) << 24
  528. A("mul %14,%3") // r1:r0 = LO(x) * HI(acc)
  529. A("add %9,r0")
  530. A("adc %10,r1")
  531. A("adc %11,%13") // %11:%10:%9:%5:%4 = LO(x) * HI(acc) << 16
  532. A("mul %15,%3") // r1:r0 = MI(x) * HI(acc)
  533. A("add %10,r0")
  534. A("adc %11,r1") // %11:%10:%9:%5:%4 += MI(x) * HI(acc) << 24
  535. A("mul %16,%3") // r1:r0 = HI(x) * HI(acc)
  536. A("add %11,r0") // %11:%10:%9:%5:%4 += MI(x) * HI(acc) << 32
  537. // At this point, %11:%10:%9 contains the new estimation of x.
  538. // Finally, we must correct the result. Estimate remainder as
  539. // (1<<24) - x*d
  540. // %11:%10:%9 = x
  541. // %8:%7:%6 = d = interval" "\n\t"
  542. A("ldi %3,1")
  543. A("clr %2")
  544. A("clr %1")
  545. A("clr %0") // %3:%2:%1:%0 = 0x1000000
  546. A("mul %6,%9") // r1:r0 = LO(d) * LO(x)
  547. A("sub %0,r0")
  548. A("sbc %1,r1")
  549. A("sbc %2,%13")
  550. A("sbc %3,%13") // %3:%2:%1:%0 -= LO(d) * LO(x)
  551. A("mul %7,%9") // r1:r0 = MI(d) * LO(x)
  552. A("sub %1,r0")
  553. A("sbc %2,r1")
  554. A("sbc %3,%13") // %3:%2:%1:%0 -= MI(d) * LO(x) << 8
  555. A("mul %8,%9") // r1:r0 = HI(d) * LO(x)
  556. A("sub %2,r0")
  557. A("sbc %3,r1") // %3:%2:%1:%0 -= MIL(d) * LO(x) << 16
  558. A("mul %6,%10") // r1:r0 = LO(d) * MI(x)
  559. A("sub %1,r0")
  560. A("sbc %2,r1")
  561. A("sbc %3,%13") // %3:%2:%1:%0 -= LO(d) * MI(x) << 8
  562. A("mul %7,%10") // r1:r0 = MI(d) * MI(x)
  563. A("sub %2,r0")
  564. A("sbc %3,r1") // %3:%2:%1:%0 -= MI(d) * MI(x) << 16
  565. A("mul %8,%10") // r1:r0 = HI(d) * MI(x)
  566. A("sub %3,r0") // %3:%2:%1:%0 -= MIL(d) * MI(x) << 24
  567. A("mul %6,%11") // r1:r0 = LO(d) * HI(x)
  568. A("sub %2,r0")
  569. A("sbc %3,r1") // %3:%2:%1:%0 -= LO(d) * HI(x) << 16
  570. A("mul %7,%11") // r1:r0 = MI(d) * HI(x)
  571. A("sub %3,r0") // %3:%2:%1:%0 -= MI(d) * HI(x) << 24
  572. // %3:%2:%1:%0 = r = (1<<24) - x*d
  573. // %8:%7:%6 = d = interval
  574. // Perform the final correction
  575. A("sub %0,%6")
  576. A("sbc %1,%7")
  577. A("sbc %2,%8") // r -= d
  578. A("brcs 14f") // if ( r >= d)
  579. // %11:%10:%9 = x
  580. A("ldi %3,1")
  581. A("add %9,%3")
  582. A("adc %10,%13")
  583. A("adc %11,%13") // x++
  584. L("14")
  585. // Estimation is done. %11:%10:%9 = x
  586. A("clr __zero_reg__") // Make C runtime happy
  587. // [211 cycles total]
  588. : "=r" (r2),
  589. "=r" (r3),
  590. "=r" (r4),
  591. "=d" (r5),
  592. "=r" (r6),
  593. "=r" (r7),
  594. "+r" (r8),
  595. "+r" (r9),
  596. "+r" (r10),
  597. "=d" (r11),
  598. "=r" (r12),
  599. "=r" (r13),
  600. "=d" (r14),
  601. "=d" (r15),
  602. "=d" (r16),
  603. "=d" (r17),
  604. "=d" (r18),
  605. "+z" (ptab)
  606. :
  607. : "r0", "r1", "cc"
  608. );
  609. // Return the result
  610. return r11 | (uint16_t(r12) << 8) | (uint32_t(r13) << 16);
  611. }
  612. #else
  613. // All other 32-bit MPUs can easily do inverse using hardware division,
  614. // so we don't need to reduce precision or to use assembly language at all.
  615. // This routine, for all other archs, returns 0x100000000 / d ~= 0xFFFFFFFF / d
  616. static FORCE_INLINE uint32_t get_period_inverse(const uint32_t d) { return 0xFFFFFFFF / d; }
  617. #endif
  618. #endif
  619. #define MINIMAL_STEP_RATE 120
  620. /**
  621. * Calculate trapezoid parameters, multiplying the entry- and exit-speeds
  622. * by the provided factors.
  623. */
  624. void Planner::calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor) {
  625. uint32_t initial_rate = CEIL(block->nominal_rate * entry_factor),
  626. final_rate = CEIL(block->nominal_rate * exit_factor); // (steps per second)
  627. // Limit minimal step rate (Otherwise the timer will overflow.)
  628. NOLESS(initial_rate, uint32_t(MINIMAL_STEP_RATE));
  629. NOLESS(final_rate, uint32_t(MINIMAL_STEP_RATE));
  630. #if ENABLED(S_CURVE_ACCELERATION)
  631. uint32_t cruise_rate = initial_rate;
  632. #endif
  633. const int32_t accel = block->acceleration_steps_per_s2;
  634. // Steps required for acceleration, deceleration to/from nominal rate
  635. uint32_t accelerate_steps = CEIL(estimate_acceleration_distance(initial_rate, block->nominal_rate, accel)),
  636. decelerate_steps = FLOOR(estimate_acceleration_distance(block->nominal_rate, final_rate, -accel));
  637. // Steps between acceleration and deceleration, if any
  638. int32_t plateau_steps = block->step_event_count - accelerate_steps - decelerate_steps;
  639. // Does accelerate_steps + decelerate_steps exceed step_event_count?
  640. // Then we can't possibly reach the nominal rate, there will be no cruising.
  641. // Use intersection_distance() to calculate accel / braking time in order to
  642. // reach the final_rate exactly at the end of this block.
  643. if (plateau_steps < 0) {
  644. const float accelerate_steps_float = CEIL(intersection_distance(initial_rate, final_rate, accel, block->step_event_count));
  645. accelerate_steps = MIN(uint32_t(MAX(accelerate_steps_float, 0)), block->step_event_count);
  646. plateau_steps = 0;
  647. #if ENABLED(S_CURVE_ACCELERATION)
  648. // We won't reach the cruising rate. Let's calculate the speed we will reach
  649. cruise_rate = final_speed(initial_rate, accel, accelerate_steps);
  650. #endif
  651. }
  652. #if ENABLED(S_CURVE_ACCELERATION)
  653. else // We have some plateau time, so the cruise rate will be the nominal rate
  654. cruise_rate = block->nominal_rate;
  655. #endif
  656. // block->accelerate_until = accelerate_steps;
  657. // block->decelerate_after = accelerate_steps+plateau_steps;
  658. #if ENABLED(S_CURVE_ACCELERATION)
  659. // Jerk controlled speed requires to express speed versus time, NOT steps
  660. uint32_t acceleration_time = ((float)(cruise_rate - initial_rate) / accel) * (HAL_STEPPER_TIMER_RATE),
  661. deceleration_time = ((float)(cruise_rate - final_rate) / accel) * (HAL_STEPPER_TIMER_RATE);
  662. // And to offload calculations from the ISR, we also calculate the inverse of those times here
  663. uint32_t acceleration_time_inverse = get_period_inverse(acceleration_time);
  664. uint32_t deceleration_time_inverse = get_period_inverse(deceleration_time);
  665. #endif
  666. // Fill variables used by the stepper in a critical section
  667. const bool was_enabled = STEPPER_ISR_ENABLED();
  668. if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
  669. // Don't update variables if block is busy: It is being interpreted by the planner
  670. if (!TEST(block->flag, BLOCK_BIT_BUSY)) {
  671. block->accelerate_until = accelerate_steps;
  672. block->decelerate_after = accelerate_steps + plateau_steps;
  673. block->initial_rate = initial_rate;
  674. #if ENABLED(S_CURVE_ACCELERATION)
  675. block->acceleration_time = acceleration_time;
  676. block->deceleration_time = deceleration_time;
  677. block->acceleration_time_inverse = acceleration_time_inverse;
  678. block->deceleration_time_inverse = deceleration_time_inverse;
  679. block->cruise_rate = cruise_rate;
  680. #endif
  681. block->final_rate = final_rate;
  682. }
  683. if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
  684. }
  685. /* PLANNER SPEED DEFINITION
  686. +--------+ <- current->nominal_speed
  687. / \
  688. current->entry_speed -> + \
  689. | + <- next->entry_speed (aka exit speed)
  690. +-------------+
  691. time -->
  692. Recalculates the motion plan according to the following basic guidelines:
  693. 1. Go over every feasible block sequentially in reverse order and calculate the junction speeds
  694. (i.e. current->entry_speed) such that:
  695. a. No junction speed exceeds the pre-computed maximum junction speed limit or nominal speeds of
  696. neighboring blocks.
  697. b. A block entry speed cannot exceed one reverse-computed from its exit speed (next->entry_speed)
  698. with a maximum allowable deceleration over the block travel distance.
  699. c. The last (or newest appended) block is planned from a complete stop (an exit speed of zero).
  700. 2. Go over every block in chronological (forward) order and dial down junction speed values if
  701. a. The exit speed exceeds the one forward-computed from its entry speed with the maximum allowable
  702. acceleration over the block travel distance.
  703. When these stages are complete, the planner will have maximized the velocity profiles throughout the all
  704. of the planner blocks, where every block is operating at its maximum allowable acceleration limits. In
  705. other words, for all of the blocks in the planner, the plan is optimal and no further speed improvements
  706. are possible. If a new block is added to the buffer, the plan is recomputed according to the said
  707. guidelines for a new optimal plan.
  708. To increase computational efficiency of these guidelines, a set of planner block pointers have been
  709. created to indicate stop-compute points for when the planner guidelines cannot logically make any further
  710. changes or improvements to the plan when in normal operation and new blocks are streamed and added to the
  711. planner buffer. For example, if a subset of sequential blocks in the planner have been planned and are
  712. bracketed by junction velocities at their maximums (or by the first planner block as well), no new block
  713. added to the planner buffer will alter the velocity profiles within them. So we no longer have to compute
  714. them. Or, if a set of sequential blocks from the first block in the planner (or a optimal stop-compute
  715. point) are all accelerating, they are all optimal and can not be altered by a new block added to the
  716. planner buffer, as this will only further increase the plan speed to chronological blocks until a maximum
  717. junction velocity is reached. However, if the operational conditions of the plan changes from infrequently
  718. used feed holds or feedrate overrides, the stop-compute pointers will be reset and the entire plan is
  719. recomputed as stated in the general guidelines.
  720. Planner buffer index mapping:
  721. - block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed.
  722. - block_buffer_head: Points to the buffer block after the last block in the buffer. Used to indicate whether
  723. the buffer is full or empty. As described for standard ring buffers, this block is always empty.
  724. - block_buffer_planned: Points to the first buffer block after the last optimally planned block for normal
  725. streaming operating conditions. Use for planning optimizations by avoiding recomputing parts of the
  726. planner buffer that don't change with the addition of a new block, as describe above. In addition,
  727. this block can never be less than block_buffer_tail and will always be pushed forward and maintain
  728. this requirement when encountered by the plan_discard_current_block() routine during a cycle.
  729. NOTE: Since the planner only computes on what's in the planner buffer, some motions with lots of short
  730. line segments, like G2/3 arcs or complex curves, may seem to move slow. This is because there simply isn't
  731. enough combined distance traveled in the entire buffer to accelerate up to the nominal speed and then
  732. decelerate to a complete stop at the end of the buffer, as stated by the guidelines. If this happens and
  733. becomes an annoyance, there are a few simple solutions: (1) Maximize the machine acceleration. The planner
  734. will be able to compute higher velocity profiles within the same combined distance. (2) Maximize line
  735. motion(s) distance per block to a desired tolerance. The more combined distance the planner has to use,
  736. the faster it can go. (3) Maximize the planner buffer size. This also will increase the combined distance
  737. for the planner to compute over. It also increases the number of computations the planner has to perform
  738. to compute an optimal plan, so select carefully.
  739. */
  740. // The kernel called by recalculate() when scanning the plan from last to first entry.
  741. void Planner::reverse_pass_kernel(block_t* const current, const block_t * const next) {
  742. if (current) {
  743. // If entry speed is already at the maximum entry speed, and there was no change of speed
  744. // in the next block, there is no need to recheck. Block is cruising and there is no need to
  745. // compute anything for this block,
  746. // If not, block entry speed needs to be recalculated to ensure maximum possible planned speed.
  747. const float max_entry_speed_sqr = current->max_entry_speed_sqr;
  748. // Compute maximum entry speed decelerating over the current block from its exit speed.
  749. // If not at the maximum entry speed, or the previous block entry speed changed
  750. if (current->entry_speed_sqr != max_entry_speed_sqr || (next && TEST(next->flag, BLOCK_BIT_RECALCULATE))) {
  751. // If nominal length true, max junction speed is guaranteed to be reached.
  752. // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
  753. // the current block and next block junction speeds are guaranteed to always be at their maximum
  754. // junction speeds in deceleration and acceleration, respectively. This is due to how the current
  755. // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
  756. // the reverse and forward planners, the corresponding block junction speed will always be at the
  757. // the maximum junction speed and may always be ignored for any speed reduction checks.
  758. const float new_entry_speed_sqr = TEST(current->flag, BLOCK_BIT_NOMINAL_LENGTH)
  759. ? max_entry_speed_sqr
  760. : MIN(max_entry_speed_sqr, max_allowable_speed_sqr(-current->acceleration, next ? next->entry_speed_sqr : sq(MINIMUM_PLANNER_SPEED), current->millimeters));
  761. if (current->entry_speed_sqr != new_entry_speed_sqr) {
  762. current->entry_speed_sqr = new_entry_speed_sqr;
  763. // Need to recalculate the block speed
  764. SBI(current->flag, BLOCK_BIT_RECALCULATE);
  765. }
  766. }
  767. }
  768. }
  769. /**
  770. * recalculate() needs to go over the current plan twice.
  771. * Once in reverse and once forward. This implements the reverse pass.
  772. */
  773. void Planner::reverse_pass() {
  774. // Initialize block index to the last block in the planner buffer.
  775. uint8_t block_index = prev_block_index(block_buffer_head);
  776. // Read the index of the last buffer planned block.
  777. // The ISR may change it so get a stable local copy.
  778. uint8_t planned_block_index = block_buffer_planned;
  779. // If there was a race condition and block_buffer_planned was incremented
  780. // or was pointing at the head (queue empty) break loop now and avoid
  781. // planning already consumed blocks
  782. if (planned_block_index == block_buffer_head) return;
  783. // Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last
  784. // block in buffer. Cease planning when the last optimal planned or tail pointer is reached.
  785. // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan.
  786. block_t *current;
  787. const block_t *next = NULL;
  788. while (block_index != planned_block_index) {
  789. // Perform the reverse pass
  790. current = &block_buffer[block_index];
  791. // Only consider non sync blocks
  792. if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) {
  793. reverse_pass_kernel(current, next);
  794. next = current;
  795. }
  796. // Advance to the next
  797. block_index = prev_block_index(block_index);
  798. }
  799. }
  800. // The kernel called by recalculate() when scanning the plan from first to last entry.
  801. void Planner::forward_pass_kernel(const block_t* const previous, block_t* const current, const uint8_t block_index) {
  802. if (previous) {
  803. // If the previous block is an acceleration block, too short to complete the full speed
  804. // change, adjust the entry speed accordingly. Entry speeds have already been reset,
  805. // maximized, and reverse-planned. If nominal length is set, max junction speed is
  806. // guaranteed to be reached. No need to recheck.
  807. if (!TEST(previous->flag, BLOCK_BIT_NOMINAL_LENGTH) &&
  808. previous->entry_speed_sqr < current->entry_speed_sqr) {
  809. // Compute the maximum allowable speed
  810. const float new_entry_speed_sqr = max_allowable_speed_sqr(-previous->acceleration, previous->entry_speed_sqr, previous->millimeters);
  811. // If true, current block is full-acceleration and we can move the planned pointer forward.
  812. if (new_entry_speed_sqr < current->entry_speed_sqr) {
  813. // Always <= max_entry_speed_sqr. Backward pass sets this.
  814. current->entry_speed_sqr = new_entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this.
  815. // Set optimal plan pointer.
  816. block_buffer_planned = block_index;
  817. // And mark we need to recompute the trapezoidal shape
  818. SBI(current->flag, BLOCK_BIT_RECALCULATE);
  819. }
  820. }
  821. // Any block set at its maximum entry speed also creates an optimal plan up to this
  822. // point in the buffer. When the plan is bracketed by either the beginning of the
  823. // buffer and a maximum entry speed or two maximum entry speeds, every block in between
  824. // cannot logically be further improved. Hence, we don't have to recompute them anymore.
  825. if (current->entry_speed_sqr == current->max_entry_speed_sqr)
  826. block_buffer_planned = block_index;
  827. }
  828. }
  829. /**
  830. * recalculate() needs to go over the current plan twice.
  831. * Once in reverse and once forward. This implements the forward pass.
  832. */
  833. void Planner::forward_pass() {
  834. // Forward Pass: Forward plan the acceleration curve from the planned pointer onward.
  835. // Also scans for optimal plan breakpoints and appropriately updates the planned pointer.
  836. // Begin at buffer planned pointer. Note that block_buffer_planned can be modified
  837. // by the stepper ISR, so read it ONCE. It it guaranteed that block_buffer_planned
  838. // will never lead head, so the loop is safe to execute. Also note that the forward
  839. // pass will never modify the values at the tail.
  840. uint8_t block_index = block_buffer_planned;
  841. block_t *current;
  842. const block_t * previous = NULL;
  843. while (block_index != block_buffer_head) {
  844. // Perform the forward pass
  845. current = &block_buffer[block_index];
  846. // Skip SYNC blocks
  847. if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) {
  848. forward_pass_kernel(previous, current, block_index);
  849. previous = current;
  850. }
  851. // Advance to the previous
  852. block_index = next_block_index(block_index);
  853. }
  854. }
  855. /**
  856. * Recalculate the trapezoid speed profiles for all blocks in the plan
  857. * according to the entry_factor for each junction. Must be called by
  858. * recalculate() after updating the blocks.
  859. */
  860. void Planner::recalculate_trapezoids() {
  861. // The tail may be changed by the ISR so get a local copy.
  862. uint8_t block_index = block_buffer_tail;
  863. // As there could be a sync block in the head of the queue, and the next loop must not
  864. // recalculate the head block (as it needs to be specially handled), scan backwards until
  865. // we find the first non SYNC block
  866. uint8_t head_block_index = block_buffer_head;
  867. while (head_block_index != block_index) {
  868. // Go back (head always point to the first free block)
  869. uint8_t prev_index = prev_block_index(head_block_index);
  870. // Get the pointer to the block
  871. block_t *prev = &block_buffer[prev_index];
  872. // If not dealing with a sync block, we are done. The last block is not a SYNC block
  873. if (!TEST(prev->flag, BLOCK_BIT_SYNC_POSITION)) break;
  874. // Examine the previous block. This and all following are SYNC blocks
  875. head_block_index = prev_index;
  876. };
  877. // Go from the tail (currently executed block) to the first block, without including it)
  878. block_t *current = NULL, *next = NULL;
  879. float current_entry_speed = 0.0, next_entry_speed = 0.0;
  880. while (block_index != head_block_index) {
  881. next = &block_buffer[block_index];
  882. // Skip sync blocks
  883. if (!TEST(next->flag, BLOCK_BIT_SYNC_POSITION)) {
  884. next_entry_speed = SQRT(next->entry_speed_sqr);
  885. if (current) {
  886. // Recalculate if current block entry or exit junction speed has changed.
  887. if (TEST(current->flag, BLOCK_BIT_RECALCULATE) || TEST(next->flag, BLOCK_BIT_RECALCULATE)) {
  888. // NOTE: Entry and exit factors always > 0 by all previous logic operations.
  889. const float current_nominal_speed = SQRT(current->nominal_speed_sqr),
  890. nomr = 1.0 / current_nominal_speed;
  891. calculate_trapezoid_for_block(current, current_entry_speed * nomr, next_entry_speed * nomr);
  892. #if ENABLED(LIN_ADVANCE)
  893. if (current->use_advance_lead) {
  894. const float comp = current->e_D_ratio * extruder_advance_K * axis_steps_per_mm[E_AXIS];
  895. current->max_adv_steps = current_nominal_speed * comp;
  896. current->final_adv_steps = next_entry_speed * comp;
  897. }
  898. #endif
  899. CBI(current->flag, BLOCK_BIT_RECALCULATE); // Reset current only to ensure next trapezoid is computed
  900. }
  901. }
  902. current = next;
  903. current_entry_speed = next_entry_speed;
  904. }
  905. block_index = next_block_index(block_index);
  906. }
  907. // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
  908. if (next) {
  909. const float next_nominal_speed = SQRT(next->nominal_speed_sqr),
  910. nomr = 1.0 / next_nominal_speed;
  911. calculate_trapezoid_for_block(next, next_entry_speed * nomr, (MINIMUM_PLANNER_SPEED) * nomr);
  912. #if ENABLED(LIN_ADVANCE)
  913. if (next->use_advance_lead) {
  914. const float comp = next->e_D_ratio * extruder_advance_K * axis_steps_per_mm[E_AXIS];
  915. next->max_adv_steps = next_nominal_speed * comp;
  916. next->final_adv_steps = (MINIMUM_PLANNER_SPEED) * comp;
  917. }
  918. #endif
  919. CBI(next->flag, BLOCK_BIT_RECALCULATE);
  920. }
  921. }
  922. void Planner::recalculate() {
  923. // Initialize block index to the last block in the planner buffer.
  924. const uint8_t block_index = prev_block_index(block_buffer_head);
  925. // If there is just one block, no planning can be done. Avoid it!
  926. if (block_index != block_buffer_planned) {
  927. reverse_pass();
  928. forward_pass();
  929. }
  930. recalculate_trapezoids();
  931. }
  932. #if ENABLED(AUTOTEMP)
  933. void Planner::getHighESpeed() {
  934. static float oldt = 0;
  935. if (!autotemp_enabled) return;
  936. if (thermalManager.degTargetHotend(0) + 2 < autotemp_min) return; // probably temperature set to zero.
  937. float high = 0.0;
  938. for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) {
  939. block_t* block = &block_buffer[b];
  940. if (block->steps[X_AXIS] || block->steps[Y_AXIS] || block->steps[Z_AXIS]) {
  941. const float se = (float)block->steps[E_AXIS] / block->step_event_count * SQRT(block->nominal_speed_sqr); // mm/sec;
  942. NOLESS(high, se);
  943. }
  944. }
  945. float t = autotemp_min + high * autotemp_factor;
  946. t = constrain(t, autotemp_min, autotemp_max);
  947. if (t < oldt) t = t * (1 - (AUTOTEMP_OLDWEIGHT)) + oldt * (AUTOTEMP_OLDWEIGHT);
  948. oldt = t;
  949. thermalManager.setTargetHotend(t, 0);
  950. }
  951. #endif // AUTOTEMP
  952. /**
  953. * Maintain fans, paste extruder pressure,
  954. */
  955. void Planner::check_axes_activity() {
  956. unsigned char axis_active[NUM_AXIS] = { 0 },
  957. tail_fan_speed[FAN_COUNT];
  958. #if ENABLED(BARICUDA)
  959. #if HAS_HEATER_1
  960. uint8_t tail_valve_pressure;
  961. #endif
  962. #if HAS_HEATER_2
  963. uint8_t tail_e_to_p_pressure;
  964. #endif
  965. #endif
  966. if (has_blocks_queued()) {
  967. #if FAN_COUNT > 0
  968. for (uint8_t i = 0; i < FAN_COUNT; i++)
  969. tail_fan_speed[i] = block_buffer[block_buffer_tail].fan_speed[i];
  970. #endif
  971. block_t* block;
  972. #if ENABLED(BARICUDA)
  973. block = &block_buffer[block_buffer_tail];
  974. #if HAS_HEATER_1
  975. tail_valve_pressure = block->valve_pressure;
  976. #endif
  977. #if HAS_HEATER_2
  978. tail_e_to_p_pressure = block->e_to_p_pressure;
  979. #endif
  980. #endif
  981. for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) {
  982. block = &block_buffer[b];
  983. LOOP_XYZE(i) if (block->steps[i]) axis_active[i]++;
  984. }
  985. }
  986. else {
  987. #if FAN_COUNT > 0
  988. for (uint8_t i = 0; i < FAN_COUNT; i++) tail_fan_speed[i] = fanSpeeds[i];
  989. #endif
  990. #if ENABLED(BARICUDA)
  991. #if HAS_HEATER_1
  992. tail_valve_pressure = baricuda_valve_pressure;
  993. #endif
  994. #if HAS_HEATER_2
  995. tail_e_to_p_pressure = baricuda_e_to_p_pressure;
  996. #endif
  997. #endif
  998. }
  999. #if ENABLED(DISABLE_X)
  1000. if (!axis_active[X_AXIS]) disable_X();
  1001. #endif
  1002. #if ENABLED(DISABLE_Y)
  1003. if (!axis_active[Y_AXIS]) disable_Y();
  1004. #endif
  1005. #if ENABLED(DISABLE_Z)
  1006. if (!axis_active[Z_AXIS]) disable_Z();
  1007. #endif
  1008. #if ENABLED(DISABLE_E)
  1009. if (!axis_active[E_AXIS]) disable_e_steppers();
  1010. #endif
  1011. #if FAN_COUNT > 0
  1012. #if FAN_KICKSTART_TIME > 0
  1013. static millis_t fan_kick_end[FAN_COUNT] = { 0 };
  1014. #define KICKSTART_FAN(f) \
  1015. if (tail_fan_speed[f]) { \
  1016. millis_t ms = millis(); \
  1017. if (fan_kick_end[f] == 0) { \
  1018. fan_kick_end[f] = ms + FAN_KICKSTART_TIME; \
  1019. tail_fan_speed[f] = 255; \
  1020. } else if (PENDING(ms, fan_kick_end[f])) \
  1021. tail_fan_speed[f] = 255; \
  1022. } else fan_kick_end[f] = 0
  1023. #if HAS_FAN0
  1024. KICKSTART_FAN(0);
  1025. #endif
  1026. #if HAS_FAN1
  1027. KICKSTART_FAN(1);
  1028. #endif
  1029. #if HAS_FAN2
  1030. KICKSTART_FAN(2);
  1031. #endif
  1032. #endif // FAN_KICKSTART_TIME > 0
  1033. #if FAN_MIN_PWM != 0 || FAN_MAX_PWM != 255
  1034. #define CALC_FAN_SPEED(f) (tail_fan_speed[f] ? map(tail_fan_speed[f], 1, 255, FAN_MIN_PWM, FAN_MAX_PWM) : 0)
  1035. #else
  1036. #define CALC_FAN_SPEED(f) tail_fan_speed[f]
  1037. #endif
  1038. #if ENABLED(FAN_SOFT_PWM)
  1039. #if HAS_FAN0
  1040. thermalManager.soft_pwm_amount_fan[0] = CALC_FAN_SPEED(0);
  1041. #endif
  1042. #if HAS_FAN1
  1043. thermalManager.soft_pwm_amount_fan[1] = CALC_FAN_SPEED(1);
  1044. #endif
  1045. #if HAS_FAN2
  1046. thermalManager.soft_pwm_amount_fan[2] = CALC_FAN_SPEED(2);
  1047. #endif
  1048. #else
  1049. #if HAS_FAN0
  1050. analogWrite(FAN_PIN, CALC_FAN_SPEED(0));
  1051. #endif
  1052. #if HAS_FAN1
  1053. analogWrite(FAN1_PIN, CALC_FAN_SPEED(1));
  1054. #endif
  1055. #if HAS_FAN2
  1056. analogWrite(FAN2_PIN, CALC_FAN_SPEED(2));
  1057. #endif
  1058. #endif
  1059. #endif // FAN_COUNT > 0
  1060. #if ENABLED(AUTOTEMP)
  1061. getHighESpeed();
  1062. #endif
  1063. #if ENABLED(BARICUDA)
  1064. #if HAS_HEATER_1
  1065. analogWrite(HEATER_1_PIN, tail_valve_pressure);
  1066. #endif
  1067. #if HAS_HEATER_2
  1068. analogWrite(HEATER_2_PIN, tail_e_to_p_pressure);
  1069. #endif
  1070. #endif
  1071. }
  1072. #if DISABLED(NO_VOLUMETRICS)
  1073. /**
  1074. * Get a volumetric multiplier from a filament diameter.
  1075. * This is the reciprocal of the circular cross-section area.
  1076. * Return 1.0 with volumetric off or a diameter of 0.0.
  1077. */
  1078. inline float calculate_volumetric_multiplier(const float &diameter) {
  1079. return (parser.volumetric_enabled && diameter) ? 1.0 / CIRCLE_AREA(diameter * 0.5) : 1.0;
  1080. }
  1081. /**
  1082. * Convert the filament sizes into volumetric multipliers.
  1083. * The multiplier converts a given E value into a length.
  1084. */
  1085. void Planner::calculate_volumetric_multipliers() {
  1086. for (uint8_t i = 0; i < COUNT(filament_size); i++) {
  1087. volumetric_multiplier[i] = calculate_volumetric_multiplier(filament_size[i]);
  1088. refresh_e_factor(i);
  1089. }
  1090. }
  1091. #endif // !NO_VOLUMETRICS
  1092. #if ENABLED(FILAMENT_WIDTH_SENSOR)
  1093. /**
  1094. * Convert the ratio value given by the filament width sensor
  1095. * into a volumetric multiplier. Conversion differs when using
  1096. * linear extrusion vs volumetric extrusion.
  1097. */
  1098. void Planner::calculate_volumetric_for_width_sensor(const int8_t encoded_ratio) {
  1099. // Reconstitute the nominal/measured ratio
  1100. const float nom_meas_ratio = 1.0 + 0.01 * encoded_ratio,
  1101. ratio_2 = sq(nom_meas_ratio);
  1102. volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM] = parser.volumetric_enabled
  1103. ? ratio_2 / CIRCLE_AREA(filament_width_nominal * 0.5) // Volumetric uses a true volumetric multiplier
  1104. : ratio_2; // Linear squares the ratio, which scales the volume
  1105. refresh_e_factor(FILAMENT_SENSOR_EXTRUDER_NUM);
  1106. }
  1107. #endif
  1108. #if PLANNER_LEVELING || HAS_UBL_AND_CURVES
  1109. /**
  1110. * rx, ry, rz - Cartesian positions in mm
  1111. * Leveled XYZ on completion
  1112. */
  1113. void Planner::apply_leveling(float &rx, float &ry, float &rz) {
  1114. #if ENABLED(SKEW_CORRECTION)
  1115. skew(rx, ry, rz);
  1116. #endif
  1117. if (!leveling_active) return;
  1118. #if ABL_PLANAR
  1119. float dx = rx - (X_TILT_FULCRUM),
  1120. dy = ry - (Y_TILT_FULCRUM);
  1121. apply_rotation_xyz(bed_level_matrix, dx, dy, rz);
  1122. rx = dx + X_TILT_FULCRUM;
  1123. ry = dy + Y_TILT_FULCRUM;
  1124. #elif HAS_MESH
  1125. #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
  1126. const float fade_scaling_factor = fade_scaling_factor_for_z(rz);
  1127. #else
  1128. constexpr float fade_scaling_factor = 1.0;
  1129. #endif
  1130. #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
  1131. const float raw[XYZ] = { rx, ry, 0 };
  1132. #endif
  1133. rz += (
  1134. #if ENABLED(MESH_BED_LEVELING)
  1135. mbl.get_z(rx, ry
  1136. #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
  1137. , fade_scaling_factor
  1138. #endif
  1139. )
  1140. #elif ENABLED(AUTO_BED_LEVELING_UBL)
  1141. fade_scaling_factor ? fade_scaling_factor * ubl.get_z_correction(rx, ry) : 0.0
  1142. #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
  1143. fade_scaling_factor ? fade_scaling_factor * bilinear_z_offset(raw) : 0.0
  1144. #endif
  1145. );
  1146. #endif
  1147. }
  1148. #endif
  1149. #if PLANNER_LEVELING
  1150. void Planner::unapply_leveling(float raw[XYZ]) {
  1151. if (leveling_active) {
  1152. #if ABL_PLANAR
  1153. matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix);
  1154. float dx = raw[X_AXIS] - (X_TILT_FULCRUM),
  1155. dy = raw[Y_AXIS] - (Y_TILT_FULCRUM);
  1156. apply_rotation_xyz(inverse, dx, dy, raw[Z_AXIS]);
  1157. raw[X_AXIS] = dx + X_TILT_FULCRUM;
  1158. raw[Y_AXIS] = dy + Y_TILT_FULCRUM;
  1159. #elif HAS_MESH
  1160. #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
  1161. const float fade_scaling_factor = fade_scaling_factor_for_z(raw[Z_AXIS]);
  1162. #else
  1163. constexpr float fade_scaling_factor = 1.0;
  1164. #endif
  1165. raw[Z_AXIS] -= (
  1166. #if ENABLED(MESH_BED_LEVELING)
  1167. mbl.get_z(raw[X_AXIS], raw[Y_AXIS]
  1168. #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
  1169. , fade_scaling_factor
  1170. #endif
  1171. )
  1172. #elif ENABLED(AUTO_BED_LEVELING_UBL)
  1173. fade_scaling_factor ? fade_scaling_factor * ubl.get_z_correction(raw[X_AXIS], raw[Y_AXIS]) : 0.0
  1174. #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
  1175. fade_scaling_factor ? fade_scaling_factor * bilinear_z_offset(raw) : 0.0
  1176. #endif
  1177. );
  1178. #endif
  1179. }
  1180. #if ENABLED(SKEW_CORRECTION)
  1181. unskew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]);
  1182. #endif
  1183. }
  1184. #endif // PLANNER_LEVELING
  1185. void Planner::quick_stop() {
  1186. // Remove all the queued blocks. Note that this function is NOT
  1187. // called from the Stepper ISR, so we must consider tail as readonly!
  1188. // that is why we set head to tail - But there is a race condition that
  1189. // must be handled: The tail could change between the read and the assignment
  1190. // so this must be enclosed in a critical section
  1191. const bool was_enabled = STEPPER_ISR_ENABLED();
  1192. if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
  1193. // Drop all queue entries
  1194. block_buffer_planned = block_buffer_head = block_buffer_tail;
  1195. // Restart the block delay for the first movement - As the queue was
  1196. // forced to empty, there's no risk the ISR will touch this.
  1197. delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE;
  1198. #if ENABLED(ULTRA_LCD)
  1199. // Clear the accumulated runtime
  1200. clear_block_buffer_runtime();
  1201. #endif
  1202. // Make sure to drop any attempt of queuing moves for at least 1 second
  1203. cleaning_buffer_counter = 1000;
  1204. // Reenable Stepper ISR
  1205. if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
  1206. // And stop the stepper ISR
  1207. stepper.quick_stop();
  1208. }
  1209. void Planner::endstop_triggered(const AxisEnum axis) {
  1210. // Record stepper position and discard the current block
  1211. stepper.endstop_triggered(axis);
  1212. }
  1213. float Planner::triggered_position_mm(const AxisEnum axis) {
  1214. return stepper.triggered_position(axis) * steps_to_mm[axis];
  1215. }
  1216. void Planner::finish_and_disable() {
  1217. while (has_blocks_queued() || cleaning_buffer_counter) idle();
  1218. disable_all_steppers();
  1219. }
  1220. /**
  1221. * Get an axis position according to stepper position(s)
  1222. * For CORE machines apply translation from ABC to XYZ.
  1223. */
  1224. float Planner::get_axis_position_mm(const AxisEnum axis) {
  1225. float axis_steps;
  1226. #if IS_CORE
  1227. // Requesting one of the "core" axes?
  1228. if (axis == CORE_AXIS_1 || axis == CORE_AXIS_2) {
  1229. // Protect the access to the position.
  1230. const bool was_enabled = STEPPER_ISR_ENABLED();
  1231. if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
  1232. // ((a1+a2)+(a1-a2))/2 -> (a1+a2+a1-a2)/2 -> (a1+a1)/2 -> a1
  1233. // ((a1+a2)-(a1-a2))/2 -> (a1+a2-a1+a2)/2 -> (a2+a2)/2 -> a2
  1234. axis_steps = 0.5f * (
  1235. axis == CORE_AXIS_2 ? CORESIGN(stepper.position(CORE_AXIS_1) - stepper.position(CORE_AXIS_2))
  1236. : stepper.position(CORE_AXIS_1) + stepper.position(CORE_AXIS_2)
  1237. );
  1238. if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
  1239. }
  1240. else
  1241. axis_steps = stepper.position(axis);
  1242. #else
  1243. axis_steps = stepper.position(axis);
  1244. #endif
  1245. return axis_steps * steps_to_mm[axis];
  1246. }
  1247. /**
  1248. * Block until all buffered steps are executed / cleaned
  1249. */
  1250. void Planner::synchronize() { while (has_blocks_queued() || cleaning_buffer_counter) idle(); }
  1251. /**
  1252. * Planner::_buffer_steps
  1253. *
  1254. * Add a new linear movement to the planner queue (in terms of steps).
  1255. *
  1256. * target - target position in steps units
  1257. * fr_mm_s - (target) speed of the move
  1258. * extruder - target extruder
  1259. * millimeters - the length of the movement, if known
  1260. *
  1261. * Returns true if movement was properly queued, false otherwise
  1262. */
  1263. bool Planner::_buffer_steps(const int32_t (&target)[XYZE]
  1264. #if HAS_POSITION_FLOAT
  1265. , const float (&target_float)[XYZE]
  1266. #endif
  1267. , float fr_mm_s, const uint8_t extruder, const float &millimeters
  1268. ) {
  1269. // If we are cleaning, do not accept queuing of movements
  1270. if (cleaning_buffer_counter) return false;
  1271. // Wait for the next available block
  1272. uint8_t next_buffer_head;
  1273. block_t * const block = get_next_free_block(next_buffer_head);
  1274. // Fill the block with the specified movement
  1275. if (!_populate_block(block, false, target
  1276. #if HAS_POSITION_FLOAT
  1277. , target_float
  1278. #endif
  1279. , fr_mm_s, extruder, millimeters
  1280. )) {
  1281. // Movement was not queued, probably because it was too short.
  1282. // Simply accept that as movement queued and done
  1283. return true;
  1284. }
  1285. // If this is the first added movement, reload the delay, otherwise, cancel it.
  1286. if (block_buffer_head == block_buffer_tail) {
  1287. // If it was the first queued block, restart the 1st block delivery delay, to
  1288. // give the planner an opportunity to queue more movements and plan them
  1289. // As there are no queued movements, the Stepper ISR will not touch this
  1290. // variable, so there is no risk setting this here (but it MUST be done
  1291. // before the following line!!)
  1292. delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE;
  1293. }
  1294. // Move buffer head
  1295. block_buffer_head = next_buffer_head;
  1296. // Recalculate and optimize trapezoidal speed profiles
  1297. recalculate();
  1298. // Movement successfully queued!
  1299. return true;
  1300. }
  1301. /**
  1302. * Planner::_populate_block
  1303. *
  1304. * Fills a new linear movement in the block (in terms of steps).
  1305. *
  1306. * target - target position in steps units
  1307. * fr_mm_s - (target) speed of the move
  1308. * extruder - target extruder
  1309. *
  1310. * Returns true is movement is acceptable, false otherwise
  1311. */
  1312. bool Planner::_populate_block(block_t * const block, bool split_move,
  1313. const int32_t (&target)[XYZE]
  1314. #if HAS_POSITION_FLOAT
  1315. , const float (&target_float)[XYZE]
  1316. #endif
  1317. , float fr_mm_s, const uint8_t extruder, const float &millimeters/*=0.0*/
  1318. ) {
  1319. const int32_t da = target[A_AXIS] - position[A_AXIS],
  1320. db = target[B_AXIS] - position[B_AXIS],
  1321. dc = target[C_AXIS] - position[C_AXIS];
  1322. int32_t de = target[E_AXIS] - position[E_AXIS];
  1323. /* <-- add a slash to enable
  1324. SERIAL_ECHOPAIR(" _populate_block FR:", fr_mm_s);
  1325. SERIAL_ECHOPAIR(" A:", target[A_AXIS]);
  1326. SERIAL_ECHOPAIR(" (", da);
  1327. SERIAL_ECHOPAIR(" steps) B:", target[B_AXIS]);
  1328. SERIAL_ECHOPAIR(" (", db);
  1329. SERIAL_ECHOPAIR(" steps) C:", target[C_AXIS]);
  1330. SERIAL_ECHOPAIR(" (", dc);
  1331. SERIAL_ECHOPAIR(" steps) E:", target[E_AXIS]);
  1332. SERIAL_ECHOPAIR(" (", de);
  1333. SERIAL_ECHOLNPGM(" steps)");
  1334. //*/
  1335. #if ENABLED(PREVENT_COLD_EXTRUSION) || ENABLED(PREVENT_LENGTHY_EXTRUDE)
  1336. if (de) {
  1337. #if ENABLED(PREVENT_COLD_EXTRUSION)
  1338. if (thermalManager.tooColdToExtrude(extruder)) {
  1339. position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
  1340. #if HAS_POSITION_FLOAT
  1341. position_float[E_AXIS] = target_float[E_AXIS];
  1342. #endif
  1343. de = 0; // no difference
  1344. SERIAL_ECHO_START();
  1345. SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
  1346. }
  1347. #endif // PREVENT_COLD_EXTRUSION
  1348. #if ENABLED(PREVENT_LENGTHY_EXTRUDE)
  1349. if (ABS(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
  1350. position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
  1351. #if HAS_POSITION_FLOAT
  1352. position_float[E_AXIS] = target_float[E_AXIS];
  1353. #endif
  1354. de = 0; // no difference
  1355. SERIAL_ECHO_START();
  1356. SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
  1357. }
  1358. #endif // PREVENT_LENGTHY_EXTRUDE
  1359. }
  1360. #endif // PREVENT_COLD_EXTRUSION || PREVENT_LENGTHY_EXTRUDE
  1361. // Compute direction bit-mask for this block
  1362. uint8_t dm = 0;
  1363. #if CORE_IS_XY
  1364. if (da < 0) SBI(dm, X_HEAD); // Save the real Extruder (head) direction in X Axis
  1365. if (db < 0) SBI(dm, Y_HEAD); // ...and Y
  1366. if (dc < 0) SBI(dm, Z_AXIS);
  1367. if (da + db < 0) SBI(dm, A_AXIS); // Motor A direction
  1368. if (CORESIGN(da - db) < 0) SBI(dm, B_AXIS); // Motor B direction
  1369. #elif CORE_IS_XZ
  1370. if (da < 0) SBI(dm, X_HEAD); // Save the real Extruder (head) direction in X Axis
  1371. if (db < 0) SBI(dm, Y_AXIS);
  1372. if (dc < 0) SBI(dm, Z_HEAD); // ...and Z
  1373. if (da + dc < 0) SBI(dm, A_AXIS); // Motor A direction
  1374. if (CORESIGN(da - dc) < 0) SBI(dm, C_AXIS); // Motor C direction
  1375. #elif CORE_IS_YZ
  1376. if (da < 0) SBI(dm, X_AXIS);
  1377. if (db < 0) SBI(dm, Y_HEAD); // Save the real Extruder (head) direction in Y Axis
  1378. if (dc < 0) SBI(dm, Z_HEAD); // ...and Z
  1379. if (db + dc < 0) SBI(dm, B_AXIS); // Motor B direction
  1380. if (CORESIGN(db - dc) < 0) SBI(dm, C_AXIS); // Motor C direction
  1381. #else
  1382. if (da < 0) SBI(dm, X_AXIS);
  1383. if (db < 0) SBI(dm, Y_AXIS);
  1384. if (dc < 0) SBI(dm, Z_AXIS);
  1385. #endif
  1386. if (de < 0) SBI(dm, E_AXIS);
  1387. const float esteps_float = de * e_factor[extruder];
  1388. const uint32_t esteps = ABS(esteps_float) + 0.5;
  1389. // Clear all flags, including the "busy" bit
  1390. block->flag = 0x00;
  1391. // Set direction bits
  1392. block->direction_bits = dm;
  1393. // Number of steps for each axis
  1394. // See http://www.corexy.com/theory.html
  1395. #if CORE_IS_XY
  1396. block->steps[A_AXIS] = ABS(da + db);
  1397. block->steps[B_AXIS] = ABS(da - db);
  1398. block->steps[Z_AXIS] = ABS(dc);
  1399. #elif CORE_IS_XZ
  1400. block->steps[A_AXIS] = ABS(da + dc);
  1401. block->steps[Y_AXIS] = ABS(db);
  1402. block->steps[C_AXIS] = ABS(da - dc);
  1403. #elif CORE_IS_YZ
  1404. block->steps[X_AXIS] = ABS(da);
  1405. block->steps[B_AXIS] = ABS(db + dc);
  1406. block->steps[C_AXIS] = ABS(db - dc);
  1407. #elif IS_SCARA
  1408. block->steps[A_AXIS] = ABS(da);
  1409. block->steps[B_AXIS] = ABS(db);
  1410. block->steps[Z_AXIS] = ABS(dc);
  1411. #else
  1412. // default non-h-bot planning
  1413. block->steps[A_AXIS] = ABS(da);
  1414. block->steps[B_AXIS] = ABS(db);
  1415. block->steps[C_AXIS] = ABS(dc);
  1416. #endif
  1417. block->steps[E_AXIS] = esteps;
  1418. block->step_event_count = MAX4(block->steps[A_AXIS], block->steps[B_AXIS], block->steps[C_AXIS], esteps);
  1419. // Bail if this is a zero-length block
  1420. if (block->step_event_count < MIN_STEPS_PER_SEGMENT) return false;
  1421. // For a mixing extruder, get a magnified esteps for each
  1422. #if ENABLED(MIXING_EXTRUDER)
  1423. for (uint8_t i = 0; i < MIXING_STEPPERS; i++)
  1424. block->mix_steps[i] = mixing_factor[i] * (
  1425. #if ENABLED(LIN_ADVANCE)
  1426. esteps
  1427. #else
  1428. block->step_event_count
  1429. #endif
  1430. );
  1431. #endif
  1432. #if FAN_COUNT > 0
  1433. for (uint8_t i = 0; i < FAN_COUNT; i++) block->fan_speed[i] = fanSpeeds[i];
  1434. #endif
  1435. #if ENABLED(BARICUDA)
  1436. block->valve_pressure = baricuda_valve_pressure;
  1437. block->e_to_p_pressure = baricuda_e_to_p_pressure;
  1438. #endif
  1439. block->active_extruder = extruder;
  1440. #if ENABLED(AUTO_POWER_CONTROL)
  1441. if (block->steps[X_AXIS] || block->steps[Y_AXIS] || block->steps[Z_AXIS])
  1442. powerManager.power_on();
  1443. #endif
  1444. // Enable active axes
  1445. #if CORE_IS_XY
  1446. if (block->steps[A_AXIS] || block->steps[B_AXIS]) {
  1447. enable_X();
  1448. enable_Y();
  1449. }
  1450. #if DISABLED(Z_LATE_ENABLE)
  1451. if (block->steps[Z_AXIS]) enable_Z();
  1452. #endif
  1453. #elif CORE_IS_XZ
  1454. if (block->steps[A_AXIS] || block->steps[C_AXIS]) {
  1455. enable_X();
  1456. enable_Z();
  1457. }
  1458. if (block->steps[Y_AXIS]) enable_Y();
  1459. #elif CORE_IS_YZ
  1460. if (block->steps[B_AXIS] || block->steps[C_AXIS]) {
  1461. enable_Y();
  1462. enable_Z();
  1463. }
  1464. if (block->steps[X_AXIS]) enable_X();
  1465. #else
  1466. if (block->steps[X_AXIS]) enable_X();
  1467. if (block->steps[Y_AXIS]) enable_Y();
  1468. #if DISABLED(Z_LATE_ENABLE)
  1469. if (block->steps[Z_AXIS]) enable_Z();
  1470. #endif
  1471. #endif
  1472. // Enable extruder(s)
  1473. if (esteps) {
  1474. #if ENABLED(AUTO_POWER_CONTROL)
  1475. powerManager.power_on();
  1476. #endif
  1477. #if ENABLED(DISABLE_INACTIVE_EXTRUDER) // Enable only the selected extruder
  1478. #define DISABLE_IDLE_E(N) if (!g_uc_extruder_last_move[N]) disable_E##N();
  1479. for (uint8_t i = 0; i < EXTRUDERS; i++)
  1480. if (g_uc_extruder_last_move[i] > 0) g_uc_extruder_last_move[i]--;
  1481. switch (extruder) {
  1482. case 0:
  1483. #if EXTRUDERS > 1
  1484. DISABLE_IDLE_E(1);
  1485. #if EXTRUDERS > 2
  1486. DISABLE_IDLE_E(2);
  1487. #if EXTRUDERS > 3
  1488. DISABLE_IDLE_E(3);
  1489. #if EXTRUDERS > 4
  1490. DISABLE_IDLE_E(4);
  1491. #endif // EXTRUDERS > 4
  1492. #endif // EXTRUDERS > 3
  1493. #endif // EXTRUDERS > 2
  1494. #endif // EXTRUDERS > 1
  1495. enable_E0();
  1496. g_uc_extruder_last_move[0] = (BLOCK_BUFFER_SIZE) * 2;
  1497. #if ENABLED(DUAL_X_CARRIAGE) || ENABLED(DUAL_NOZZLE_DUPLICATION_MODE)
  1498. if (extruder_duplication_enabled) {
  1499. enable_E1();
  1500. g_uc_extruder_last_move[1] = (BLOCK_BUFFER_SIZE) * 2;
  1501. }
  1502. #endif
  1503. break;
  1504. #if EXTRUDERS > 1
  1505. case 1:
  1506. DISABLE_IDLE_E(0);
  1507. #if EXTRUDERS > 2
  1508. DISABLE_IDLE_E(2);
  1509. #if EXTRUDERS > 3
  1510. DISABLE_IDLE_E(3);
  1511. #if EXTRUDERS > 4
  1512. DISABLE_IDLE_E(4);
  1513. #endif // EXTRUDERS > 4
  1514. #endif // EXTRUDERS > 3
  1515. #endif // EXTRUDERS > 2
  1516. enable_E1();
  1517. g_uc_extruder_last_move[1] = (BLOCK_BUFFER_SIZE) * 2;
  1518. break;
  1519. #if EXTRUDERS > 2
  1520. case 2:
  1521. DISABLE_IDLE_E(0);
  1522. DISABLE_IDLE_E(1);
  1523. #if EXTRUDERS > 3
  1524. DISABLE_IDLE_E(3);
  1525. #if EXTRUDERS > 4
  1526. DISABLE_IDLE_E(4);
  1527. #endif
  1528. #endif
  1529. enable_E2();
  1530. g_uc_extruder_last_move[2] = (BLOCK_BUFFER_SIZE) * 2;
  1531. break;
  1532. #if EXTRUDERS > 3
  1533. case 3:
  1534. DISABLE_IDLE_E(0);
  1535. DISABLE_IDLE_E(1);
  1536. DISABLE_IDLE_E(2);
  1537. #if EXTRUDERS > 4
  1538. DISABLE_IDLE_E(4);
  1539. #endif
  1540. enable_E3();
  1541. g_uc_extruder_last_move[3] = (BLOCK_BUFFER_SIZE) * 2;
  1542. break;
  1543. #if EXTRUDERS > 4
  1544. case 4:
  1545. DISABLE_IDLE_E(0);
  1546. DISABLE_IDLE_E(1);
  1547. DISABLE_IDLE_E(2);
  1548. DISABLE_IDLE_E(3);
  1549. enable_E4();
  1550. g_uc_extruder_last_move[4] = (BLOCK_BUFFER_SIZE) * 2;
  1551. break;
  1552. #endif // EXTRUDERS > 4
  1553. #endif // EXTRUDERS > 3
  1554. #endif // EXTRUDERS > 2
  1555. #endif // EXTRUDERS > 1
  1556. }
  1557. #else
  1558. enable_E0();
  1559. enable_E1();
  1560. enable_E2();
  1561. enable_E3();
  1562. enable_E4();
  1563. #endif
  1564. }
  1565. if (esteps)
  1566. NOLESS(fr_mm_s, min_feedrate_mm_s);
  1567. else
  1568. NOLESS(fr_mm_s, min_travel_feedrate_mm_s);
  1569. /**
  1570. * This part of the code calculates the total length of the movement.
  1571. * For cartesian bots, the X_AXIS is the real X movement and same for Y_AXIS.
  1572. * But for corexy bots, that is not true. The "X_AXIS" and "Y_AXIS" motors (that should be named to A_AXIS
  1573. * and B_AXIS) cannot be used for X and Y length, because A=X+Y and B=X-Y.
  1574. * So we need to create other 2 "AXIS", named X_HEAD and Y_HEAD, meaning the real displacement of the Head.
  1575. * Having the real displacement of the head, we can calculate the total movement length and apply the desired speed.
  1576. */
  1577. #if IS_CORE
  1578. float delta_mm[Z_HEAD + 1];
  1579. #if CORE_IS_XY
  1580. delta_mm[X_HEAD] = da * steps_to_mm[A_AXIS];
  1581. delta_mm[Y_HEAD] = db * steps_to_mm[B_AXIS];
  1582. delta_mm[Z_AXIS] = dc * steps_to_mm[Z_AXIS];
  1583. delta_mm[A_AXIS] = (da + db) * steps_to_mm[A_AXIS];
  1584. delta_mm[B_AXIS] = CORESIGN(da - db) * steps_to_mm[B_AXIS];
  1585. #elif CORE_IS_XZ
  1586. delta_mm[X_HEAD] = da * steps_to_mm[A_AXIS];
  1587. delta_mm[Y_AXIS] = db * steps_to_mm[Y_AXIS];
  1588. delta_mm[Z_HEAD] = dc * steps_to_mm[C_AXIS];
  1589. delta_mm[A_AXIS] = (da + dc) * steps_to_mm[A_AXIS];
  1590. delta_mm[C_AXIS] = CORESIGN(da - dc) * steps_to_mm[C_AXIS];
  1591. #elif CORE_IS_YZ
  1592. delta_mm[X_AXIS] = da * steps_to_mm[X_AXIS];
  1593. delta_mm[Y_HEAD] = db * steps_to_mm[B_AXIS];
  1594. delta_mm[Z_HEAD] = dc * steps_to_mm[C_AXIS];
  1595. delta_mm[B_AXIS] = (db + dc) * steps_to_mm[B_AXIS];
  1596. delta_mm[C_AXIS] = CORESIGN(db - dc) * steps_to_mm[C_AXIS];
  1597. #endif
  1598. #else
  1599. float delta_mm[ABCE];
  1600. delta_mm[A_AXIS] = da * steps_to_mm[A_AXIS];
  1601. delta_mm[B_AXIS] = db * steps_to_mm[B_AXIS];
  1602. delta_mm[C_AXIS] = dc * steps_to_mm[C_AXIS];
  1603. #endif
  1604. delta_mm[E_AXIS] = esteps_float * steps_to_mm[E_AXIS_N];
  1605. if (block->steps[A_AXIS] < MIN_STEPS_PER_SEGMENT && block->steps[B_AXIS] < MIN_STEPS_PER_SEGMENT && block->steps[C_AXIS] < MIN_STEPS_PER_SEGMENT) {
  1606. block->millimeters = ABS(delta_mm[E_AXIS]);
  1607. }
  1608. else if (!millimeters) {
  1609. block->millimeters = SQRT(
  1610. #if CORE_IS_XY
  1611. sq(delta_mm[X_HEAD]) + sq(delta_mm[Y_HEAD]) + sq(delta_mm[Z_AXIS])
  1612. #elif CORE_IS_XZ
  1613. sq(delta_mm[X_HEAD]) + sq(delta_mm[Y_AXIS]) + sq(delta_mm[Z_HEAD])
  1614. #elif CORE_IS_YZ
  1615. sq(delta_mm[X_AXIS]) + sq(delta_mm[Y_HEAD]) + sq(delta_mm[Z_HEAD])
  1616. #else
  1617. sq(delta_mm[X_AXIS]) + sq(delta_mm[Y_AXIS]) + sq(delta_mm[Z_AXIS])
  1618. #endif
  1619. );
  1620. }
  1621. else
  1622. block->millimeters = millimeters;
  1623. const float inverse_millimeters = 1.0 / block->millimeters; // Inverse millimeters to remove multiple divides
  1624. // Calculate inverse time for this move. No divide by zero due to previous checks.
  1625. // Example: At 120mm/s a 60mm move takes 0.5s. So this will give 2.0.
  1626. float inverse_secs = fr_mm_s * inverse_millimeters;
  1627. const uint8_t moves_queued = movesplanned();
  1628. // Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
  1629. #if ENABLED(SLOWDOWN) || ENABLED(ULTRA_LCD) || defined(XY_FREQUENCY_LIMIT)
  1630. // Segment time im micro seconds
  1631. uint32_t segment_time_us = LROUND(1000000.0 / inverse_secs);
  1632. #endif
  1633. #if ENABLED(SLOWDOWN)
  1634. if (WITHIN(moves_queued, 2, (BLOCK_BUFFER_SIZE) / 2 - 1)) {
  1635. if (segment_time_us < min_segment_time_us) {
  1636. // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
  1637. const uint32_t nst = segment_time_us + LROUND(2 * (min_segment_time_us - segment_time_us) / moves_queued);
  1638. inverse_secs = 1000000.0 / nst;
  1639. #if defined(XY_FREQUENCY_LIMIT) || ENABLED(ULTRA_LCD)
  1640. segment_time_us = nst;
  1641. #endif
  1642. }
  1643. }
  1644. #endif
  1645. #if ENABLED(ULTRA_LCD)
  1646. // Protect the access to the position.
  1647. const bool was_enabled = STEPPER_ISR_ENABLED();
  1648. if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
  1649. block_buffer_runtime_us += segment_time_us;
  1650. if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
  1651. #endif
  1652. block->nominal_speed_sqr = sq(block->millimeters * inverse_secs); // (mm/sec)^2 Always > 0
  1653. block->nominal_rate = CEIL(block->step_event_count * inverse_secs); // (step/sec) Always > 0
  1654. #if ENABLED(FILAMENT_WIDTH_SENSOR)
  1655. static float filwidth_e_count = 0, filwidth_delay_dist = 0;
  1656. //FMM update ring buffer used for delay with filament measurements
  1657. if (extruder == FILAMENT_SENSOR_EXTRUDER_NUM && filwidth_delay_index[1] >= 0) { //only for extruder with filament sensor and if ring buffer is initialized
  1658. constexpr int MMD_CM = MAX_MEASUREMENT_DELAY + 1, MMD_MM = MMD_CM * 10;
  1659. // increment counters with next move in e axis
  1660. filwidth_e_count += delta_mm[E_AXIS];
  1661. filwidth_delay_dist += delta_mm[E_AXIS];
  1662. // Only get new measurements on forward E movement
  1663. if (!UNEAR_ZERO(filwidth_e_count)) {
  1664. // Loop the delay distance counter (modulus by the mm length)
  1665. while (filwidth_delay_dist >= MMD_MM) filwidth_delay_dist -= MMD_MM;
  1666. // Convert into an index into the measurement array
  1667. filwidth_delay_index[0] = int8_t(filwidth_delay_dist * 0.1);
  1668. // If the index has changed (must have gone forward)...
  1669. if (filwidth_delay_index[0] != filwidth_delay_index[1]) {
  1670. filwidth_e_count = 0; // Reset the E movement counter
  1671. const int8_t meas_sample = thermalManager.widthFil_to_size_ratio();
  1672. do {
  1673. filwidth_delay_index[1] = (filwidth_delay_index[1] + 1) % MMD_CM; // The next unused slot
  1674. measurement_delay[filwidth_delay_index[1]] = meas_sample; // Store the measurement
  1675. } while (filwidth_delay_index[0] != filwidth_delay_index[1]); // More slots to fill?
  1676. }
  1677. }
  1678. }
  1679. #endif
  1680. // Calculate and limit speed in mm/sec for each axis
  1681. float current_speed[NUM_AXIS], speed_factor = 1.0; // factor <1 decreases speed
  1682. LOOP_XYZE(i) {
  1683. const float cs = ABS((current_speed[i] = delta_mm[i] * inverse_secs));
  1684. #if ENABLED(DISTINCT_E_FACTORS)
  1685. if (i == E_AXIS) i += extruder;
  1686. #endif
  1687. if (cs > max_feedrate_mm_s[i]) NOMORE(speed_factor, max_feedrate_mm_s[i] / cs);
  1688. }
  1689. // Max segment time in µs.
  1690. #ifdef XY_FREQUENCY_LIMIT
  1691. // Check and limit the xy direction change frequency
  1692. const unsigned char direction_change = block->direction_bits ^ old_direction_bits;
  1693. old_direction_bits = block->direction_bits;
  1694. segment_time_us = LROUND((float)segment_time_us / speed_factor);
  1695. uint32_t xs0 = axis_segment_time_us[X_AXIS][0],
  1696. xs1 = axis_segment_time_us[X_AXIS][1],
  1697. xs2 = axis_segment_time_us[X_AXIS][2],
  1698. ys0 = axis_segment_time_us[Y_AXIS][0],
  1699. ys1 = axis_segment_time_us[Y_AXIS][1],
  1700. ys2 = axis_segment_time_us[Y_AXIS][2];
  1701. if (TEST(direction_change, X_AXIS)) {
  1702. xs2 = axis_segment_time_us[X_AXIS][2] = xs1;
  1703. xs1 = axis_segment_time_us[X_AXIS][1] = xs0;
  1704. xs0 = 0;
  1705. }
  1706. xs0 = axis_segment_time_us[X_AXIS][0] = xs0 + segment_time_us;
  1707. if (TEST(direction_change, Y_AXIS)) {
  1708. ys2 = axis_segment_time_us[Y_AXIS][2] = axis_segment_time_us[Y_AXIS][1];
  1709. ys1 = axis_segment_time_us[Y_AXIS][1] = axis_segment_time_us[Y_AXIS][0];
  1710. ys0 = 0;
  1711. }
  1712. ys0 = axis_segment_time_us[Y_AXIS][0] = ys0 + segment_time_us;
  1713. const uint32_t max_x_segment_time = MAX3(xs0, xs1, xs2),
  1714. max_y_segment_time = MAX3(ys0, ys1, ys2),
  1715. min_xy_segment_time = MIN(max_x_segment_time, max_y_segment_time);
  1716. if (min_xy_segment_time < MAX_FREQ_TIME_US) {
  1717. const float low_sf = speed_factor * min_xy_segment_time / (MAX_FREQ_TIME_US);
  1718. NOMORE(speed_factor, low_sf);
  1719. }
  1720. #endif // XY_FREQUENCY_LIMIT
  1721. // Correct the speed
  1722. if (speed_factor < 1.0) {
  1723. LOOP_XYZE(i) current_speed[i] *= speed_factor;
  1724. block->nominal_rate *= speed_factor;
  1725. block->nominal_speed_sqr = block->nominal_speed_sqr * sq(speed_factor);
  1726. }
  1727. // Compute and limit the acceleration rate for the trapezoid generator.
  1728. const float steps_per_mm = block->step_event_count * inverse_millimeters;
  1729. uint32_t accel;
  1730. if (!block->steps[A_AXIS] && !block->steps[B_AXIS] && !block->steps[C_AXIS]) {
  1731. // convert to: acceleration steps/sec^2
  1732. accel = CEIL(retract_acceleration * steps_per_mm);
  1733. #if ENABLED(LIN_ADVANCE)
  1734. block->use_advance_lead = false;
  1735. #endif
  1736. }
  1737. else {
  1738. #define LIMIT_ACCEL_LONG(AXIS,INDX) do{ \
  1739. if (block->steps[AXIS] && max_acceleration_steps_per_s2[AXIS+INDX] < accel) { \
  1740. const uint32_t comp = max_acceleration_steps_per_s2[AXIS+INDX] * block->step_event_count; \
  1741. if (accel * block->steps[AXIS] > comp) accel = comp / block->steps[AXIS]; \
  1742. } \
  1743. }while(0)
  1744. #define LIMIT_ACCEL_FLOAT(AXIS,INDX) do{ \
  1745. if (block->steps[AXIS] && max_acceleration_steps_per_s2[AXIS+INDX] < accel) { \
  1746. const float comp = (float)max_acceleration_steps_per_s2[AXIS+INDX] * (float)block->step_event_count; \
  1747. if ((float)accel * (float)block->steps[AXIS] > comp) accel = comp / (float)block->steps[AXIS]; \
  1748. } \
  1749. }while(0)
  1750. // Start with print or travel acceleration
  1751. accel = CEIL((esteps ? acceleration : travel_acceleration) * steps_per_mm);
  1752. #if ENABLED(LIN_ADVANCE)
  1753. /**
  1754. *
  1755. * Use LIN_ADVANCE for blocks if all these are true:
  1756. *
  1757. * esteps : This is a print move, because we checked for A, B, C steps before.
  1758. *
  1759. * extruder_advance_K : There is an advance factor set.
  1760. *
  1761. * de > 0 : Extruder is running forward (e.g., for "Wipe while retracting" (Slic3r) or "Combing" (Cura) moves)
  1762. */
  1763. block->use_advance_lead = esteps
  1764. && extruder_advance_K
  1765. && de > 0;
  1766. if (block->use_advance_lead) {
  1767. block->e_D_ratio = (target_float[E_AXIS] - position_float[E_AXIS]) /
  1768. #if IS_KINEMATIC
  1769. block->millimeters
  1770. #else
  1771. SQRT(sq(target_float[X_AXIS] - position_float[X_AXIS])
  1772. + sq(target_float[Y_AXIS] - position_float[Y_AXIS])
  1773. + sq(target_float[Z_AXIS] - position_float[Z_AXIS]))
  1774. #endif
  1775. ;
  1776. // Check for unusual high e_D ratio to detect if a retract move was combined with the last print move due to min. steps per segment. Never execute this with advance!
  1777. // This assumes no one will use a retract length of 0mm < retr_length < ~0.2mm and no one will print 100mm wide lines using 3mm filament or 35mm wide lines using 1.75mm filament.
  1778. if (block->e_D_ratio > 3.0)
  1779. block->use_advance_lead = false;
  1780. else {
  1781. const uint32_t max_accel_steps_per_s2 = max_jerk[E_AXIS] / (extruder_advance_K * block->e_D_ratio) * steps_per_mm;
  1782. #if ENABLED(LA_DEBUG)
  1783. if (accel > max_accel_steps_per_s2)
  1784. SERIAL_ECHOLNPGM("Acceleration limited.");
  1785. #endif
  1786. NOMORE(accel, max_accel_steps_per_s2);
  1787. }
  1788. }
  1789. #endif
  1790. #if ENABLED(DISTINCT_E_FACTORS)
  1791. #define ACCEL_IDX extruder
  1792. #else
  1793. #define ACCEL_IDX 0
  1794. #endif
  1795. // Limit acceleration per axis
  1796. if (block->step_event_count <= cutoff_long) {
  1797. LIMIT_ACCEL_LONG(A_AXIS, 0);
  1798. LIMIT_ACCEL_LONG(B_AXIS, 0);
  1799. LIMIT_ACCEL_LONG(C_AXIS, 0);
  1800. LIMIT_ACCEL_LONG(E_AXIS, ACCEL_IDX);
  1801. }
  1802. else {
  1803. LIMIT_ACCEL_FLOAT(A_AXIS, 0);
  1804. LIMIT_ACCEL_FLOAT(B_AXIS, 0);
  1805. LIMIT_ACCEL_FLOAT(C_AXIS, 0);
  1806. LIMIT_ACCEL_FLOAT(E_AXIS, ACCEL_IDX);
  1807. }
  1808. }
  1809. block->acceleration_steps_per_s2 = accel;
  1810. block->acceleration = accel / steps_per_mm;
  1811. #if DISABLED(S_CURVE_ACCELERATION)
  1812. block->acceleration_rate = (uint32_t)(accel * (4096.0 * 4096.0 / (HAL_STEPPER_TIMER_RATE)));
  1813. #endif
  1814. #if ENABLED(LIN_ADVANCE)
  1815. if (block->use_advance_lead) {
  1816. block->advance_speed = (HAL_STEPPER_TIMER_RATE) / (extruder_advance_K * block->e_D_ratio * block->acceleration * axis_steps_per_mm[E_AXIS_N]);
  1817. #if ENABLED(LA_DEBUG)
  1818. if (extruder_advance_K * block->e_D_ratio * block->acceleration * 2 < SQRT(block->nominal_speed_sqr) * block->e_D_ratio)
  1819. SERIAL_ECHOLNPGM("More than 2 steps per eISR loop executed.");
  1820. if (block->advance_speed < 200)
  1821. SERIAL_ECHOLNPGM("eISR running at > 10kHz.");
  1822. #endif
  1823. }
  1824. #endif
  1825. float vmax_junction_sqr; // Initial limit on the segment entry velocity (mm/s)^2
  1826. #if ENABLED(JUNCTION_DEVIATION)
  1827. /**
  1828. * Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
  1829. * Let a circle be tangent to both previous and current path line segments, where the junction
  1830. * deviation is defined as the distance from the junction to the closest edge of the circle,
  1831. * colinear with the circle center. The circular segment joining the two paths represents the
  1832. * path of centripetal acceleration. Solve for max velocity based on max acceleration about the
  1833. * radius of the circle, defined indirectly by junction deviation. This may be also viewed as
  1834. * path width or max_jerk in the previous Grbl version. This approach does not actually deviate
  1835. * from path, but used as a robust way to compute cornering speeds, as it takes into account the
  1836. * nonlinearities of both the junction angle and junction velocity.
  1837. *
  1838. * NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path
  1839. * mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact
  1840. * stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here
  1841. * is exactly the same. Instead of motioning all the way to junction point, the machine will
  1842. * just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform
  1843. * a continuous mode path, but ARM-based microcontrollers most certainly do.
  1844. *
  1845. * NOTE: The max junction speed is a fixed value, since machine acceleration limits cannot be
  1846. * changed dynamically during operation nor can the line move geometry. This must be kept in
  1847. * memory in the event of a feedrate override changing the nominal speeds of blocks, which can
  1848. * change the overall maximum entry speed conditions of all blocks.
  1849. *
  1850. * #######
  1851. * https://github.com/MarlinFirmware/Marlin/issues/10341#issuecomment-388191754
  1852. *
  1853. * hoffbaked: on May 10 2018 tuned and improved the GRBL algorithm for Marlin:
  1854. Okay! It seems to be working good. I somewhat arbitrarily cut it off at 1mm
  1855. on then on anything with less sides than an octagon. With this, and the
  1856. reverse pass actually recalculating things, a corner acceleration value
  1857. of 1000 junction deviation of .05 are pretty reasonable. If the cycles
  1858. can be spared, a better acos could be used. For all I know, it may be
  1859. already calculated in a different place. */
  1860. // Unit vector of previous path line segment
  1861. static float previous_unit_vec[
  1862. #if ENABLED(JUNCTION_DEVIATION_INCLUDE_E)
  1863. XYZE
  1864. #else
  1865. XYZ
  1866. #endif
  1867. ];
  1868. float unit_vec[] = {
  1869. delta_mm[A_AXIS] * inverse_millimeters,
  1870. delta_mm[B_AXIS] * inverse_millimeters,
  1871. delta_mm[C_AXIS] * inverse_millimeters
  1872. #if ENABLED(JUNCTION_DEVIATION_INCLUDE_E)
  1873. , delta_mm[E_AXIS] * inverse_millimeters
  1874. #endif
  1875. };
  1876. // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
  1877. if (moves_queued && !UNEAR_ZERO(previous_nominal_speed_sqr)) {
  1878. // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
  1879. // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
  1880. float junction_cos_theta = -previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
  1881. -previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
  1882. -previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS]
  1883. #if ENABLED(JUNCTION_DEVIATION_INCLUDE_E)
  1884. -previous_unit_vec[E_AXIS] * unit_vec[E_AXIS]
  1885. #endif
  1886. ;
  1887. // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta).
  1888. if (junction_cos_theta > 0.999999) {
  1889. // For a 0 degree acute junction, just set minimum junction speed.
  1890. vmax_junction_sqr = sq(MINIMUM_PLANNER_SPEED);
  1891. }
  1892. else {
  1893. NOLESS(junction_cos_theta, -0.999999); // Check for numerical round-off to avoid divide by zero.
  1894. float junction_unit_vec[JD_AXES] = {
  1895. unit_vec[X_AXIS] - previous_unit_vec[X_AXIS],
  1896. unit_vec[Y_AXIS] - previous_unit_vec[Y_AXIS],
  1897. unit_vec[Z_AXIS] - previous_unit_vec[Z_AXIS]
  1898. #if ENABLED(JUNCTION_DEVIATION_INCLUDE_E)
  1899. , unit_vec[E_AXIS] - previous_unit_vec[E_AXIS]
  1900. #endif
  1901. };
  1902. // Convert delta vector to unit vector
  1903. normalize_junction_vector(junction_unit_vec);
  1904. const float junction_acceleration = limit_value_by_axis_maximum(block->acceleration, junction_unit_vec),
  1905. sin_theta_d2 = SQRT(0.5 * (1.0 - junction_cos_theta)); // Trig half angle identity. Always positive.
  1906. vmax_junction_sqr = (junction_acceleration * JUNCTION_DEVIATION_MM * sin_theta_d2) / (1.0 - sin_theta_d2);
  1907. if (block->millimeters < 1.0) {
  1908. // Fast acos approximation, minus the error bar to be safe
  1909. const float junction_theta = (RADIANS(-40) * sq(junction_cos_theta) - RADIANS(50)) * junction_cos_theta + RADIANS(90) - 0.18;
  1910. // If angle is greater than 135 degrees (octagon), find speed for approximate arc
  1911. if (junction_theta > RADIANS(135)) {
  1912. const float limit_sqr = block->millimeters / (RADIANS(180) - junction_theta) * junction_acceleration;
  1913. NOMORE(vmax_junction_sqr, limit_sqr);
  1914. }
  1915. }
  1916. }
  1917. // Get the lowest speed
  1918. vmax_junction_sqr = MIN3(vmax_junction_sqr, block->nominal_speed_sqr, previous_nominal_speed_sqr);
  1919. }
  1920. else // Init entry speed to zero. Assume it starts from rest. Planner will correct this later.
  1921. vmax_junction_sqr = 0.0;
  1922. COPY(previous_unit_vec, unit_vec);
  1923. #else // Classic Jerk Limiting
  1924. /**
  1925. * Adapted from Průša MKS firmware
  1926. * https://github.com/prusa3d/Prusa-Firmware
  1927. *
  1928. * Start with a safe speed (from which the machine may halt to stop immediately).
  1929. */
  1930. // Exit speed limited by a jerk to full halt of a previous last segment
  1931. static float previous_safe_speed;
  1932. const float nominal_speed = SQRT(block->nominal_speed_sqr);
  1933. float safe_speed = nominal_speed;
  1934. uint8_t limited = 0;
  1935. LOOP_XYZE(i) {
  1936. const float jerk = ABS(current_speed[i]), maxj = max_jerk[i];
  1937. if (jerk > maxj) {
  1938. if (limited) {
  1939. const float mjerk = maxj * nominal_speed;
  1940. if (jerk * safe_speed > mjerk) safe_speed = mjerk / jerk;
  1941. }
  1942. else {
  1943. ++limited;
  1944. safe_speed = maxj;
  1945. }
  1946. }
  1947. }
  1948. float vmax_junction;
  1949. if (moves_queued && !UNEAR_ZERO(previous_nominal_speed_sqr)) {
  1950. // Estimate a maximum velocity allowed at a joint of two successive segments.
  1951. // If this maximum velocity allowed is lower than the minimum of the entry / exit safe velocities,
  1952. // then the machine is not coasting anymore and the safe entry / exit velocities shall be used.
  1953. // Factor to multiply the previous / current nominal velocities to get componentwise limited velocities.
  1954. float v_factor = 1;
  1955. limited = 0;
  1956. // The junction velocity will be shared between successive segments. Limit the junction velocity to their minimum.
  1957. // Pick the smaller of the nominal speeds. Higher speed shall not be achieved at the junction during coasting.
  1958. const float previous_nominal_speed = SQRT(previous_nominal_speed_sqr);
  1959. vmax_junction = MIN(nominal_speed, previous_nominal_speed);
  1960. // Now limit the jerk in all axes.
  1961. const float smaller_speed_factor = vmax_junction / previous_nominal_speed;
  1962. LOOP_XYZE(axis) {
  1963. // Limit an axis. We have to differentiate: coasting, reversal of an axis, full stop.
  1964. float v_exit = previous_speed[axis] * smaller_speed_factor,
  1965. v_entry = current_speed[axis];
  1966. if (limited) {
  1967. v_exit *= v_factor;
  1968. v_entry *= v_factor;
  1969. }
  1970. // Calculate jerk depending on whether the axis is coasting in the same direction or reversing.
  1971. const float jerk = (v_exit > v_entry)
  1972. ? // coasting axis reversal
  1973. ( (v_entry > 0 || v_exit < 0) ? (v_exit - v_entry) : MAX(v_exit, -v_entry) )
  1974. : // v_exit <= v_entry coasting axis reversal
  1975. ( (v_entry < 0 || v_exit > 0) ? (v_entry - v_exit) : MAX(-v_exit, v_entry) );
  1976. if (jerk > max_jerk[axis]) {
  1977. v_factor *= max_jerk[axis] / jerk;
  1978. ++limited;
  1979. }
  1980. }
  1981. if (limited) vmax_junction *= v_factor;
  1982. // Now the transition velocity is known, which maximizes the shared exit / entry velocity while
  1983. // respecting the jerk factors, it may be possible, that applying separate safe exit / entry velocities will achieve faster prints.
  1984. const float vmax_junction_threshold = vmax_junction * 0.99f;
  1985. if (previous_safe_speed > vmax_junction_threshold && safe_speed > vmax_junction_threshold)
  1986. vmax_junction = safe_speed;
  1987. }
  1988. else
  1989. vmax_junction = safe_speed;
  1990. previous_safe_speed = safe_speed;
  1991. vmax_junction_sqr = sq(vmax_junction);
  1992. #endif // Classic Jerk Limiting
  1993. // Max entry speed of this block equals the max exit speed of the previous block.
  1994. block->max_entry_speed_sqr = vmax_junction_sqr;
  1995. // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
  1996. const float v_allowable_sqr = max_allowable_speed_sqr(-block->acceleration, sq(MINIMUM_PLANNER_SPEED), block->millimeters);
  1997. // If we are trying to add a split block, start with the
  1998. // max. allowed speed to avoid an interrupted first move.
  1999. block->entry_speed_sqr = !split_move ? sq(MINIMUM_PLANNER_SPEED) : MIN(vmax_junction_sqr, v_allowable_sqr);
  2000. // Initialize planner efficiency flags
  2001. // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
  2002. // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
  2003. // the current block and next block junction speeds are guaranteed to always be at their maximum
  2004. // junction speeds in deceleration and acceleration, respectively. This is due to how the current
  2005. // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
  2006. // the reverse and forward planners, the corresponding block junction speed will always be at the
  2007. // the maximum junction speed and may always be ignored for any speed reduction checks.
  2008. block->flag |= block->nominal_speed_sqr <= v_allowable_sqr ? BLOCK_FLAG_RECALCULATE | BLOCK_FLAG_NOMINAL_LENGTH : BLOCK_FLAG_RECALCULATE;
  2009. // Update previous path unit_vector and nominal speed
  2010. COPY(previous_speed, current_speed);
  2011. previous_nominal_speed_sqr = block->nominal_speed_sqr;
  2012. // Update the position
  2013. static_assert(COUNT(target) > 1, "Parameter to _buffer_steps must be (&target)[XYZE]!");
  2014. COPY(position, target);
  2015. #if HAS_POSITION_FLOAT
  2016. COPY(position_float, target_float);
  2017. #endif
  2018. // Movement was accepted
  2019. return true;
  2020. } // _populate_block()
  2021. /**
  2022. * Planner::buffer_sync_block
  2023. * Add a block to the buffer that just updates the position
  2024. */
  2025. void Planner::buffer_sync_block() {
  2026. // Wait for the next available block
  2027. uint8_t next_buffer_head;
  2028. block_t * const block = get_next_free_block(next_buffer_head);
  2029. // Clear block
  2030. memset(block, 0, sizeof(block_t));
  2031. block->flag = BLOCK_FLAG_SYNC_POSITION;
  2032. block->position[A_AXIS] = position[A_AXIS];
  2033. block->position[B_AXIS] = position[B_AXIS];
  2034. block->position[C_AXIS] = position[C_AXIS];
  2035. block->position[E_AXIS] = position[E_AXIS];
  2036. // If this is the first added movement, reload the delay, otherwise, cancel it.
  2037. if (block_buffer_head == block_buffer_tail) {
  2038. // If it was the first queued block, restart the 1st block delivery delay, to
  2039. // give the planner an opportunity to queue more movements and plan them
  2040. // As there are no queued movements, the Stepper ISR will not touch this
  2041. // variable, so there is no risk setting this here (but it MUST be done
  2042. // before the following line!!)
  2043. delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE;
  2044. }
  2045. block_buffer_head = next_buffer_head;
  2046. stepper.wake_up();
  2047. } // buffer_sync_block()
  2048. /**
  2049. * Planner::buffer_segment
  2050. *
  2051. * Add a new linear movement to the buffer in axis units.
  2052. *
  2053. * Leveling and kinematics should be applied ahead of calling this.
  2054. *
  2055. * a,b,c,e - target positions in mm and/or degrees
  2056. * fr_mm_s - (target) speed of the move
  2057. * extruder - target extruder
  2058. * millimeters - the length of the movement, if known
  2059. */
  2060. bool Planner::buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder, const float &millimeters/*=0.0*/) {
  2061. // If we are cleaning, do not accept queuing of movements
  2062. if (cleaning_buffer_counter) return false;
  2063. // When changing extruders recalculate steps corresponding to the E position
  2064. #if ENABLED(DISTINCT_E_FACTORS)
  2065. if (last_extruder != extruder && axis_steps_per_mm[E_AXIS_N] != axis_steps_per_mm[E_AXIS + last_extruder]) {
  2066. position[E_AXIS] = LROUND(position[E_AXIS] * axis_steps_per_mm[E_AXIS_N] * steps_to_mm[E_AXIS + last_extruder]);
  2067. last_extruder = extruder;
  2068. }
  2069. #endif
  2070. // The target position of the tool in absolute steps
  2071. // Calculate target position in absolute steps
  2072. const int32_t target[ABCE] = {
  2073. LROUND(a * axis_steps_per_mm[A_AXIS]),
  2074. LROUND(b * axis_steps_per_mm[B_AXIS]),
  2075. LROUND(c * axis_steps_per_mm[C_AXIS]),
  2076. LROUND(e * axis_steps_per_mm[E_AXIS_N])
  2077. };
  2078. #if HAS_POSITION_FLOAT
  2079. const float target_float[XYZE] = { a, b, c, e };
  2080. #endif
  2081. // DRYRUN prevents E moves from taking place
  2082. if (DEBUGGING(DRYRUN)) {
  2083. position[E_AXIS] = target[E_AXIS];
  2084. #if HAS_POSITION_FLOAT
  2085. position_float[E_AXIS] = e;
  2086. #endif
  2087. }
  2088. /* <-- add a slash to enable
  2089. SERIAL_ECHOPAIR(" buffer_segment FR:", fr_mm_s);
  2090. #if IS_KINEMATIC
  2091. SERIAL_ECHOPAIR(" A:", a);
  2092. SERIAL_ECHOPAIR(" (", position[A_AXIS]);
  2093. SERIAL_ECHOPAIR("->", target[A_AXIS]);
  2094. SERIAL_ECHOPAIR(") B:", b);
  2095. #else
  2096. SERIAL_ECHOPAIR(" X:", a);
  2097. SERIAL_ECHOPAIR(" (", position[X_AXIS]);
  2098. SERIAL_ECHOPAIR("->", target[X_AXIS]);
  2099. SERIAL_ECHOPAIR(") Y:", b);
  2100. #endif
  2101. SERIAL_ECHOPAIR(" (", position[Y_AXIS]);
  2102. SERIAL_ECHOPAIR("->", target[Y_AXIS]);
  2103. #if ENABLED(DELTA)
  2104. SERIAL_ECHOPAIR(") C:", c);
  2105. #else
  2106. SERIAL_ECHOPAIR(") Z:", c);
  2107. #endif
  2108. SERIAL_ECHOPAIR(" (", position[Z_AXIS]);
  2109. SERIAL_ECHOPAIR("->", target[Z_AXIS]);
  2110. SERIAL_ECHOPAIR(") E:", e);
  2111. SERIAL_ECHOPAIR(" (", position[E_AXIS]);
  2112. SERIAL_ECHOPAIR("->", target[E_AXIS]);
  2113. SERIAL_ECHOLNPGM(")");
  2114. //*/
  2115. // Queue the movement
  2116. if (
  2117. !_buffer_steps(target
  2118. #if HAS_POSITION_FLOAT
  2119. , target_float
  2120. #endif
  2121. , fr_mm_s, extruder, millimeters
  2122. )
  2123. ) return false;
  2124. stepper.wake_up();
  2125. return true;
  2126. } // buffer_segment()
  2127. /**
  2128. * Directly set the planner XYZ position (and stepper positions)
  2129. * converting mm (or angles for SCARA) into steps.
  2130. *
  2131. * On CORE machines stepper ABC will be translated from the given XYZ.
  2132. */
  2133. void Planner::_set_position_mm(const float &a, const float &b, const float &c, const float &e) {
  2134. #if ENABLED(DISTINCT_E_FACTORS)
  2135. #define _EINDEX (E_AXIS + active_extruder)
  2136. last_extruder = active_extruder;
  2137. #else
  2138. #define _EINDEX E_AXIS
  2139. #endif
  2140. position[A_AXIS] = LROUND(a * axis_steps_per_mm[A_AXIS]),
  2141. position[B_AXIS] = LROUND(b * axis_steps_per_mm[B_AXIS]),
  2142. position[C_AXIS] = LROUND(c * axis_steps_per_mm[C_AXIS]),
  2143. position[E_AXIS] = LROUND(e * axis_steps_per_mm[_EINDEX]);
  2144. #if HAS_POSITION_FLOAT
  2145. position_float[A_AXIS] = a;
  2146. position_float[B_AXIS] = b;
  2147. position_float[C_AXIS] = c;
  2148. position_float[E_AXIS] = e;
  2149. #endif
  2150. if (has_blocks_queued()) {
  2151. //previous_nominal_speed_sqr = 0.0; // Reset planner junction speeds. Assume start from rest.
  2152. //ZERO(previous_speed);
  2153. buffer_sync_block();
  2154. }
  2155. else
  2156. stepper.set_position(position[A_AXIS], position[B_AXIS], position[C_AXIS], position[E_AXIS]);
  2157. }
  2158. void Planner::set_position_mm_kinematic(const float (&cart)[XYZE]) {
  2159. #if PLANNER_LEVELING
  2160. float raw[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] };
  2161. apply_leveling(raw);
  2162. #else
  2163. const float (&raw)[XYZE] = cart;
  2164. #endif
  2165. #if IS_KINEMATIC
  2166. inverse_kinematics(raw);
  2167. _set_position_mm(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS]);
  2168. #else
  2169. _set_position_mm(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS]);
  2170. #endif
  2171. }
  2172. /**
  2173. * Setters for planner position (also setting stepper position).
  2174. */
  2175. void Planner::set_position_mm(const AxisEnum axis, const float &v) {
  2176. #if ENABLED(DISTINCT_E_FACTORS)
  2177. const uint8_t axis_index = axis + (axis == E_AXIS ? active_extruder : 0);
  2178. last_extruder = active_extruder;
  2179. #else
  2180. const uint8_t axis_index = axis;
  2181. #endif
  2182. position[axis] = LROUND(v * axis_steps_per_mm[axis_index]);
  2183. #if HAS_POSITION_FLOAT
  2184. position_float[axis] = v;
  2185. #endif
  2186. if (has_blocks_queued()) {
  2187. //previous_speed[axis] = 0.0;
  2188. buffer_sync_block();
  2189. }
  2190. else
  2191. stepper.set_position(axis, position[axis]);
  2192. }
  2193. // Recalculate the steps/s^2 acceleration rates, based on the mm/s^2
  2194. void Planner::reset_acceleration_rates() {
  2195. #if ENABLED(DISTINCT_E_FACTORS)
  2196. #define AXIS_CONDITION (i < E_AXIS || i == E_AXIS + active_extruder)
  2197. #else
  2198. #define AXIS_CONDITION true
  2199. #endif
  2200. uint32_t highest_rate = 1;
  2201. LOOP_XYZE_N(i) {
  2202. max_acceleration_steps_per_s2[i] = max_acceleration_mm_per_s2[i] * axis_steps_per_mm[i];
  2203. if (AXIS_CONDITION) NOLESS(highest_rate, max_acceleration_steps_per_s2[i]);
  2204. }
  2205. cutoff_long = 4294967295UL / highest_rate; // 0xFFFFFFFFUL
  2206. }
  2207. // Recalculate position, steps_to_mm if axis_steps_per_mm changes!
  2208. void Planner::refresh_positioning() {
  2209. LOOP_XYZE_N(i) steps_to_mm[i] = 1.0 / axis_steps_per_mm[i];
  2210. set_position_mm_kinematic(current_position);
  2211. reset_acceleration_rates();
  2212. }
  2213. #if ENABLED(AUTOTEMP)
  2214. void Planner::autotemp_M104_M109() {
  2215. if ((autotemp_enabled = parser.seen('F'))) autotemp_factor = parser.value_float();
  2216. if (parser.seen('S')) autotemp_min = parser.value_celsius();
  2217. if (parser.seen('B')) autotemp_max = parser.value_celsius();
  2218. }
  2219. #endif