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

planner.cpp 111KB

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