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.

motion.cpp 82KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353135413551356135713581359136013611362136313641365136613671368136913701371137213731374137513761377137813791380138113821383138413851386138713881389139013911392139313941395139613971398139914001401140214031404140514061407140814091410141114121413141414151416141714181419142014211422142314241425142614271428142914301431143214331434143514361437143814391440144114421443144414451446144714481449145014511452145314541455145614571458145914601461146214631464146514661467146814691470147114721473147414751476147714781479148014811482148314841485148614871488148914901491149214931494149514961497149814991500150115021503150415051506150715081509151015111512151315141515151615171518151915201521152215231524152515261527152815291530153115321533153415351536153715381539154015411542154315441545154615471548154915501551155215531554155515561557155815591560156115621563156415651566156715681569157015711572157315741575157615771578157915801581158215831584158515861587158815891590159115921593159415951596159715981599160016011602160316041605160616071608160916101611161216131614161516161617161816191620162116221623162416251626162716281629163016311632163316341635163616371638163916401641164216431644164516461647164816491650165116521653165416551656165716581659166016611662166316641665166616671668166916701671167216731674167516761677167816791680168116821683168416851686168716881689169016911692169316941695169616971698169917001701170217031704170517061707170817091710171117121713171417151716171717181719172017211722172317241725172617271728172917301731173217331734173517361737173817391740174117421743174417451746174717481749175017511752175317541755175617571758175917601761176217631764176517661767176817691770177117721773177417751776177717781779178017811782178317841785178617871788178917901791179217931794179517961797179817991800180118021803180418051806180718081809181018111812181318141815181618171818181918201821182218231824182518261827182818291830183118321833183418351836183718381839184018411842184318441845184618471848184918501851185218531854185518561857185818591860186118621863186418651866186718681869187018711872187318741875187618771878187918801881188218831884188518861887188818891890189118921893189418951896189718981899190019011902190319041905190619071908190919101911191219131914191519161917191819191920192119221923192419251926192719281929193019311932193319341935193619371938193919401941194219431944194519461947194819491950195119521953195419551956195719581959196019611962196319641965196619671968196919701971197219731974197519761977197819791980198119821983198419851986198719881989199019911992199319941995199619971998199920002001200220032004200520062007200820092010201120122013201420152016201720182019202020212022202320242025202620272028202920302031203220332034203520362037203820392040204120422043204420452046204720482049205020512052205320542055205620572058205920602061206220632064206520662067206820692070207120722073207420752076207720782079208020812082208320842085208620872088208920902091209220932094209520962097209820992100210121022103210421052106210721082109211021112112211321142115211621172118211921202121212221232124212521262127212821292130213121322133213421352136213721382139214021412142214321442145214621472148214921502151215221532154215521562157215821592160216121622163216421652166216721682169217021712172217321742175217621772178217921802181218221832184218521862187218821892190219121922193219421952196219721982199220022012202220322042205220622072208220922102211221222132214221522162217221822192220222122222223222422252226222722282229223022312232223322342235223622372238223922402241224222432244224522462247224822492250225122522253225422552256225722582259226022612262226322642265226622672268226922702271227222732274227522762277227822792280228122822283228422852286228722882289229022912292229322942295229622972298229923002301230223032304230523062307230823092310231123122313231423152316231723182319232023212322232323242325232623272328232923302331233223332334233523362337233823392340234123422343234423452346234723482349235023512352235323542355235623572358235923602361236223632364236523662367236823692370237123722373237423752376237723782379238023812382238323842385
  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 <https://www.gnu.org/licenses/>.
  20. *
  21. */
  22. /**
  23. * motion.cpp
  24. */
  25. #include "motion.h"
  26. #include "endstops.h"
  27. #include "stepper.h"
  28. #include "planner.h"
  29. #include "temperature.h"
  30. #include "../gcode/gcode.h"
  31. #include "../lcd/marlinui.h"
  32. #include "../inc/MarlinConfig.h"
  33. #if IS_SCARA
  34. #include "../libs/buzzer.h"
  35. #include "../lcd/marlinui.h"
  36. #endif
  37. #if HAS_BED_PROBE
  38. #include "probe.h"
  39. #endif
  40. #if HAS_LEVELING
  41. #include "../feature/bedlevel/bedlevel.h"
  42. #endif
  43. #if ENABLED(BLTOUCH)
  44. #include "../feature/bltouch.h"
  45. #endif
  46. #if HAS_FILAMENT_SENSOR
  47. #include "../feature/runout.h"
  48. #endif
  49. #if ENABLED(SENSORLESS_HOMING)
  50. #include "../feature/tmc_util.h"
  51. #endif
  52. #if ENABLED(FWRETRACT)
  53. #include "../feature/fwretract.h"
  54. #endif
  55. #if ENABLED(BABYSTEP_DISPLAY_TOTAL)
  56. #include "../feature/babystep.h"
  57. #endif
  58. #define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
  59. #include "../core/debug_out.h"
  60. // Relative Mode. Enable with G91, disable with G90.
  61. bool relative_mode; // = false;
  62. /**
  63. * Cartesian Current Position
  64. * Used to track the native machine position as moves are queued.
  65. * Used by 'line_to_current_position' to do a move after changing it.
  66. * Used by 'sync_plan_position' to update 'planner.position'.
  67. */
  68. #ifdef Z_IDLE_HEIGHT
  69. #define Z_INIT_POS Z_IDLE_HEIGHT
  70. #else
  71. #define Z_INIT_POS Z_HOME_POS
  72. #endif
  73. xyze_pos_t current_position = LOGICAL_AXIS_ARRAY(0, X_HOME_POS, Y_HOME_POS, Z_INIT_POS, I_HOME_POS, J_HOME_POS, K_HOME_POS, U_HOME_POS, V_HOME_POS, W_HOME_POS);
  74. /**
  75. * Cartesian Destination
  76. * The destination for a move, filled in by G-code movement commands,
  77. * and expected by functions like 'prepare_line_to_destination'.
  78. * G-codes can set destination using 'get_destination_from_command'
  79. */
  80. xyze_pos_t destination; // {0}
  81. // G60/G61 Position Save and Return
  82. #if SAVED_POSITIONS
  83. uint8_t saved_slots[(SAVED_POSITIONS + 7) >> 3];
  84. xyze_pos_t stored_position[SAVED_POSITIONS];
  85. #endif
  86. // The active extruder (tool). Set with T<extruder> command.
  87. #if HAS_MULTI_EXTRUDER
  88. uint8_t active_extruder = 0; // = 0
  89. #endif
  90. #if ENABLED(LCD_SHOW_E_TOTAL)
  91. float e_move_accumulator; // = 0
  92. #endif
  93. // Extruder offsets
  94. #if HAS_HOTEND_OFFSET
  95. xyz_pos_t hotend_offset[HOTENDS]; // Initialized by settings.load()
  96. void reset_hotend_offsets() {
  97. constexpr float tmp[XYZ][HOTENDS] = { HOTEND_OFFSET_X, HOTEND_OFFSET_Y, HOTEND_OFFSET_Z };
  98. static_assert(
  99. !tmp[X_AXIS][0] && !tmp[Y_AXIS][0] && !tmp[Z_AXIS][0],
  100. "Offsets for the first hotend must be 0.0."
  101. );
  102. // Transpose from [XYZ][HOTENDS] to [HOTENDS][XYZ]
  103. HOTEND_LOOP() LOOP_ABC(a) hotend_offset[e][a] = tmp[a][e];
  104. TERN_(DUAL_X_CARRIAGE, hotend_offset[1].x = _MAX(X2_HOME_POS, X2_MAX_POS));
  105. }
  106. #endif
  107. // The feedrate for the current move, often used as the default if
  108. // no other feedrate is specified. Overridden for special moves.
  109. // Set by the last G0 through G5 command's "F" parameter.
  110. // Functions that override this for custom moves *must always* restore it!
  111. feedRate_t feedrate_mm_s = MMM_TO_MMS(1500);
  112. int16_t feedrate_percentage = 100;
  113. // Cartesian conversion result goes here:
  114. xyz_pos_t cartes;
  115. #if IS_KINEMATIC
  116. abce_pos_t delta;
  117. #if HAS_SCARA_OFFSET
  118. abc_pos_t scara_home_offset;
  119. #endif
  120. #if HAS_SOFTWARE_ENDSTOPS
  121. float delta_max_radius, delta_max_radius_2;
  122. #elif IS_SCARA
  123. constexpr float delta_max_radius = SCARA_PRINTABLE_RADIUS,
  124. delta_max_radius_2 = sq(SCARA_PRINTABLE_RADIUS);
  125. #else // DELTA
  126. constexpr float delta_max_radius = DELTA_PRINTABLE_RADIUS,
  127. delta_max_radius_2 = sq(DELTA_PRINTABLE_RADIUS);
  128. #endif
  129. #endif
  130. /**
  131. * The workspace can be offset by some commands, or
  132. * these offsets may be omitted to save on computation.
  133. */
  134. #if HAS_POSITION_SHIFT
  135. // The distance that XYZ has been offset by G92. Reset by G28.
  136. xyz_pos_t position_shift{0};
  137. #endif
  138. #if HAS_HOME_OFFSET
  139. // This offset is added to the configured home position.
  140. // Set by M206, M428, or menu item. Saved to EEPROM.
  141. xyz_pos_t home_offset{0};
  142. #endif
  143. #if HAS_HOME_OFFSET && HAS_POSITION_SHIFT
  144. // The above two are combined to save on computes
  145. xyz_pos_t workspace_offset{0};
  146. #endif
  147. #if HAS_ABL_NOT_UBL
  148. feedRate_t xy_probe_feedrate_mm_s = MMM_TO_MMS(XY_PROBE_FEEDRATE);
  149. #endif
  150. /**
  151. * Output the current position to serial
  152. */
  153. inline void report_more_positions() {
  154. stepper.report_positions();
  155. TERN_(IS_SCARA, scara_report_positions());
  156. }
  157. // Report the logical position for a given machine position
  158. inline void report_logical_position(const xyze_pos_t &rpos) {
  159. const xyze_pos_t lpos = rpos.asLogical();
  160. SERIAL_ECHOPGM_P(
  161. LIST_N(DOUBLE(NUM_AXES),
  162. X_LBL, lpos.x,
  163. SP_Y_LBL, lpos.y,
  164. SP_Z_LBL, lpos.z,
  165. SP_I_LBL, lpos.i,
  166. SP_J_LBL, lpos.j,
  167. SP_K_LBL, lpos.k,
  168. SP_U_LBL, lpos.u,
  169. SP_V_LBL, lpos.v,
  170. SP_W_LBL, lpos.w
  171. )
  172. #if HAS_EXTRUDERS
  173. , SP_E_LBL, lpos.e
  174. #endif
  175. );
  176. }
  177. // Report the real current position according to the steppers.
  178. // Forward kinematics and un-leveling are applied.
  179. void report_real_position() {
  180. get_cartesian_from_steppers();
  181. xyze_pos_t npos = LOGICAL_AXIS_ARRAY(
  182. planner.get_axis_position_mm(E_AXIS),
  183. cartes.x, cartes.y, cartes.z,
  184. cartes.i, cartes.j, cartes.k,
  185. cartes.u, cartes.v, cartes.w
  186. );
  187. TERN_(HAS_POSITION_MODIFIERS, planner.unapply_modifiers(npos, true));
  188. report_logical_position(npos);
  189. report_more_positions();
  190. }
  191. // Report the logical current position according to the most recent G-code command
  192. void report_current_position() {
  193. report_logical_position(current_position);
  194. report_more_positions();
  195. }
  196. /**
  197. * Report the logical current position according to the most recent G-code command.
  198. * The planner.position always corresponds to the last G-code too. This makes M114
  199. * suitable for debugging kinematics and leveling while avoiding planner sync that
  200. * definitively interrupts the printing flow.
  201. */
  202. void report_current_position_projected() {
  203. report_logical_position(current_position);
  204. stepper.report_a_position(planner.position);
  205. }
  206. #if ENABLED(AUTO_REPORT_POSITION)
  207. AutoReporter<PositionReport> position_auto_reporter;
  208. #endif
  209. #if EITHER(FULL_REPORT_TO_HOST_FEATURE, REALTIME_REPORTING_COMMANDS)
  210. M_StateEnum M_State_grbl = M_INIT;
  211. /**
  212. * Output the current grbl compatible state to serial while moving
  213. */
  214. void report_current_grblstate_moving() { SERIAL_ECHOLNPGM("S_XYZ:", int(M_State_grbl)); }
  215. /**
  216. * Output the current position (processed) to serial while moving
  217. */
  218. void report_current_position_moving() {
  219. get_cartesian_from_steppers();
  220. const xyz_pos_t lpos = cartes.asLogical();
  221. SERIAL_ECHOPGM_P(
  222. LIST_N(DOUBLE(NUM_AXES),
  223. X_LBL, lpos.x,
  224. SP_Y_LBL, lpos.y,
  225. SP_Z_LBL, lpos.z,
  226. SP_I_LBL, lpos.i,
  227. SP_J_LBL, lpos.j,
  228. SP_K_LBL, lpos.k,
  229. SP_U_LBL, lpos.u,
  230. SP_V_LBL, lpos.v,
  231. SP_W_LBL, lpos.w
  232. )
  233. #if HAS_EXTRUDERS
  234. , SP_E_LBL, current_position.e
  235. #endif
  236. );
  237. stepper.report_positions();
  238. TERN_(IS_SCARA, scara_report_positions());
  239. report_current_grblstate_moving();
  240. }
  241. /**
  242. * Set a Grbl-compatible state from the current marlin_state
  243. */
  244. M_StateEnum grbl_state_for_marlin_state() {
  245. switch (marlin_state) {
  246. case MF_INITIALIZING: return M_INIT;
  247. case MF_SD_COMPLETE: return M_ALARM;
  248. case MF_WAITING: return M_IDLE;
  249. case MF_STOPPED: return M_END;
  250. case MF_RUNNING: return M_RUNNING;
  251. case MF_PAUSED: return M_HOLD;
  252. case MF_KILLED: return M_ERROR;
  253. default: return M_IDLE;
  254. }
  255. }
  256. #endif
  257. #if IS_KINEMATIC
  258. bool position_is_reachable(const_float_t rx, const_float_t ry, const float inset/*=0*/) {
  259. bool can_reach;
  260. #if ENABLED(DELTA)
  261. can_reach = HYPOT2(rx, ry) <= sq(DELTA_PRINTABLE_RADIUS - inset + fslop);
  262. #elif ENABLED(AXEL_TPARA)
  263. const float R2 = HYPOT2(rx - TPARA_OFFSET_X, ry - TPARA_OFFSET_Y);
  264. can_reach = (
  265. R2 <= sq(L1 + L2) - inset
  266. #if MIDDLE_DEAD_ZONE_R > 0
  267. && R2 >= sq(float(MIDDLE_DEAD_ZONE_R))
  268. #endif
  269. );
  270. #elif IS_SCARA
  271. const float R2 = HYPOT2(rx - SCARA_OFFSET_X, ry - SCARA_OFFSET_Y);
  272. can_reach = (
  273. R2 <= sq(L1 + L2) - inset
  274. #if MIDDLE_DEAD_ZONE_R > 0
  275. && R2 >= sq(float(MIDDLE_DEAD_ZONE_R))
  276. #endif
  277. );
  278. #elif ENABLED(POLARGRAPH)
  279. const float d1 = rx - (draw_area_min.x),
  280. d2 = (draw_area_max.x) - rx,
  281. y = ry - (draw_area_max.y),
  282. a = HYPOT(d1, y),
  283. b = HYPOT(d2, y);
  284. can_reach = (
  285. a < polargraph_max_belt_len + 1
  286. && b < polargraph_max_belt_len + 1
  287. && (a + b) > _MIN(draw_area_size.x, draw_area_size.y)
  288. );
  289. #endif
  290. return can_reach;
  291. }
  292. #else // CARTESIAN
  293. // Return true if the given position is within the machine bounds.
  294. bool position_is_reachable(const_float_t rx, const_float_t ry) {
  295. if (!COORDINATE_OKAY(ry, Y_MIN_POS - fslop, Y_MAX_POS + fslop)) return false;
  296. #if ENABLED(DUAL_X_CARRIAGE)
  297. if (active_extruder)
  298. return COORDINATE_OKAY(rx, X2_MIN_POS - fslop, X2_MAX_POS + fslop);
  299. else
  300. return COORDINATE_OKAY(rx, X1_MIN_POS - fslop, X1_MAX_POS + fslop);
  301. #else
  302. return COORDINATE_OKAY(rx, X_MIN_POS - fslop, X_MAX_POS + fslop);
  303. #endif
  304. }
  305. #endif // CARTESIAN
  306. void home_if_needed(const bool keeplev/*=false*/) {
  307. if (!all_axes_trusted()) gcode.home_all_axes(keeplev);
  308. }
  309. /**
  310. * Run out the planner buffer and re-sync the current
  311. * position from the last-updated stepper positions.
  312. */
  313. void quickstop_stepper() {
  314. planner.quick_stop();
  315. planner.synchronize();
  316. set_current_from_steppers_for_axis(ALL_AXES_ENUM);
  317. sync_plan_position();
  318. }
  319. #if ENABLED(REALTIME_REPORTING_COMMANDS)
  320. void quickpause_stepper() {
  321. planner.quick_pause();
  322. //planner.synchronize();
  323. }
  324. void quickresume_stepper() {
  325. planner.quick_resume();
  326. //planner.synchronize();
  327. }
  328. #endif
  329. /**
  330. * Set the planner/stepper positions directly from current_position with
  331. * no kinematic translation. Used for homing axes and cartesian/core syncing.
  332. */
  333. void sync_plan_position() {
  334. if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position", current_position);
  335. planner.set_position_mm(current_position);
  336. }
  337. #if HAS_EXTRUDERS
  338. void sync_plan_position_e() { planner.set_e_position_mm(current_position.e); }
  339. #endif
  340. /**
  341. * Get the stepper positions in the cartes[] array.
  342. * Forward kinematics are applied for DELTA and SCARA.
  343. *
  344. * The result is in the current coordinate space with
  345. * leveling applied. The coordinates need to be run through
  346. * unapply_leveling to obtain the "ideal" coordinates
  347. * suitable for current_position, etc.
  348. */
  349. void get_cartesian_from_steppers() {
  350. #if ENABLED(DELTA)
  351. forward_kinematics(planner.get_axis_positions_mm());
  352. #elif IS_SCARA
  353. forward_kinematics(
  354. planner.get_axis_position_degrees(A_AXIS), planner.get_axis_position_degrees(B_AXIS)
  355. OPTARG(AXEL_TPARA, planner.get_axis_position_degrees(C_AXIS))
  356. );
  357. cartes.z = planner.get_axis_position_mm(Z_AXIS);
  358. #else
  359. NUM_AXIS_CODE(
  360. cartes.x = planner.get_axis_position_mm(X_AXIS),
  361. cartes.y = planner.get_axis_position_mm(Y_AXIS),
  362. cartes.z = planner.get_axis_position_mm(Z_AXIS),
  363. cartes.i = planner.get_axis_position_mm(I_AXIS),
  364. cartes.j = planner.get_axis_position_mm(J_AXIS),
  365. cartes.k = planner.get_axis_position_mm(K_AXIS),
  366. cartes.u = planner.get_axis_position_mm(U_AXIS),
  367. cartes.v = planner.get_axis_position_mm(V_AXIS),
  368. cartes.w = planner.get_axis_position_mm(W_AXIS)
  369. );
  370. #endif
  371. }
  372. /**
  373. * Set the current_position for an axis based on
  374. * the stepper positions, removing any leveling that
  375. * may have been applied.
  376. *
  377. * To prevent small shifts in axis position always call
  378. * sync_plan_position after updating axes with this.
  379. *
  380. * To keep hosts in sync, always call report_current_position
  381. * after updating the current_position.
  382. */
  383. void set_current_from_steppers_for_axis(const AxisEnum axis) {
  384. get_cartesian_from_steppers();
  385. xyze_pos_t pos = cartes;
  386. TERN_(HAS_EXTRUDERS, pos.e = planner.get_axis_position_mm(E_AXIS));
  387. TERN_(HAS_POSITION_MODIFIERS, planner.unapply_modifiers(pos, true));
  388. if (axis == ALL_AXES_ENUM)
  389. current_position = pos;
  390. else
  391. current_position[axis] = pos[axis];
  392. }
  393. /**
  394. * Move the planner to the current position from wherever it last moved
  395. * (or from wherever it has been told it is located).
  396. */
  397. void line_to_current_position(const_feedRate_t fr_mm_s/*=feedrate_mm_s*/) {
  398. planner.buffer_line(current_position, fr_mm_s);
  399. }
  400. #if HAS_EXTRUDERS
  401. void unscaled_e_move(const_float_t length, const_feedRate_t fr_mm_s) {
  402. TERN_(HAS_FILAMENT_SENSOR, runout.reset());
  403. current_position.e += length / planner.e_factor[active_extruder];
  404. line_to_current_position(fr_mm_s);
  405. planner.synchronize();
  406. }
  407. #endif
  408. #if IS_KINEMATIC
  409. /**
  410. * Buffer a fast move without interpolation. Set current_position to destination
  411. */
  412. void prepare_fast_move_to_destination(const_feedRate_t scaled_fr_mm_s/*=MMS_SCALED(feedrate_mm_s)*/) {
  413. if (DEBUGGING(LEVELING)) DEBUG_POS("prepare_fast_move_to_destination", destination);
  414. #if UBL_SEGMENTED
  415. // UBL segmented line will do Z-only moves in single segment
  416. bedlevel.line_to_destination_segmented(scaled_fr_mm_s);
  417. #else
  418. if (current_position == destination) return;
  419. planner.buffer_line(destination, scaled_fr_mm_s);
  420. #endif
  421. current_position = destination;
  422. }
  423. #endif // IS_KINEMATIC
  424. /**
  425. * Do a fast or normal move to 'destination' with an optional FR.
  426. * - Move at normal speed regardless of feedrate percentage.
  427. * - Extrude the specified length regardless of flow percentage.
  428. */
  429. void _internal_move_to_destination(const_feedRate_t fr_mm_s/*=0.0f*/
  430. OPTARG(IS_KINEMATIC, const bool is_fast/*=false*/)
  431. ) {
  432. const feedRate_t old_feedrate = feedrate_mm_s;
  433. if (fr_mm_s) feedrate_mm_s = fr_mm_s;
  434. const uint16_t old_pct = feedrate_percentage;
  435. feedrate_percentage = 100;
  436. #if HAS_EXTRUDERS
  437. const float old_fac = planner.e_factor[active_extruder];
  438. planner.e_factor[active_extruder] = 1.0f;
  439. #endif
  440. if (TERN0(IS_KINEMATIC, is_fast))
  441. TERN(IS_KINEMATIC, prepare_fast_move_to_destination(), NOOP);
  442. else
  443. prepare_line_to_destination();
  444. feedrate_mm_s = old_feedrate;
  445. feedrate_percentage = old_pct;
  446. TERN_(HAS_EXTRUDERS, planner.e_factor[active_extruder] = old_fac);
  447. }
  448. /**
  449. * Plan a move to (X, Y, Z, [I, [J, [K...]]]) and set the current_position
  450. * Plan a move to (X, Y, Z, [I, [J, [K...]]]) with separation of Z from other components.
  451. *
  452. * - If Z is moving up, the Z move is done before XY, etc.
  453. * - If Z is moving down, the Z move is done after XY, etc.
  454. * - Delta may lower Z first to get into the free motion zone.
  455. * - Before returning, wait for the planner buffer to empty.
  456. */
  457. void do_blocking_move_to(NUM_AXIS_ARGS(const float), const_feedRate_t fr_mm_s/*=0.0f*/) {
  458. DEBUG_SECTION(log_move, "do_blocking_move_to", DEBUGGING(LEVELING));
  459. if (DEBUGGING(LEVELING)) DEBUG_XYZ("> ", NUM_AXIS_ARGS());
  460. const feedRate_t xy_feedrate = fr_mm_s ?: feedRate_t(XY_PROBE_FEEDRATE_MM_S);
  461. #if HAS_Z_AXIS
  462. const feedRate_t z_feedrate = fr_mm_s ?: homing_feedrate(Z_AXIS);
  463. #endif
  464. SECONDARY_AXIS_CODE(
  465. const feedRate_t i_feedrate = fr_mm_s ?: homing_feedrate(I_AXIS),
  466. const feedRate_t j_feedrate = fr_mm_s ?: homing_feedrate(J_AXIS),
  467. const feedRate_t k_feedrate = fr_mm_s ?: homing_feedrate(K_AXIS),
  468. const feedRate_t u_feedrate = fr_mm_s ?: homing_feedrate(U_AXIS),
  469. const feedRate_t v_feedrate = fr_mm_s ?: homing_feedrate(V_AXIS),
  470. const feedRate_t w_feedrate = fr_mm_s ?: homing_feedrate(W_AXIS)
  471. );
  472. #if IS_KINEMATIC
  473. if (!position_is_reachable(x, y)) return;
  474. destination = current_position; // sync destination at the start
  475. #endif
  476. #if ENABLED(DELTA)
  477. REMEMBER(fr, feedrate_mm_s, xy_feedrate);
  478. if (DEBUGGING(LEVELING)) DEBUG_POS("destination = current_position", destination);
  479. // when in the danger zone
  480. if (current_position.z > delta_clip_start_height) {
  481. if (z > delta_clip_start_height) { // staying in the danger zone
  482. destination.set(x, y, z); // move directly (uninterpolated)
  483. prepare_internal_fast_move_to_destination(); // set current_position from destination
  484. if (DEBUGGING(LEVELING)) DEBUG_POS("danger zone move", current_position);
  485. return;
  486. }
  487. destination.z = delta_clip_start_height;
  488. prepare_internal_fast_move_to_destination(); // set current_position from destination
  489. if (DEBUGGING(LEVELING)) DEBUG_POS("zone border move", current_position);
  490. }
  491. if (z > current_position.z) { // raising?
  492. destination.z = z;
  493. prepare_internal_fast_move_to_destination(z_feedrate); // set current_position from destination
  494. if (DEBUGGING(LEVELING)) DEBUG_POS("z raise move", current_position);
  495. }
  496. destination.set(x, y);
  497. prepare_internal_move_to_destination(); // set current_position from destination
  498. if (DEBUGGING(LEVELING)) DEBUG_POS("xy move", current_position);
  499. if (z < current_position.z) { // lowering?
  500. destination.z = z;
  501. prepare_internal_fast_move_to_destination(z_feedrate); // set current_position from destination
  502. if (DEBUGGING(LEVELING)) DEBUG_POS("z lower move", current_position);
  503. }
  504. #elif IS_SCARA
  505. // If Z needs to raise, do it before moving XY
  506. if (destination.z < z) { destination.z = z; prepare_internal_fast_move_to_destination(z_feedrate); }
  507. destination.set(x, y); prepare_internal_fast_move_to_destination(xy_feedrate);
  508. // If Z needs to lower, do it after moving XY
  509. if (destination.z > z) { destination.z = z; prepare_internal_fast_move_to_destination(z_feedrate); }
  510. #else
  511. #if HAS_Z_AXIS // If Z needs to raise, do it before moving XY
  512. if (current_position.z < z) { current_position.z = z; line_to_current_position(z_feedrate); }
  513. #endif
  514. current_position.set(x, y); line_to_current_position(xy_feedrate);
  515. #if HAS_I_AXIS
  516. current_position.i = i; line_to_current_position(i_feedrate);
  517. #endif
  518. #if HAS_J_AXIS
  519. current_position.j = j; line_to_current_position(j_feedrate);
  520. #endif
  521. #if HAS_K_AXIS
  522. current_position.k = k; line_to_current_position(k_feedrate);
  523. #endif
  524. #if HAS_U_AXIS
  525. current_position.u = u; line_to_current_position(u_feedrate);
  526. #endif
  527. #if HAS_V_AXIS
  528. current_position.v = v; line_to_current_position(v_feedrate);
  529. #endif
  530. #if HAS_W_AXIS
  531. current_position.w = w; line_to_current_position(w_feedrate);
  532. #endif
  533. #if HAS_Z_AXIS
  534. // If Z needs to lower, do it after moving XY
  535. if (current_position.z > z) { current_position.z = z; line_to_current_position(z_feedrate); }
  536. #endif
  537. #endif
  538. planner.synchronize();
  539. }
  540. void do_blocking_move_to(const xy_pos_t &raw, const_feedRate_t fr_mm_s/*=0.0f*/) {
  541. do_blocking_move_to(NUM_AXIS_LIST(raw.x, raw.y, current_position.z, current_position.i, current_position.j, current_position.k,
  542. current_position.u, current_position.v, current_position.w), fr_mm_s);
  543. }
  544. void do_blocking_move_to(const xyz_pos_t &raw, const_feedRate_t fr_mm_s/*=0.0f*/) {
  545. do_blocking_move_to(NUM_AXIS_ELEM(raw), fr_mm_s);
  546. }
  547. void do_blocking_move_to(const xyze_pos_t &raw, const_feedRate_t fr_mm_s/*=0.0f*/) {
  548. do_blocking_move_to(NUM_AXIS_ELEM(raw), fr_mm_s);
  549. }
  550. void do_blocking_move_to_x(const_float_t rx, const_feedRate_t fr_mm_s/*=0.0*/) {
  551. do_blocking_move_to(
  552. NUM_AXIS_LIST(rx, current_position.y, current_position.z, current_position.i, current_position.j, current_position.k,
  553. current_position.u, current_position.v, current_position.w),
  554. fr_mm_s
  555. );
  556. }
  557. #if HAS_Y_AXIS
  558. void do_blocking_move_to_y(const_float_t ry, const_feedRate_t fr_mm_s/*=0.0*/) {
  559. do_blocking_move_to(
  560. NUM_AXIS_LIST(current_position.x, ry, current_position.z, current_position.i, current_position.j, current_position.k,
  561. current_position.u, current_position.v, current_position.w),
  562. fr_mm_s
  563. );
  564. }
  565. #endif
  566. #if HAS_Z_AXIS
  567. void do_blocking_move_to_z(const_float_t rz, const_feedRate_t fr_mm_s/*=0.0*/) {
  568. do_blocking_move_to_xy_z(current_position, rz, fr_mm_s);
  569. }
  570. #endif
  571. #if HAS_I_AXIS
  572. void do_blocking_move_to_i(const_float_t ri, const_feedRate_t fr_mm_s/*=0.0*/) {
  573. do_blocking_move_to_xyz_i(current_position, ri, fr_mm_s);
  574. }
  575. void do_blocking_move_to_xyz_i(const xyze_pos_t &raw, const_float_t i, const_feedRate_t fr_mm_s/*=0.0f*/) {
  576. do_blocking_move_to(
  577. NUM_AXIS_LIST(raw.x, raw.y, raw.z, i, raw.j, raw.k, raw.u, raw.v, raw.w),
  578. fr_mm_s
  579. );
  580. }
  581. #endif
  582. #if HAS_J_AXIS
  583. void do_blocking_move_to_j(const_float_t rj, const_feedRate_t fr_mm_s/*=0.0*/) {
  584. do_blocking_move_to_xyzi_j(current_position, rj, fr_mm_s);
  585. }
  586. void do_blocking_move_to_xyzi_j(const xyze_pos_t &raw, const_float_t j, const_feedRate_t fr_mm_s/*=0.0f*/) {
  587. do_blocking_move_to(
  588. NUM_AXIS_LIST(raw.x, raw.y, raw.z, raw.i, j, raw.k, raw.u, raw.v, raw.w),
  589. fr_mm_s
  590. );
  591. }
  592. #endif
  593. #if HAS_K_AXIS
  594. void do_blocking_move_to_k(const_float_t rk, const_feedRate_t fr_mm_s/*=0.0*/) {
  595. do_blocking_move_to_xyzij_k(current_position, rk, fr_mm_s);
  596. }
  597. void do_blocking_move_to_xyzij_k(const xyze_pos_t &raw, const_float_t k, const_feedRate_t fr_mm_s/*=0.0f*/) {
  598. do_blocking_move_to(
  599. NUM_AXIS_LIST(raw.x, raw.y, raw.z, raw.i, raw.j, k, raw.u, raw.v, raw.w),
  600. fr_mm_s
  601. );
  602. }
  603. #endif
  604. #if HAS_U_AXIS
  605. void do_blocking_move_to_u(const_float_t ru, const_feedRate_t fr_mm_s/*=0.0*/) {
  606. do_blocking_move_to_xyzijk_u(current_position, ru, fr_mm_s);
  607. }
  608. void do_blocking_move_to_xyzijk_u(const xyze_pos_t &raw, const_float_t u, const_feedRate_t fr_mm_s/*=0.0f*/) {
  609. do_blocking_move_to(
  610. NUM_AXIS_LIST(raw.x, raw.y, raw.z, raw.i, raw.j, raw.k, u, raw.v, raw.w),
  611. fr_mm_s
  612. );
  613. }
  614. #endif
  615. #if HAS_V_AXIS
  616. void do_blocking_move_to_v(const_float_t rv, const_feedRate_t fr_mm_s/*=0.0*/) {
  617. do_blocking_move_to_xyzijku_v(current_position, rv, fr_mm_s);
  618. }
  619. void do_blocking_move_to_xyzijku_v(const xyze_pos_t &raw, const_float_t v, const_feedRate_t fr_mm_s/*=0.0f*/) {
  620. do_blocking_move_to(
  621. NUM_AXIS_LIST(raw.x, raw.y, raw.z, raw.i, raw.j, raw.k, raw.u, v, raw.w),
  622. fr_mm_s
  623. );
  624. }
  625. #endif
  626. #if HAS_W_AXIS
  627. void do_blocking_move_to_w(const_float_t rw, const_feedRate_t fr_mm_s/*=0.0*/) {
  628. do_blocking_move_to_xyzijkuv_w(current_position, rw, fr_mm_s);
  629. }
  630. void do_blocking_move_to_xyzijkuv_w(const xyze_pos_t &raw, const_float_t w, const_feedRate_t fr_mm_s/*=0.0f*/) {
  631. do_blocking_move_to(
  632. NUM_AXIS_LIST(raw.x, raw.y, raw.z, raw.i, raw.j, raw.k, raw.u, raw.v, w),
  633. fr_mm_s
  634. );
  635. }
  636. #endif
  637. #if HAS_Y_AXIS
  638. void do_blocking_move_to_xy(const_float_t rx, const_float_t ry, const_feedRate_t fr_mm_s/*=0.0*/) {
  639. do_blocking_move_to(
  640. NUM_AXIS_LIST(rx, ry, current_position.z, current_position.i, current_position.j, current_position.k,
  641. current_position.u, current_position.v, current_position.w),
  642. fr_mm_s
  643. );
  644. }
  645. void do_blocking_move_to_xy(const xy_pos_t &raw, const_feedRate_t fr_mm_s/*=0.0f*/) {
  646. do_blocking_move_to_xy(raw.x, raw.y, fr_mm_s);
  647. }
  648. #endif
  649. #if HAS_Z_AXIS
  650. void do_blocking_move_to_xy_z(const xy_pos_t &raw, const_float_t z, const_feedRate_t fr_mm_s/*=0.0f*/) {
  651. do_blocking_move_to(
  652. NUM_AXIS_LIST(raw.x, raw.y, z, current_position.i, current_position.j, current_position.k,
  653. current_position.u, current_position.v, current_position.w),
  654. fr_mm_s
  655. );
  656. }
  657. void do_z_clearance(const_float_t zclear, const bool lower_allowed/*=false*/) {
  658. float zdest = zclear;
  659. if (!lower_allowed) NOLESS(zdest, current_position.z);
  660. do_blocking_move_to_z(_MIN(zdest, Z_MAX_POS), TERN(HAS_BED_PROBE, z_probe_fast_mm_s, homing_feedrate(Z_AXIS)));
  661. }
  662. #endif
  663. //
  664. // Prepare to do endstop or probe moves with custom feedrates.
  665. // - Save / restore current feedrate and multiplier
  666. //
  667. static float saved_feedrate_mm_s;
  668. static int16_t saved_feedrate_percentage;
  669. void remember_feedrate_and_scaling() {
  670. saved_feedrate_mm_s = feedrate_mm_s;
  671. saved_feedrate_percentage = feedrate_percentage;
  672. }
  673. void remember_feedrate_scaling_off() {
  674. remember_feedrate_and_scaling();
  675. feedrate_percentage = 100;
  676. }
  677. void restore_feedrate_and_scaling() {
  678. feedrate_mm_s = saved_feedrate_mm_s;
  679. feedrate_percentage = saved_feedrate_percentage;
  680. }
  681. #if HAS_SOFTWARE_ENDSTOPS
  682. // Software Endstops are based on the configured limits.
  683. #define _AMIN(A) A##_MIN_POS
  684. #define _AMAX(A) A##_MAX_POS
  685. soft_endstops_t soft_endstop = {
  686. true, false,
  687. { MAPLIST(_AMIN, MAIN_AXIS_NAMES) },
  688. { MAPLIST(_AMAX, MAIN_AXIS_NAMES) },
  689. };
  690. /**
  691. * Software endstops can be used to monitor the open end of
  692. * an axis that has a hardware endstop on the other end. Or
  693. * they can prevent axes from moving past endstops and grinding.
  694. *
  695. * To keep doing their job as the coordinate system changes,
  696. * the software endstop positions must be refreshed to remain
  697. * at the same positions relative to the machine.
  698. */
  699. void update_software_endstops(const AxisEnum axis
  700. OPTARG(HAS_HOTEND_OFFSET, const uint8_t old_tool_index/*=0*/, const uint8_t new_tool_index/*=0*/)
  701. ) {
  702. #if ENABLED(DUAL_X_CARRIAGE)
  703. if (axis == X_AXIS) {
  704. // In Dual X mode hotend_offset[X] is T1's home position
  705. const float dual_max_x = _MAX(hotend_offset[1].x, X2_MAX_POS);
  706. if (new_tool_index != 0) {
  707. // T1 can move from X2_MIN_POS to X2_MAX_POS or X2 home position (whichever is larger)
  708. soft_endstop.min.x = X2_MIN_POS;
  709. soft_endstop.max.x = dual_max_x;
  710. }
  711. else if (idex_is_duplicating()) {
  712. // In Duplication Mode, T0 can move as far left as X1_MIN_POS
  713. // but not so far to the right that T1 would move past the end
  714. soft_endstop.min.x = X1_MIN_POS;
  715. soft_endstop.max.x = _MIN(X1_MAX_POS, dual_max_x - duplicate_extruder_x_offset);
  716. }
  717. else {
  718. // In other modes, T0 can move from X1_MIN_POS to X1_MAX_POS
  719. soft_endstop.min.x = X1_MIN_POS;
  720. soft_endstop.max.x = X1_MAX_POS;
  721. }
  722. }
  723. #elif ENABLED(DELTA)
  724. soft_endstop.min[axis] = base_min_pos(axis);
  725. soft_endstop.max[axis] = (axis == Z_AXIS) ? DIFF_TERN(HAS_BED_PROBE, delta_height, probe.offset.z) : base_max_pos(axis);
  726. switch (axis) {
  727. case X_AXIS:
  728. case Y_AXIS:
  729. // Get a minimum radius for clamping
  730. delta_max_radius = _MIN(ABS(_MAX(soft_endstop.min.x, soft_endstop.min.y)), soft_endstop.max.x, soft_endstop.max.y);
  731. delta_max_radius_2 = sq(delta_max_radius);
  732. break;
  733. case Z_AXIS:
  734. refresh_delta_clip_start_height();
  735. default: break;
  736. }
  737. #elif HAS_HOTEND_OFFSET
  738. // Software endstops are relative to the tool 0 workspace, so
  739. // the movement limits must be shifted by the tool offset to
  740. // retain the same physical limit when other tools are selected.
  741. if (new_tool_index == old_tool_index || axis == Z_AXIS) { // The Z axis is "special" and shouldn't be modified
  742. const float offs = (axis == Z_AXIS) ? 0 : hotend_offset[active_extruder][axis];
  743. soft_endstop.min[axis] = base_min_pos(axis) + offs;
  744. soft_endstop.max[axis] = base_max_pos(axis) + offs;
  745. }
  746. else {
  747. const float diff = hotend_offset[new_tool_index][axis] - hotend_offset[old_tool_index][axis];
  748. soft_endstop.min[axis] += diff;
  749. soft_endstop.max[axis] += diff;
  750. }
  751. #else
  752. soft_endstop.min[axis] = base_min_pos(axis);
  753. soft_endstop.max[axis] = base_max_pos(axis);
  754. #endif
  755. if (DEBUGGING(LEVELING))
  756. SERIAL_ECHOLNPGM("Axis ", AS_CHAR(AXIS_CHAR(axis)), " min:", soft_endstop.min[axis], " max:", soft_endstop.max[axis]);
  757. }
  758. /**
  759. * Constrain the given coordinates to the software endstops.
  760. *
  761. * For DELTA/SCARA the XY constraint is based on the smallest
  762. * radius within the set software endstops.
  763. */
  764. void apply_motion_limits(xyz_pos_t &target) {
  765. if (!soft_endstop._enabled) return;
  766. #if IS_KINEMATIC
  767. if (TERN0(DELTA, !all_axes_homed())) return;
  768. #if BOTH(HAS_HOTEND_OFFSET, DELTA)
  769. // The effector center position will be the target minus the hotend offset.
  770. const xy_pos_t offs = hotend_offset[active_extruder];
  771. #else
  772. // SCARA needs to consider the angle of the arm through the entire move, so for now use no tool offset.
  773. constexpr xy_pos_t offs{0};
  774. #endif
  775. if (TERN1(IS_SCARA, axis_was_homed(X_AXIS) && axis_was_homed(Y_AXIS))) {
  776. const float dist_2 = HYPOT2(target.x - offs.x, target.y - offs.y);
  777. if (dist_2 > delta_max_radius_2)
  778. target *= float(delta_max_radius / SQRT(dist_2)); // 200 / 300 = 0.66
  779. }
  780. #else
  781. if (axis_was_homed(X_AXIS)) {
  782. #if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MIN_SOFTWARE_ENDSTOP_X)
  783. NOLESS(target.x, soft_endstop.min.x);
  784. #endif
  785. #if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MAX_SOFTWARE_ENDSTOP_X)
  786. NOMORE(target.x, soft_endstop.max.x);
  787. #endif
  788. }
  789. #if HAS_Y_AXIS
  790. if (axis_was_homed(Y_AXIS)) {
  791. #if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MIN_SOFTWARE_ENDSTOP_Y)
  792. NOLESS(target.y, soft_endstop.min.y);
  793. #endif
  794. #if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MAX_SOFTWARE_ENDSTOP_Y)
  795. NOMORE(target.y, soft_endstop.max.y);
  796. #endif
  797. }
  798. #endif
  799. #endif
  800. #if HAS_Z_AXIS
  801. if (axis_was_homed(Z_AXIS)) {
  802. #if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MIN_SOFTWARE_ENDSTOP_Z)
  803. NOLESS(target.z, soft_endstop.min.z);
  804. #endif
  805. #if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MAX_SOFTWARE_ENDSTOP_Z)
  806. NOMORE(target.z, soft_endstop.max.z);
  807. #endif
  808. }
  809. #endif
  810. #if HAS_I_AXIS
  811. if (axis_was_homed(I_AXIS)) {
  812. #if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MIN_SOFTWARE_ENDSTOP_I)
  813. NOLESS(target.i, soft_endstop.min.i);
  814. #endif
  815. #if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MAX_SOFTWARE_ENDSTOP_I)
  816. NOMORE(target.i, soft_endstop.max.i);
  817. #endif
  818. }
  819. #endif
  820. #if HAS_J_AXIS
  821. if (axis_was_homed(J_AXIS)) {
  822. #if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MIN_SOFTWARE_ENDSTOP_J)
  823. NOLESS(target.j, soft_endstop.min.j);
  824. #endif
  825. #if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MAX_SOFTWARE_ENDSTOP_J)
  826. NOMORE(target.j, soft_endstop.max.j);
  827. #endif
  828. }
  829. #endif
  830. #if HAS_K_AXIS
  831. if (axis_was_homed(K_AXIS)) {
  832. #if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MIN_SOFTWARE_ENDSTOP_K)
  833. NOLESS(target.k, soft_endstop.min.k);
  834. #endif
  835. #if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MAX_SOFTWARE_ENDSTOP_K)
  836. NOMORE(target.k, soft_endstop.max.k);
  837. #endif
  838. }
  839. #endif
  840. #if HAS_U_AXIS
  841. if (axis_was_homed(U_AXIS)) {
  842. #if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MIN_SOFTWARE_ENDSTOP_U)
  843. NOLESS(target.u, soft_endstop.min.u);
  844. #endif
  845. #if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MAX_SOFTWARE_ENDSTOP_U)
  846. NOMORE(target.u, soft_endstop.max.u);
  847. #endif
  848. }
  849. #endif
  850. #if HAS_V_AXIS
  851. if (axis_was_homed(V_AXIS)) {
  852. #if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MIN_SOFTWARE_ENDSTOP_V)
  853. NOLESS(target.v, soft_endstop.min.v);
  854. #endif
  855. #if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MAX_SOFTWARE_ENDSTOP_V)
  856. NOMORE(target.v, soft_endstop.max.v);
  857. #endif
  858. }
  859. #endif
  860. #if HAS_W_AXIS
  861. if (axis_was_homed(W_AXIS)) {
  862. #if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MIN_SOFTWARE_ENDSTOP_W)
  863. NOLESS(target.w, soft_endstop.min.w);
  864. #endif
  865. #if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MAX_SOFTWARE_ENDSTOP_W)
  866. NOMORE(target.w, soft_endstop.max.w);
  867. #endif
  868. }
  869. #endif
  870. }
  871. #else // !HAS_SOFTWARE_ENDSTOPS
  872. soft_endstops_t soft_endstop;
  873. #endif // !HAS_SOFTWARE_ENDSTOPS
  874. FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
  875. const millis_t ms = millis();
  876. if (ELAPSED(ms, next_idle_ms)) {
  877. next_idle_ms = ms + 200UL;
  878. return idle();
  879. }
  880. thermalManager.task(); // Returns immediately on most calls
  881. }
  882. #if IS_KINEMATIC
  883. #if IS_SCARA
  884. /**
  885. * Before raising this value, use M665 S[seg_per_sec] to decrease
  886. * the number of segments-per-second. Default is 200. Some deltas
  887. * do better with 160 or lower. It would be good to know how many
  888. * segments-per-second are actually possible for SCARA on AVR.
  889. *
  890. * Longer segments result in less kinematic overhead
  891. * but may produce jagged lines. Try 0.5mm, 1.0mm, and 2.0mm
  892. * and compare the difference.
  893. */
  894. #define SCARA_MIN_SEGMENT_LENGTH 0.5f
  895. #endif
  896. /**
  897. * Prepare a linear move in a DELTA or SCARA setup.
  898. *
  899. * Called from prepare_line_to_destination as the
  900. * default Delta/SCARA segmenter.
  901. *
  902. * This calls planner.buffer_line several times, adding
  903. * small incremental moves for DELTA or SCARA.
  904. *
  905. * For Unified Bed Leveling (Delta or Segmented Cartesian)
  906. * the bedlevel.line_to_destination_segmented method replaces this.
  907. *
  908. * For Auto Bed Leveling (Bilinear) with SEGMENT_LEVELED_MOVES
  909. * this is replaced by segmented_line_to_destination below.
  910. */
  911. inline bool line_to_destination_kinematic() {
  912. // Get the top feedrate of the move in the XY plane
  913. const float scaled_fr_mm_s = MMS_SCALED(feedrate_mm_s);
  914. const xyze_float_t diff = destination - current_position;
  915. // If the move is only in Z/E don't split up the move
  916. if (!diff.x && !diff.y) {
  917. planner.buffer_line(destination, scaled_fr_mm_s);
  918. return false; // caller will update current_position
  919. }
  920. // Fail if attempting move outside printable radius
  921. if (!position_is_reachable(destination)) return true;
  922. // Get the linear distance in XYZ
  923. float cartesian_mm = xyz_float_t(diff).magnitude();
  924. // If the move is very short, check the E move distance
  925. TERN_(HAS_EXTRUDERS, if (UNEAR_ZERO(cartesian_mm)) cartesian_mm = ABS(diff.e));
  926. // No E move either? Game over.
  927. if (UNEAR_ZERO(cartesian_mm)) return true;
  928. // Minimum number of seconds to move the given distance
  929. const float seconds = cartesian_mm / scaled_fr_mm_s;
  930. // The number of segments-per-second times the duration
  931. // gives the number of segments
  932. uint16_t segments = segments_per_second * seconds;
  933. // For SCARA enforce a minimum segment size
  934. #if IS_SCARA
  935. NOMORE(segments, cartesian_mm * RECIPROCAL(SCARA_MIN_SEGMENT_LENGTH));
  936. #endif
  937. // At least one segment is required
  938. NOLESS(segments, 1U);
  939. // The approximate length of each segment
  940. const float inv_segments = 1.0f / float(segments);
  941. const xyze_float_t segment_distance = diff * inv_segments;
  942. // Add hints to help optimize the move
  943. PlannerHints hints(cartesian_mm * inv_segments);
  944. TERN_(SCARA_FEEDRATE_SCALING, hints.inv_duration = scaled_fr_mm_s / hints.millimeters);
  945. /*
  946. SERIAL_ECHOPGM("mm=", cartesian_mm);
  947. SERIAL_ECHOPGM(" seconds=", seconds);
  948. SERIAL_ECHOPGM(" segments=", segments);
  949. SERIAL_ECHOPGM(" segment_mm=", hints.millimeters);
  950. SERIAL_EOL();
  951. //*/
  952. // Get the current position as starting point
  953. xyze_pos_t raw = current_position;
  954. // Calculate and execute the segments
  955. millis_t next_idle_ms = millis() + 200UL;
  956. while (--segments) {
  957. segment_idle(next_idle_ms);
  958. raw += segment_distance;
  959. if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, hints))
  960. break;
  961. }
  962. // Ensure last segment arrives at target location.
  963. planner.buffer_line(destination, scaled_fr_mm_s, active_extruder, hints);
  964. return false; // caller will update current_position
  965. }
  966. #else // !IS_KINEMATIC
  967. #if ENABLED(SEGMENT_LEVELED_MOVES) && DISABLED(AUTO_BED_LEVELING_UBL)
  968. /**
  969. * Prepare a segmented move on a CARTESIAN setup.
  970. *
  971. * This calls planner.buffer_line several times, adding
  972. * small incremental moves. This allows the planner to
  973. * apply more detailed bed leveling to the full move.
  974. */
  975. inline void segmented_line_to_destination(const_feedRate_t fr_mm_s, const float segment_size=LEVELED_SEGMENT_LENGTH) {
  976. const xyze_float_t diff = destination - current_position;
  977. // If the move is only in Z/E don't split up the move
  978. if (!diff.x && !diff.y) {
  979. planner.buffer_line(destination, fr_mm_s);
  980. return;
  981. }
  982. // Get the linear distance in XYZ
  983. // If the move is very short, check the E move distance
  984. // No E move either? Game over.
  985. float cartesian_mm = diff.magnitude();
  986. TERN_(HAS_EXTRUDERS, if (UNEAR_ZERO(cartesian_mm)) cartesian_mm = ABS(diff.e));
  987. if (UNEAR_ZERO(cartesian_mm)) return;
  988. // The length divided by the segment size
  989. // At least one segment is required
  990. uint16_t segments = cartesian_mm / segment_size;
  991. NOLESS(segments, 1U);
  992. // The approximate length of each segment
  993. const float inv_segments = 1.0f / float(segments);
  994. const xyze_float_t segment_distance = diff * inv_segments;
  995. // Add hints to help optimize the move
  996. PlannerHints hints(cartesian_mm * inv_segments);
  997. TERN_(SCARA_FEEDRATE_SCALING, hints.inv_duration = scaled_fr_mm_s / hints.millimeters);
  998. //SERIAL_ECHOPGM("mm=", cartesian_mm);
  999. //SERIAL_ECHOLNPGM(" segments=", segments);
  1000. //SERIAL_ECHOLNPGM(" segment_mm=", hints.millimeters);
  1001. // Get the raw current position as starting point
  1002. xyze_pos_t raw = current_position;
  1003. // Calculate and execute the segments
  1004. millis_t next_idle_ms = millis() + 200UL;
  1005. while (--segments) {
  1006. segment_idle(next_idle_ms);
  1007. raw += segment_distance;
  1008. if (!planner.buffer_line(raw, fr_mm_s, active_extruder, hints))
  1009. break;
  1010. }
  1011. // Since segment_distance is only approximate,
  1012. // the final move must be to the exact destination.
  1013. planner.buffer_line(destination, fr_mm_s, active_extruder, hints);
  1014. }
  1015. #endif // SEGMENT_LEVELED_MOVES && !AUTO_BED_LEVELING_UBL
  1016. /**
  1017. * Prepare a linear move in a Cartesian setup.
  1018. *
  1019. * When a mesh-based leveling system is active, moves are segmented
  1020. * according to the configuration of the leveling system.
  1021. *
  1022. * Return true if 'current_position' was set to 'destination'
  1023. */
  1024. inline bool line_to_destination_cartesian() {
  1025. const float scaled_fr_mm_s = MMS_SCALED(feedrate_mm_s);
  1026. #if HAS_MESH
  1027. if (planner.leveling_active && planner.leveling_active_at_z(destination.z)) {
  1028. #if ENABLED(AUTO_BED_LEVELING_UBL)
  1029. #if UBL_SEGMENTED
  1030. return bedlevel.line_to_destination_segmented(scaled_fr_mm_s);
  1031. #else
  1032. bedlevel.line_to_destination_cartesian(scaled_fr_mm_s, active_extruder); // UBL's motion routine needs to know about
  1033. return true; // all moves, including Z-only moves.
  1034. #endif
  1035. #elif ENABLED(SEGMENT_LEVELED_MOVES)
  1036. segmented_line_to_destination(scaled_fr_mm_s);
  1037. return false; // caller will update current_position
  1038. #else
  1039. /**
  1040. * For MBL and ABL-BILINEAR only segment moves when X or Y are involved.
  1041. * Otherwise fall through to do a direct single move.
  1042. */
  1043. if (xy_pos_t(current_position) != xy_pos_t(destination)) {
  1044. #if ENABLED(MESH_BED_LEVELING)
  1045. bedlevel.line_to_destination(scaled_fr_mm_s);
  1046. #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
  1047. bedlevel.line_to_destination(scaled_fr_mm_s);
  1048. #endif
  1049. return true;
  1050. }
  1051. #endif
  1052. }
  1053. #endif // HAS_MESH
  1054. planner.buffer_line(destination, scaled_fr_mm_s);
  1055. return false; // caller will update current_position
  1056. }
  1057. #endif // !IS_KINEMATIC
  1058. #if HAS_DUPLICATION_MODE
  1059. bool extruder_duplication_enabled;
  1060. #if ENABLED(MULTI_NOZZLE_DUPLICATION)
  1061. uint8_t duplication_e_mask; // = 0
  1062. #endif
  1063. #endif
  1064. #if ENABLED(DUAL_X_CARRIAGE)
  1065. DualXMode dual_x_carriage_mode = DEFAULT_DUAL_X_CARRIAGE_MODE;
  1066. float inactive_extruder_x = X2_MAX_POS, // Used in mode 0 & 1
  1067. duplicate_extruder_x_offset = DEFAULT_DUPLICATION_X_OFFSET; // Used in mode 2 & 3
  1068. xyz_pos_t raised_parked_position; // Used in mode 1
  1069. bool active_extruder_parked = false; // Used in mode 1, 2 & 3
  1070. millis_t delayed_move_time = 0; // Used in mode 1
  1071. celsius_t duplicate_extruder_temp_offset = 0; // Used in mode 2 & 3
  1072. bool idex_mirrored_mode = false; // Used in mode 3
  1073. float x_home_pos(const uint8_t extruder) {
  1074. if (extruder == 0) return X_HOME_POS;
  1075. /**
  1076. * In dual carriage mode the extruder offset provides an override of the
  1077. * second X-carriage position when homed - otherwise X2_HOME_POS is used.
  1078. * This allows soft recalibration of the second extruder home position
  1079. * (with M218 T1 Xn) without firmware reflash.
  1080. */
  1081. return hotend_offset[1].x > 0 ? hotend_offset[1].x : X2_HOME_POS;
  1082. }
  1083. void idex_set_mirrored_mode(const bool mirr) {
  1084. idex_mirrored_mode = mirr;
  1085. stepper.set_directions();
  1086. }
  1087. void set_duplication_enabled(const bool dupe, const int8_t tool_index/*=-1*/) {
  1088. extruder_duplication_enabled = dupe;
  1089. if (tool_index >= 0) active_extruder = tool_index;
  1090. stepper.set_directions();
  1091. }
  1092. void idex_set_parked(const bool park/*=true*/) {
  1093. delayed_move_time = 0;
  1094. active_extruder_parked = park;
  1095. if (park) raised_parked_position = current_position; // Remember current raised toolhead position for use by unpark
  1096. }
  1097. /**
  1098. * Prepare a linear move in a dual X axis setup
  1099. *
  1100. * Return true if current_position[] was set to destination[]
  1101. */
  1102. inline bool dual_x_carriage_unpark() {
  1103. if (active_extruder_parked) {
  1104. switch (dual_x_carriage_mode) {
  1105. case DXC_FULL_CONTROL_MODE: break;
  1106. case DXC_AUTO_PARK_MODE: {
  1107. if (current_position.e == destination.e) {
  1108. // This is a travel move (with no extrusion)
  1109. // Skip it, but keep track of the current position
  1110. // (so it can be used as the start of the next non-travel move)
  1111. if (delayed_move_time != 0xFFFFFFFFUL) {
  1112. current_position = destination;
  1113. NOLESS(raised_parked_position.z, destination.z);
  1114. delayed_move_time = millis() + 1000UL;
  1115. return true;
  1116. }
  1117. }
  1118. //
  1119. // Un-park the active extruder
  1120. //
  1121. const feedRate_t fr_zfast = planner.settings.max_feedrate_mm_s[Z_AXIS];
  1122. // 1. Move to the raised parked XYZ. Presumably the tool is already at XY.
  1123. xyze_pos_t raised = raised_parked_position; raised.e = current_position.e;
  1124. if (planner.buffer_line(raised, fr_zfast)) {
  1125. // 2. Move to the current native XY and raised Z. Presumably this is a null move.
  1126. xyze_pos_t curpos = current_position; curpos.z = raised_parked_position.z;
  1127. if (planner.buffer_line(curpos, PLANNER_XY_FEEDRATE())) {
  1128. // 3. Lower Z back down
  1129. line_to_current_position(fr_zfast);
  1130. }
  1131. }
  1132. stepper.set_directions();
  1133. idex_set_parked(false);
  1134. if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("idex_set_parked(false)");
  1135. } break;
  1136. case DXC_MIRRORED_MODE:
  1137. case DXC_DUPLICATION_MODE:
  1138. if (active_extruder == 0) {
  1139. set_duplication_enabled(false); // Clear stale duplication state
  1140. // Restore planner to parked head (T1) X position
  1141. float x0_pos = current_position.x;
  1142. xyze_pos_t pos_now = current_position;
  1143. pos_now.x = inactive_extruder_x;
  1144. planner.set_position_mm(pos_now);
  1145. // Keep the same X or add the duplication X offset
  1146. xyze_pos_t new_pos = pos_now;
  1147. if (dual_x_carriage_mode == DXC_DUPLICATION_MODE)
  1148. new_pos.x = x0_pos + duplicate_extruder_x_offset;
  1149. else
  1150. new_pos.x = _MIN(X_BED_SIZE - x0_pos, X_MAX_POS);
  1151. // Move duplicate extruder into the correct position
  1152. if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Set planner X", inactive_extruder_x, " ... Line to X", new_pos.x);
  1153. if (!planner.buffer_line(new_pos, planner.settings.max_feedrate_mm_s[X_AXIS], 1)) break;
  1154. planner.synchronize();
  1155. sync_plan_position(); // Extra sync for good measure
  1156. set_duplication_enabled(true); // Enable Duplication
  1157. idex_set_parked(false); // No longer parked
  1158. if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("set_duplication_enabled(true)\nidex_set_parked(false)");
  1159. }
  1160. else if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Active extruder not 0");
  1161. break;
  1162. }
  1163. }
  1164. return false;
  1165. }
  1166. #endif // DUAL_X_CARRIAGE
  1167. /**
  1168. * Prepare a single move and get ready for the next one
  1169. *
  1170. * This may result in several calls to planner.buffer_line to
  1171. * do smaller moves for DELTA, SCARA, mesh moves, etc.
  1172. *
  1173. * Make sure current_position.e and destination.e are good
  1174. * before calling or cold/lengthy extrusion may get missed.
  1175. *
  1176. * Before exit, current_position is set to destination.
  1177. */
  1178. void prepare_line_to_destination() {
  1179. apply_motion_limits(destination);
  1180. #if EITHER(PREVENT_COLD_EXTRUSION, PREVENT_LENGTHY_EXTRUDE)
  1181. if (!DEBUGGING(DRYRUN) && destination.e != current_position.e) {
  1182. bool ignore_e = false;
  1183. #if ENABLED(PREVENT_COLD_EXTRUSION)
  1184. ignore_e = thermalManager.tooColdToExtrude(active_extruder);
  1185. if (ignore_e) SERIAL_ECHO_MSG(STR_ERR_COLD_EXTRUDE_STOP);
  1186. #endif
  1187. #if ENABLED(PREVENT_LENGTHY_EXTRUDE)
  1188. const float e_delta = ABS(destination.e - current_position.e) * planner.e_factor[active_extruder];
  1189. if (e_delta > (EXTRUDE_MAXLENGTH)) {
  1190. #if ENABLED(MIXING_EXTRUDER)
  1191. float collector[MIXING_STEPPERS];
  1192. mixer.refresh_collector(1.0, mixer.get_current_vtool(), collector);
  1193. MIXER_STEPPER_LOOP(e) {
  1194. if (e_delta * collector[e] > (EXTRUDE_MAXLENGTH)) {
  1195. ignore_e = true;
  1196. SERIAL_ECHO_MSG(STR_ERR_LONG_EXTRUDE_STOP);
  1197. break;
  1198. }
  1199. }
  1200. #else
  1201. ignore_e = true;
  1202. SERIAL_ECHO_MSG(STR_ERR_LONG_EXTRUDE_STOP);
  1203. #endif
  1204. }
  1205. #endif
  1206. if (ignore_e) {
  1207. current_position.e = destination.e; // Behave as if the E move really took place
  1208. planner.set_e_position_mm(destination.e); // Prevent the planner from complaining too
  1209. }
  1210. }
  1211. #endif // PREVENT_COLD_EXTRUSION || PREVENT_LENGTHY_EXTRUDE
  1212. if (TERN0(DUAL_X_CARRIAGE, dual_x_carriage_unpark())) return;
  1213. if (
  1214. #if UBL_SEGMENTED
  1215. #if IS_KINEMATIC // UBL using Kinematic / Cartesian cases as a workaround for now.
  1216. bedlevel.line_to_destination_segmented(MMS_SCALED(feedrate_mm_s))
  1217. #else
  1218. line_to_destination_cartesian()
  1219. #endif
  1220. #elif IS_KINEMATIC
  1221. line_to_destination_kinematic()
  1222. #else
  1223. line_to_destination_cartesian()
  1224. #endif
  1225. ) return;
  1226. current_position = destination;
  1227. }
  1228. #if HAS_ENDSTOPS
  1229. main_axes_bits_t axes_homed, axes_trusted; // = 0
  1230. main_axes_bits_t axes_should_home(main_axes_bits_t axis_bits/*=main_axes_mask*/) {
  1231. auto set_should = [](main_axes_bits_t &b, AxisEnum a) {
  1232. if (TEST(b, a) && TERN(HOME_AFTER_DEACTIVATE, axis_is_trusted, axis_was_homed)(a))
  1233. CBI(b, a);
  1234. };
  1235. // Clear test bits that are trusted
  1236. NUM_AXIS_CODE(
  1237. set_should(axis_bits, X_AXIS), set_should(axis_bits, Y_AXIS), set_should(axis_bits, Z_AXIS),
  1238. set_should(axis_bits, I_AXIS), set_should(axis_bits, J_AXIS), set_should(axis_bits, K_AXIS),
  1239. set_should(axis_bits, U_AXIS), set_should(axis_bits, V_AXIS), set_should(axis_bits, W_AXIS)
  1240. );
  1241. return axis_bits;
  1242. }
  1243. bool homing_needed_error(main_axes_bits_t axis_bits/*=main_axes_mask*/) {
  1244. if ((axis_bits = axes_should_home(axis_bits))) {
  1245. PGM_P home_first = GET_TEXT(MSG_HOME_FIRST);
  1246. char msg[30];
  1247. #define _AXIS_CHAR(N) TEST(axis_bits, _AXIS(N)) ? STR_##N : ""
  1248. sprintf_P(msg, home_first, MAPLIST(_AXIS_CHAR, MAIN_AXIS_NAMES));
  1249. SERIAL_ECHO_START();
  1250. SERIAL_ECHOLN(msg);
  1251. ui.set_status(msg);
  1252. return true;
  1253. }
  1254. return false;
  1255. }
  1256. /**
  1257. * Homing bump feedrate (mm/s)
  1258. */
  1259. feedRate_t get_homing_bump_feedrate(const AxisEnum axis) {
  1260. #if HOMING_Z_WITH_PROBE
  1261. if (axis == Z_AXIS) return MMM_TO_MMS(Z_PROBE_FEEDRATE_SLOW);
  1262. #endif
  1263. static const uint8_t homing_bump_divisor[] PROGMEM = HOMING_BUMP_DIVISOR;
  1264. uint8_t hbd = pgm_read_byte(&homing_bump_divisor[axis]);
  1265. if (hbd < 1) {
  1266. hbd = 10;
  1267. SERIAL_ECHO_MSG("Warning: Homing Bump Divisor < 1");
  1268. }
  1269. return homing_feedrate(axis) / float(hbd);
  1270. }
  1271. #if ENABLED(SENSORLESS_HOMING)
  1272. /**
  1273. * Set sensorless homing if the axis has it, accounting for Core Kinematics.
  1274. */
  1275. sensorless_t start_sensorless_homing_per_axis(const AxisEnum axis) {
  1276. sensorless_t stealth_states { false };
  1277. switch (axis) {
  1278. default: break;
  1279. #if X_SENSORLESS
  1280. case X_AXIS:
  1281. stealth_states.x = tmc_enable_stallguard(stepperX);
  1282. TERN_(X2_SENSORLESS, stealth_states.x2 = tmc_enable_stallguard(stepperX2));
  1283. #if ANY(CORE_IS_XY, MARKFORGED_XY, MARKFORGED_YX) && Y_SENSORLESS
  1284. stealth_states.y = tmc_enable_stallguard(stepperY);
  1285. #elif CORE_IS_XZ && Z_SENSORLESS
  1286. stealth_states.z = tmc_enable_stallguard(stepperZ);
  1287. #endif
  1288. break;
  1289. #endif
  1290. #if Y_SENSORLESS
  1291. case Y_AXIS:
  1292. stealth_states.y = tmc_enable_stallguard(stepperY);
  1293. TERN_(Y2_SENSORLESS, stealth_states.y2 = tmc_enable_stallguard(stepperY2));
  1294. #if ANY(CORE_IS_XY, MARKFORGED_XY, MARKFORGED_YX) && X_SENSORLESS
  1295. stealth_states.x = tmc_enable_stallguard(stepperX);
  1296. #elif CORE_IS_YZ && Z_SENSORLESS
  1297. stealth_states.z = tmc_enable_stallguard(stepperZ);
  1298. #endif
  1299. break;
  1300. #endif
  1301. #if Z_SENSORLESS
  1302. case Z_AXIS:
  1303. stealth_states.z = tmc_enable_stallguard(stepperZ);
  1304. TERN_(Z2_SENSORLESS, stealth_states.z2 = tmc_enable_stallguard(stepperZ2));
  1305. TERN_(Z3_SENSORLESS, stealth_states.z3 = tmc_enable_stallguard(stepperZ3));
  1306. TERN_(Z4_SENSORLESS, stealth_states.z4 = tmc_enable_stallguard(stepperZ4));
  1307. #if CORE_IS_XZ && X_SENSORLESS
  1308. stealth_states.x = tmc_enable_stallguard(stepperX);
  1309. #elif CORE_IS_YZ && Y_SENSORLESS
  1310. stealth_states.y = tmc_enable_stallguard(stepperY);
  1311. #endif
  1312. break;
  1313. #endif
  1314. #if I_SENSORLESS
  1315. case I_AXIS: stealth_states.i = tmc_enable_stallguard(stepperI); break;
  1316. #endif
  1317. #if J_SENSORLESS
  1318. case J_AXIS: stealth_states.j = tmc_enable_stallguard(stepperJ); break;
  1319. #endif
  1320. #if K_SENSORLESS
  1321. case K_AXIS: stealth_states.k = tmc_enable_stallguard(stepperK); break;
  1322. #endif
  1323. #if U_SENSORLESS
  1324. case U_AXIS: stealth_states.u = tmc_enable_stallguard(stepperU); break;
  1325. #endif
  1326. #if V_SENSORLESS
  1327. case V_AXIS: stealth_states.v = tmc_enable_stallguard(stepperV); break;
  1328. #endif
  1329. #if W_SENSORLESS
  1330. case W_AXIS: stealth_states.w = tmc_enable_stallguard(stepperW); break;
  1331. #endif
  1332. }
  1333. #if ENABLED(SPI_ENDSTOPS)
  1334. switch (axis) {
  1335. case X_AXIS: if (ENABLED(X_SPI_SENSORLESS)) endstops.tmc_spi_homing.x = true; break;
  1336. #if HAS_Y_AXIS
  1337. case Y_AXIS: if (ENABLED(Y_SPI_SENSORLESS)) endstops.tmc_spi_homing.y = true; break;
  1338. #endif
  1339. #if HAS_Z_AXIS
  1340. case Z_AXIS: if (ENABLED(Z_SPI_SENSORLESS)) endstops.tmc_spi_homing.z = true; break;
  1341. #endif
  1342. #if HAS_I_AXIS
  1343. case I_AXIS: if (ENABLED(I_SPI_SENSORLESS)) endstops.tmc_spi_homing.i = true; break;
  1344. #endif
  1345. #if HAS_J_AXIS
  1346. case J_AXIS: if (ENABLED(J_SPI_SENSORLESS)) endstops.tmc_spi_homing.j = true; break;
  1347. #endif
  1348. #if HAS_K_AXIS
  1349. case K_AXIS: if (ENABLED(K_SPI_SENSORLESS)) endstops.tmc_spi_homing.k = true; break;
  1350. #endif
  1351. #if HAS_U_AXIS
  1352. case U_AXIS: if (ENABLED(U_SPI_SENSORLESS)) endstops.tmc_spi_homing.u = true; break;
  1353. #endif
  1354. #if HAS_V_AXIS
  1355. case V_AXIS: if (ENABLED(V_SPI_SENSORLESS)) endstops.tmc_spi_homing.v = true; break;
  1356. #endif
  1357. #if HAS_W_AXIS
  1358. case W_AXIS: if (ENABLED(W_SPI_SENSORLESS)) endstops.tmc_spi_homing.w = true; break;
  1359. #endif
  1360. default: break;
  1361. }
  1362. #endif
  1363. TERN_(IMPROVE_HOMING_RELIABILITY, sg_guard_period = millis() + default_sg_guard_duration);
  1364. return stealth_states;
  1365. }
  1366. void end_sensorless_homing_per_axis(const AxisEnum axis, sensorless_t enable_stealth) {
  1367. switch (axis) {
  1368. default: break;
  1369. #if X_SENSORLESS
  1370. case X_AXIS:
  1371. tmc_disable_stallguard(stepperX, enable_stealth.x);
  1372. TERN_(X2_SENSORLESS, tmc_disable_stallguard(stepperX2, enable_stealth.x2));
  1373. #if ANY(CORE_IS_XY, MARKFORGED_XY, MARKFORGED_YX) && Y_SENSORLESS
  1374. tmc_disable_stallguard(stepperY, enable_stealth.y);
  1375. #elif CORE_IS_XZ && Z_SENSORLESS
  1376. tmc_disable_stallguard(stepperZ, enable_stealth.z);
  1377. #endif
  1378. break;
  1379. #endif
  1380. #if Y_SENSORLESS
  1381. case Y_AXIS:
  1382. tmc_disable_stallguard(stepperY, enable_stealth.y);
  1383. TERN_(Y2_SENSORLESS, tmc_disable_stallguard(stepperY2, enable_stealth.y2));
  1384. #if ANY(CORE_IS_XY, MARKFORGED_XY, MARKFORGED_YX) && X_SENSORLESS
  1385. tmc_disable_stallguard(stepperX, enable_stealth.x);
  1386. #elif CORE_IS_YZ && Z_SENSORLESS
  1387. tmc_disable_stallguard(stepperZ, enable_stealth.z);
  1388. #endif
  1389. break;
  1390. #endif
  1391. #if Z_SENSORLESS
  1392. case Z_AXIS:
  1393. tmc_disable_stallguard(stepperZ, enable_stealth.z);
  1394. TERN_(Z2_SENSORLESS, tmc_disable_stallguard(stepperZ2, enable_stealth.z2));
  1395. TERN_(Z3_SENSORLESS, tmc_disable_stallguard(stepperZ3, enable_stealth.z3));
  1396. TERN_(Z4_SENSORLESS, tmc_disable_stallguard(stepperZ4, enable_stealth.z4));
  1397. #if CORE_IS_XZ && X_SENSORLESS
  1398. tmc_disable_stallguard(stepperX, enable_stealth.x);
  1399. #elif CORE_IS_YZ && Y_SENSORLESS
  1400. tmc_disable_stallguard(stepperY, enable_stealth.y);
  1401. #endif
  1402. break;
  1403. #endif
  1404. #if I_SENSORLESS
  1405. case I_AXIS: tmc_disable_stallguard(stepperI, enable_stealth.i); break;
  1406. #endif
  1407. #if J_SENSORLESS
  1408. case J_AXIS: tmc_disable_stallguard(stepperJ, enable_stealth.j); break;
  1409. #endif
  1410. #if K_SENSORLESS
  1411. case K_AXIS: tmc_disable_stallguard(stepperK, enable_stealth.k); break;
  1412. #endif
  1413. #if U_SENSORLESS
  1414. case U_AXIS: tmc_disable_stallguard(stepperU, enable_stealth.u); break;
  1415. #endif
  1416. #if V_SENSORLESS
  1417. case V_AXIS: tmc_disable_stallguard(stepperV, enable_stealth.v); break;
  1418. #endif
  1419. #if W_SENSORLESS
  1420. case W_AXIS: tmc_disable_stallguard(stepperW, enable_stealth.w); break;
  1421. #endif
  1422. }
  1423. #if ENABLED(SPI_ENDSTOPS)
  1424. switch (axis) {
  1425. case X_AXIS: if (ENABLED(X_SPI_SENSORLESS)) endstops.tmc_spi_homing.x = false; break;
  1426. #if HAS_Y_AXIS
  1427. case Y_AXIS: if (ENABLED(Y_SPI_SENSORLESS)) endstops.tmc_spi_homing.y = false; break;
  1428. #endif
  1429. #if HAS_Z_AXIS
  1430. case Z_AXIS: if (ENABLED(Z_SPI_SENSORLESS)) endstops.tmc_spi_homing.z = false; break;
  1431. #endif
  1432. #if HAS_I_AXIS
  1433. case I_AXIS: if (ENABLED(I_SPI_SENSORLESS)) endstops.tmc_spi_homing.i = false; break;
  1434. #endif
  1435. #if HAS_J_AXIS
  1436. case J_AXIS: if (ENABLED(J_SPI_SENSORLESS)) endstops.tmc_spi_homing.j = false; break;
  1437. #endif
  1438. #if HAS_K_AXIS
  1439. case K_AXIS: if (ENABLED(K_SPI_SENSORLESS)) endstops.tmc_spi_homing.k = false; break;
  1440. #endif
  1441. #if HAS_U_AXIS
  1442. case U_AXIS: if (ENABLED(U_SPI_SENSORLESS)) endstops.tmc_spi_homing.u = false; break;
  1443. #endif
  1444. #if HAS_V_AXIS
  1445. case V_AXIS: if (ENABLED(V_SPI_SENSORLESS)) endstops.tmc_spi_homing.v = false; break;
  1446. #endif
  1447. #if HAS_W_AXIS
  1448. case W_AXIS: if (ENABLED(W_SPI_SENSORLESS)) endstops.tmc_spi_homing.w = false; break;
  1449. #endif
  1450. default: break;
  1451. }
  1452. #endif
  1453. }
  1454. #endif // SENSORLESS_HOMING
  1455. /**
  1456. * Home an individual linear axis
  1457. */
  1458. void do_homing_move(const AxisEnum axis, const float distance, const feedRate_t fr_mm_s=0.0, const bool final_approach=true) {
  1459. DEBUG_SECTION(log_move, "do_homing_move", DEBUGGING(LEVELING));
  1460. const feedRate_t home_fr_mm_s = fr_mm_s ?: homing_feedrate(axis);
  1461. if (DEBUGGING(LEVELING)) {
  1462. DEBUG_ECHOPGM("...(", AS_CHAR(AXIS_CHAR(axis)), ", ", distance, ", ");
  1463. if (fr_mm_s)
  1464. DEBUG_ECHO(fr_mm_s);
  1465. else
  1466. DEBUG_ECHOPGM("[", home_fr_mm_s, "]");
  1467. DEBUG_ECHOLNPGM(")");
  1468. }
  1469. // Only do some things when moving towards an endstop
  1470. const int8_t axis_home_dir = TERN0(DUAL_X_CARRIAGE, axis == X_AXIS)
  1471. ? TOOL_X_HOME_DIR(active_extruder) : home_dir(axis);
  1472. const bool is_home_dir = (axis_home_dir > 0) == (distance > 0);
  1473. #if ENABLED(SENSORLESS_HOMING)
  1474. sensorless_t stealth_states;
  1475. #endif
  1476. if (is_home_dir) {
  1477. if (TERN0(HOMING_Z_WITH_PROBE, axis == Z_AXIS)) {
  1478. #if BOTH(HAS_HEATED_BED, WAIT_FOR_BED_HEATER)
  1479. // Wait for bed to heat back up between probing points
  1480. thermalManager.wait_for_bed_heating();
  1481. #endif
  1482. #if BOTH(HAS_HOTEND, WAIT_FOR_HOTEND)
  1483. // Wait for the hotend to heat back up between probing points
  1484. thermalManager.wait_for_hotend_heating(active_extruder);
  1485. #endif
  1486. TERN_(HAS_QUIET_PROBING, if (final_approach) probe.set_probing_paused(true));
  1487. }
  1488. // Disable stealthChop if used. Enable diag1 pin on driver.
  1489. #if ENABLED(SENSORLESS_HOMING)
  1490. stealth_states = start_sensorless_homing_per_axis(axis);
  1491. #if SENSORLESS_STALLGUARD_DELAY
  1492. safe_delay(SENSORLESS_STALLGUARD_DELAY); // Short delay needed to settle
  1493. #endif
  1494. #endif
  1495. }
  1496. #if EITHER(MORGAN_SCARA, MP_SCARA)
  1497. // Tell the planner the axis is at 0
  1498. current_position[axis] = 0;
  1499. sync_plan_position();
  1500. current_position[axis] = distance;
  1501. line_to_current_position(home_fr_mm_s);
  1502. #else
  1503. // Get the ABC or XYZ positions in mm
  1504. abce_pos_t target = planner.get_axis_positions_mm();
  1505. target[axis] = 0; // Set the single homing axis to 0
  1506. planner.set_machine_position_mm(target); // Update the machine position
  1507. #if HAS_DIST_MM_ARG
  1508. const xyze_float_t cart_dist_mm{0};
  1509. #endif
  1510. // Set delta/cartesian axes directly
  1511. target[axis] = distance; // The move will be towards the endstop
  1512. planner.buffer_segment(target OPTARG(HAS_DIST_MM_ARG, cart_dist_mm), home_fr_mm_s, active_extruder);
  1513. #endif
  1514. planner.synchronize();
  1515. if (is_home_dir) {
  1516. #if HOMING_Z_WITH_PROBE && HAS_QUIET_PROBING
  1517. if (axis == Z_AXIS && final_approach) probe.set_probing_paused(false);
  1518. #endif
  1519. endstops.validate_homing_move();
  1520. // Re-enable stealthChop if used. Disable diag1 pin on driver.
  1521. #if ENABLED(SENSORLESS_HOMING)
  1522. end_sensorless_homing_per_axis(axis, stealth_states);
  1523. #if SENSORLESS_STALLGUARD_DELAY
  1524. safe_delay(SENSORLESS_STALLGUARD_DELAY); // Short delay needed to settle
  1525. #endif
  1526. #endif
  1527. }
  1528. }
  1529. /**
  1530. * Set an axis to be unhomed. (Unless we are on a machine - e.g. a cheap Chinese CNC machine -
  1531. * that has no endstops. Such machines should always be considered to be in a "known" and
  1532. * "trusted" position).
  1533. */
  1534. void set_axis_never_homed(const AxisEnum axis) {
  1535. if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM(">>> set_axis_never_homed(", AS_CHAR(AXIS_CHAR(axis)), ")");
  1536. set_axis_untrusted(axis);
  1537. set_axis_unhomed(axis);
  1538. if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("<<< set_axis_never_homed(", AS_CHAR(AXIS_CHAR(axis)), ")");
  1539. TERN_(I2C_POSITION_ENCODERS, I2CPEM.unhomed(axis));
  1540. }
  1541. #ifdef TMC_HOME_PHASE
  1542. /**
  1543. * Move the axis back to its home_phase if set and driver is capable (TMC)
  1544. *
  1545. * Improves homing repeatability by homing to stepper coil's nearest absolute
  1546. * phase position. Trinamic drivers use a stepper phase table with 1024 values
  1547. * spanning 4 full steps with 256 positions each (ergo, 1024 positions).
  1548. */
  1549. void backout_to_tmc_homing_phase(const AxisEnum axis) {
  1550. const xyz_long_t home_phase = TMC_HOME_PHASE;
  1551. // check if home phase is disabled for this axis.
  1552. if (home_phase[axis] < 0) return;
  1553. int16_t phasePerUStep, // TMC µsteps(phase) per Marlin µsteps
  1554. phaseCurrent, // The TMC µsteps(phase) count of the current position
  1555. effectorBackoutDir, // Direction in which the effector mm coordinates move away from endstop.
  1556. stepperBackoutDir; // Direction in which the TMC µstep count(phase) move away from endstop.
  1557. #define PHASE_PER_MICROSTEP(N) (256 / _MAX(1, N##_MICROSTEPS))
  1558. switch (axis) {
  1559. #ifdef X_MICROSTEPS
  1560. case X_AXIS:
  1561. phasePerUStep = PHASE_PER_MICROSTEP(X);
  1562. phaseCurrent = stepperX.get_microstep_counter();
  1563. effectorBackoutDir = -X_HOME_DIR;
  1564. stepperBackoutDir = IF_DISABLED(INVERT_X_DIR, -)effectorBackoutDir;
  1565. break;
  1566. #endif
  1567. #ifdef Y_MICROSTEPS
  1568. case Y_AXIS:
  1569. phasePerUStep = PHASE_PER_MICROSTEP(Y);
  1570. phaseCurrent = stepperY.get_microstep_counter();
  1571. effectorBackoutDir = -Y_HOME_DIR;
  1572. stepperBackoutDir = IF_DISABLED(INVERT_Y_DIR, -)effectorBackoutDir;
  1573. break;
  1574. #endif
  1575. #ifdef Z_MICROSTEPS
  1576. case Z_AXIS:
  1577. phasePerUStep = PHASE_PER_MICROSTEP(Z);
  1578. phaseCurrent = stepperZ.get_microstep_counter();
  1579. effectorBackoutDir = -Z_HOME_DIR;
  1580. stepperBackoutDir = IF_DISABLED(INVERT_Z_DIR, -)effectorBackoutDir;
  1581. break;
  1582. #endif
  1583. #ifdef I_MICROSTEPS
  1584. case I_AXIS:
  1585. phasePerUStep = PHASE_PER_MICROSTEP(I);
  1586. phaseCurrent = stepperI.get_microstep_counter();
  1587. effectorBackoutDir = -I_HOME_DIR;
  1588. stepperBackoutDir = IF_DISABLED(INVERT_I_DIR, -)effectorBackoutDir;
  1589. break;
  1590. #endif
  1591. #ifdef J_MICROSTEPS
  1592. case J_AXIS:
  1593. phasePerUStep = PHASE_PER_MICROSTEP(J);
  1594. phaseCurrent = stepperJ.get_microstep_counter();
  1595. effectorBackoutDir = -J_HOME_DIR;
  1596. stepperBackoutDir = IF_DISABLED(INVERT_J_DIR, -)effectorBackoutDir;
  1597. break;
  1598. #endif
  1599. #ifdef K_MICROSTEPS
  1600. case K_AXIS:
  1601. phasePerUStep = PHASE_PER_MICROSTEP(K);
  1602. phaseCurrent = stepperK.get_microstep_counter();
  1603. effectorBackoutDir = -K_HOME_DIR;
  1604. stepperBackoutDir = IF_DISABLED(INVERT_K_DIR, -)effectorBackoutDir;
  1605. break;
  1606. #endif
  1607. #ifdef U_MICROSTEPS
  1608. case U_AXIS:
  1609. phasePerUStep = PHASE_PER_MICROSTEP(U);
  1610. phaseCurrent = stepperU.get_microstep_counter();
  1611. effectorBackoutDir = -U_HOME_DIR;
  1612. stepperBackoutDir = IF_DISABLED(INVERT_U_DIR, -)effectorBackoutDir;
  1613. break;
  1614. #endif
  1615. #ifdef V_MICROSTEPS
  1616. case V_AXIS:
  1617. phasePerUStep = PHASE_PER_MICROSTEP(V);
  1618. phaseCurrent = stepperV.get_microstep_counter();
  1619. effectorBackoutDir = -V_HOME_DIR;
  1620. stepperBackoutDir = IF_DISABLED(INVERT_V_DIR, -)effectorBackoutDir;
  1621. break;
  1622. #endif
  1623. #ifdef W_MICROSTEPS
  1624. case W_AXIS:
  1625. phasePerUStep = PHASE_PER_MICROSTEP(W);
  1626. phaseCurrent = stepperW.get_microstep_counter();
  1627. effectorBackoutDir = -W_HOME_DIR;
  1628. stepperBackoutDir = IF_DISABLED(INVERT_W_DIR, -)effectorBackoutDir;
  1629. break;
  1630. #endif
  1631. default: return;
  1632. }
  1633. // Phase distance to nearest home phase position when moving in the backout direction from endstop(may be negative).
  1634. int16_t phaseDelta = (home_phase[axis] - phaseCurrent) * stepperBackoutDir;
  1635. // Check if home distance within endstop assumed repeatability noise of .05mm and warn.
  1636. if (ABS(phaseDelta) * planner.mm_per_step[axis] / phasePerUStep < 0.05f)
  1637. SERIAL_ECHOLNPGM("Selected home phase ", home_phase[axis],
  1638. " too close to endstop trigger phase ", phaseCurrent,
  1639. ". Pick a different phase for ", AS_CHAR(AXIS_CHAR(axis)));
  1640. // Skip to next if target position is behind current. So it only moves away from endstop.
  1641. if (phaseDelta < 0) phaseDelta += 1024;
  1642. // Convert TMC µsteps(phase) to whole Marlin µsteps to effector backout direction to mm
  1643. const float mmDelta = int16_t(phaseDelta / phasePerUStep) * effectorBackoutDir * planner.mm_per_step[axis];
  1644. // Optional debug messages
  1645. if (DEBUGGING(LEVELING)) {
  1646. DEBUG_ECHOLNPGM(
  1647. "Endstop ", AS_CHAR(AXIS_CHAR(axis)), " hit at Phase:", phaseCurrent,
  1648. " Delta:", phaseDelta, " Distance:", mmDelta
  1649. );
  1650. }
  1651. if (mmDelta != 0) {
  1652. // Retrace by the amount computed in mmDelta.
  1653. do_homing_move(axis, mmDelta, get_homing_bump_feedrate(axis));
  1654. }
  1655. }
  1656. #endif
  1657. /**
  1658. * Home an individual "raw axis" to its endstop.
  1659. * This applies to XYZ on Cartesian and Core robots, and
  1660. * to the individual ABC steppers on DELTA and SCARA.
  1661. *
  1662. * At the end of the procedure the axis is marked as
  1663. * homed and the current position of that axis is updated.
  1664. * Kinematic robots should wait till all axes are homed
  1665. * before updating the current position.
  1666. */
  1667. void homeaxis(const AxisEnum axis) {
  1668. #if EITHER(MORGAN_SCARA, MP_SCARA)
  1669. // Only Z homing (with probe) is permitted
  1670. if (axis != Z_AXIS) { BUZZ(100, 880); return; }
  1671. #else
  1672. #define _CAN_HOME(A) (axis == _AXIS(A) && ( \
  1673. ENABLED(A##_SPI_SENSORLESS) \
  1674. || TERN0(HAS_Z_AXIS, TERN0(HOMING_Z_WITH_PROBE, _AXIS(A) == Z_AXIS)) \
  1675. || TERN0(A##_HOME_TO_MIN, A##_MIN_PIN > -1) \
  1676. || TERN0(A##_HOME_TO_MAX, A##_MAX_PIN > -1) \
  1677. ))
  1678. #define _ANDCANT(N) && !_CAN_HOME(N)
  1679. if (true MAIN_AXIS_MAP(_ANDCANT)) return;
  1680. #endif
  1681. if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM(">>> homeaxis(", AS_CHAR(AXIS_CHAR(axis)), ")");
  1682. const int axis_home_dir = TERN0(DUAL_X_CARRIAGE, axis == X_AXIS)
  1683. ? TOOL_X_HOME_DIR(active_extruder) : home_dir(axis);
  1684. //
  1685. // Homing Z with a probe? Raise Z (maybe) and deploy the Z probe.
  1686. //
  1687. if (TERN0(HOMING_Z_WITH_PROBE, axis == Z_AXIS && probe.deploy()))
  1688. return;
  1689. // Set flags for X, Y, Z motor locking
  1690. #if HAS_EXTRA_ENDSTOPS
  1691. switch (axis) {
  1692. TERN_(X_DUAL_ENDSTOPS, case X_AXIS:)
  1693. TERN_(Y_DUAL_ENDSTOPS, case Y_AXIS:)
  1694. TERN_(Z_MULTI_ENDSTOPS, case Z_AXIS:)
  1695. stepper.set_separate_multi_axis(true);
  1696. default: break;
  1697. }
  1698. #endif
  1699. //
  1700. // Deploy BLTouch or tare the probe just before probing
  1701. //
  1702. #if HOMING_Z_WITH_PROBE
  1703. if (axis == Z_AXIS) {
  1704. if (TERN0(BLTOUCH, bltouch.deploy())) return; // BLTouch was deployed above, but get the alarm state.
  1705. if (TERN0(PROBE_TARE, probe.tare())) return;
  1706. }
  1707. #endif
  1708. //
  1709. // Back away to prevent an early sensorless trigger
  1710. //
  1711. #if DISABLED(DELTA) && defined(SENSORLESS_BACKOFF_MM)
  1712. const xyz_float_t backoff = SENSORLESS_BACKOFF_MM;
  1713. if ((TERN0(X_SENSORLESS, axis == X_AXIS) || TERN0(Y_SENSORLESS, axis == Y_AXIS) || TERN0(Z_SENSORLESS, axis == Z_AXIS) || TERN0(I_SENSORLESS, axis == I_AXIS) || TERN0(J_SENSORLESS, axis == J_AXIS) || TERN0(K_SENSORLESS, axis == K_AXIS)) && backoff[axis]) {
  1714. const float backoff_length = -ABS(backoff[axis]) * axis_home_dir;
  1715. if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Sensorless backoff: ", backoff_length, "mm");
  1716. do_homing_move(axis, backoff_length, homing_feedrate(axis));
  1717. }
  1718. #endif
  1719. //
  1720. // Back away to prevent opposite endstop damage
  1721. //
  1722. #if !defined(SENSORLESS_BACKOFF_MM) && XY_COUNTERPART_BACKOFF_MM
  1723. if (!(axis_was_homed(X_AXIS) || axis_was_homed(Y_AXIS)) && (axis == X_AXIS || axis == Y_AXIS)) {
  1724. const AxisEnum opposite_axis = axis == X_AXIS ? Y_AXIS : X_AXIS;
  1725. const float backoff_length = -ABS(XY_COUNTERPART_BACKOFF_MM) * home_dir(opposite_axis);
  1726. do_homing_move(opposite_axis, backoff_length, homing_feedrate(opposite_axis));
  1727. }
  1728. #endif
  1729. // Determine if a homing bump will be done and the bumps distance
  1730. // When homing Z with probe respect probe clearance
  1731. const bool use_probe_bump = TERN0(HOMING_Z_WITH_PROBE, axis == Z_AXIS && home_bump_mm(axis));
  1732. const float bump = axis_home_dir * (
  1733. use_probe_bump ? _MAX(TERN0(HOMING_Z_WITH_PROBE, Z_CLEARANCE_BETWEEN_PROBES), home_bump_mm(axis)) : home_bump_mm(axis)
  1734. );
  1735. //
  1736. // Fast move towards endstop until triggered
  1737. //
  1738. const float move_length = 1.5f * max_length(TERN(DELTA, Z_AXIS, axis)) * axis_home_dir;
  1739. if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Home Fast: ", move_length, "mm");
  1740. do_homing_move(axis, move_length, 0.0, !use_probe_bump);
  1741. #if BOTH(HOMING_Z_WITH_PROBE, BLTOUCH)
  1742. if (axis == Z_AXIS && !bltouch.high_speed_mode) bltouch.stow(); // Intermediate STOW (in LOW SPEED MODE)
  1743. #endif
  1744. // If a second homing move is configured...
  1745. if (bump) {
  1746. // Move away from the endstop by the axis HOMING_BUMP_MM
  1747. if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Move Away: ", -bump, "mm");
  1748. do_homing_move(axis, -bump, TERN(HOMING_Z_WITH_PROBE, (axis == Z_AXIS ? z_probe_fast_mm_s : 0), 0), false);
  1749. #if ENABLED(DETECT_BROKEN_ENDSTOP)
  1750. // Check for a broken endstop
  1751. EndstopEnum es;
  1752. switch (axis) {
  1753. default:
  1754. case X_AXIS: es = X_ENDSTOP; break;
  1755. #if HAS_Y_AXIS
  1756. case Y_AXIS: es = Y_ENDSTOP; break;
  1757. #endif
  1758. #if HAS_Z_AXIS
  1759. case Z_AXIS: es = Z_ENDSTOP; break;
  1760. #endif
  1761. #if HAS_I_AXIS
  1762. case I_AXIS: es = I_ENDSTOP; break;
  1763. #endif
  1764. #if HAS_J_AXIS
  1765. case J_AXIS: es = J_ENDSTOP; break;
  1766. #endif
  1767. #if HAS_K_AXIS
  1768. case K_AXIS: es = K_ENDSTOP; break;
  1769. #endif
  1770. #if HAS_U_AXIS
  1771. case U_AXIS: es = U_ENDSTOP; break;
  1772. #endif
  1773. #if HAS_V_AXIS
  1774. case V_AXIS: es = V_ENDSTOP; break;
  1775. #endif
  1776. #if HAS_W_AXIS
  1777. case W_AXIS: es = W_ENDSTOP; break;
  1778. #endif
  1779. }
  1780. if (TEST(endstops.state(), es)) {
  1781. SERIAL_ECHO_MSG("Bad ", AS_CHAR(AXIS_CHAR(axis)), " Endstop?");
  1782. kill(GET_TEXT_F(MSG_KILL_HOMING_FAILED));
  1783. }
  1784. #endif
  1785. #if BOTH(HOMING_Z_WITH_PROBE, BLTOUCH)
  1786. if (axis == Z_AXIS && !bltouch.high_speed_mode && bltouch.deploy())
  1787. return; // Intermediate DEPLOY (in LOW SPEED MODE)
  1788. #endif
  1789. // Slow move towards endstop until triggered
  1790. const float rebump = bump * 2;
  1791. if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Re-bump: ", rebump, "mm");
  1792. do_homing_move(axis, rebump, get_homing_bump_feedrate(axis), true);
  1793. #if BOTH(HOMING_Z_WITH_PROBE, BLTOUCH)
  1794. if (axis == Z_AXIS) bltouch.stow(); // The final STOW
  1795. #endif
  1796. }
  1797. #if HAS_EXTRA_ENDSTOPS
  1798. const bool pos_dir = axis_home_dir > 0;
  1799. #if ENABLED(X_DUAL_ENDSTOPS)
  1800. if (axis == X_AXIS) {
  1801. const float adj = ABS(endstops.x2_endstop_adj);
  1802. if (adj) {
  1803. if (pos_dir ? (endstops.x2_endstop_adj > 0) : (endstops.x2_endstop_adj < 0)) stepper.set_x_lock(true); else stepper.set_x2_lock(true);
  1804. do_homing_move(axis, pos_dir ? -adj : adj);
  1805. stepper.set_x_lock(false);
  1806. stepper.set_x2_lock(false);
  1807. }
  1808. }
  1809. #endif
  1810. #if ENABLED(Y_DUAL_ENDSTOPS)
  1811. if (axis == Y_AXIS) {
  1812. const float adj = ABS(endstops.y2_endstop_adj);
  1813. if (adj) {
  1814. if (pos_dir ? (endstops.y2_endstop_adj > 0) : (endstops.y2_endstop_adj < 0)) stepper.set_y_lock(true); else stepper.set_y2_lock(true);
  1815. do_homing_move(axis, pos_dir ? -adj : adj);
  1816. stepper.set_y_lock(false);
  1817. stepper.set_y2_lock(false);
  1818. }
  1819. }
  1820. #endif
  1821. #if ENABLED(Z_MULTI_ENDSTOPS)
  1822. if (axis == Z_AXIS) {
  1823. #if NUM_Z_STEPPERS == 2
  1824. const float adj = ABS(endstops.z2_endstop_adj);
  1825. if (adj) {
  1826. if (pos_dir ? (endstops.z2_endstop_adj > 0) : (endstops.z2_endstop_adj < 0)) stepper.set_z1_lock(true); else stepper.set_z2_lock(true);
  1827. do_homing_move(axis, pos_dir ? -adj : adj);
  1828. stepper.set_z1_lock(false);
  1829. stepper.set_z2_lock(false);
  1830. }
  1831. #else
  1832. // Handy arrays of stepper lock function pointers
  1833. typedef void (*adjustFunc_t)(const bool);
  1834. adjustFunc_t lock[] = {
  1835. stepper.set_z1_lock, stepper.set_z2_lock, stepper.set_z3_lock
  1836. #if NUM_Z_STEPPERS >= 4
  1837. , stepper.set_z4_lock
  1838. #endif
  1839. };
  1840. float adj[] = {
  1841. 0, endstops.z2_endstop_adj, endstops.z3_endstop_adj
  1842. #if NUM_Z_STEPPERS >= 4
  1843. , endstops.z4_endstop_adj
  1844. #endif
  1845. };
  1846. adjustFunc_t tempLock;
  1847. float tempAdj;
  1848. // Manual bubble sort by adjust value
  1849. if (adj[1] < adj[0]) {
  1850. tempLock = lock[0], tempAdj = adj[0];
  1851. lock[0] = lock[1], adj[0] = adj[1];
  1852. lock[1] = tempLock, adj[1] = tempAdj;
  1853. }
  1854. if (adj[2] < adj[1]) {
  1855. tempLock = lock[1], tempAdj = adj[1];
  1856. lock[1] = lock[2], adj[1] = adj[2];
  1857. lock[2] = tempLock, adj[2] = tempAdj;
  1858. }
  1859. #if NUM_Z_STEPPERS >= 4
  1860. if (adj[3] < adj[2]) {
  1861. tempLock = lock[2], tempAdj = adj[2];
  1862. lock[2] = lock[3], adj[2] = adj[3];
  1863. lock[3] = tempLock, adj[3] = tempAdj;
  1864. }
  1865. if (adj[2] < adj[1]) {
  1866. tempLock = lock[1], tempAdj = adj[1];
  1867. lock[1] = lock[2], adj[1] = adj[2];
  1868. lock[2] = tempLock, adj[2] = tempAdj;
  1869. }
  1870. #endif
  1871. if (adj[1] < adj[0]) {
  1872. tempLock = lock[0], tempAdj = adj[0];
  1873. lock[0] = lock[1], adj[0] = adj[1];
  1874. lock[1] = tempLock, adj[1] = tempAdj;
  1875. }
  1876. if (pos_dir) {
  1877. // normalize adj to smallest value and do the first move
  1878. (*lock[0])(true);
  1879. do_homing_move(axis, adj[1] - adj[0]);
  1880. // lock the second stepper for the final correction
  1881. (*lock[1])(true);
  1882. do_homing_move(axis, adj[2] - adj[1]);
  1883. #if NUM_Z_STEPPERS >= 4
  1884. // lock the third stepper for the final correction
  1885. (*lock[2])(true);
  1886. do_homing_move(axis, adj[3] - adj[2]);
  1887. #endif
  1888. }
  1889. else {
  1890. #if NUM_Z_STEPPERS >= 4
  1891. (*lock[3])(true);
  1892. do_homing_move(axis, adj[2] - adj[3]);
  1893. #endif
  1894. (*lock[2])(true);
  1895. do_homing_move(axis, adj[1] - adj[2]);
  1896. (*lock[1])(true);
  1897. do_homing_move(axis, adj[0] - adj[1]);
  1898. }
  1899. stepper.set_z1_lock(false);
  1900. stepper.set_z2_lock(false);
  1901. stepper.set_z3_lock(false);
  1902. #if NUM_Z_STEPPERS >= 4
  1903. stepper.set_z4_lock(false);
  1904. #endif
  1905. #endif
  1906. }
  1907. #endif
  1908. // Reset flags for X, Y, Z motor locking
  1909. switch (axis) {
  1910. default: break;
  1911. TERN_(X_DUAL_ENDSTOPS, case X_AXIS:)
  1912. TERN_(Y_DUAL_ENDSTOPS, case Y_AXIS:)
  1913. TERN_(Z_MULTI_ENDSTOPS, case Z_AXIS:)
  1914. stepper.set_separate_multi_axis(false);
  1915. }
  1916. #endif // HAS_EXTRA_ENDSTOPS
  1917. #ifdef TMC_HOME_PHASE
  1918. // move back to homing phase if configured and capable
  1919. backout_to_tmc_homing_phase(axis);
  1920. #endif
  1921. #if IS_SCARA
  1922. set_axis_is_at_home(axis);
  1923. sync_plan_position();
  1924. #elif ENABLED(DELTA)
  1925. // Delta has already moved all three towers up in G28
  1926. // so here it re-homes each tower in turn.
  1927. // Delta homing treats the axes as normal linear axes.
  1928. const float adjDistance = delta_endstop_adj[axis],
  1929. minDistance = (MIN_STEPS_PER_SEGMENT) * planner.mm_per_step[axis];
  1930. // Retrace by the amount specified in delta_endstop_adj if more than min steps.
  1931. if (adjDistance * (Z_HOME_DIR) < 0 && ABS(adjDistance) > minDistance) { // away from endstop, more than min distance
  1932. if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("adjDistance:", adjDistance);
  1933. do_homing_move(axis, adjDistance, get_homing_bump_feedrate(axis));
  1934. }
  1935. #else // CARTESIAN / CORE / MARKFORGED_XY / MARKFORGED_YX
  1936. set_axis_is_at_home(axis);
  1937. sync_plan_position();
  1938. destination[axis] = current_position[axis];
  1939. if (DEBUGGING(LEVELING)) DEBUG_POS("> AFTER set_axis_is_at_home", current_position);
  1940. #endif
  1941. // Put away the Z probe
  1942. #if HOMING_Z_WITH_PROBE
  1943. if (axis == Z_AXIS && probe.stow()) return;
  1944. #endif
  1945. #if DISABLED(DELTA) && defined(HOMING_BACKOFF_POST_MM)
  1946. const xyz_float_t endstop_backoff = HOMING_BACKOFF_POST_MM;
  1947. if (endstop_backoff[axis]) {
  1948. current_position[axis] -= ABS(endstop_backoff[axis]) * axis_home_dir;
  1949. line_to_current_position(
  1950. #if HOMING_Z_WITH_PROBE
  1951. (axis == Z_AXIS) ? z_probe_fast_mm_s :
  1952. #endif
  1953. homing_feedrate(axis)
  1954. );
  1955. #if ENABLED(SENSORLESS_HOMING)
  1956. planner.synchronize();
  1957. if (false
  1958. #if ANY(IS_CORE, MARKFORGED_XY, MARKFORGED_YX)
  1959. || axis != NORMAL_AXIS
  1960. #endif
  1961. ) safe_delay(200); // Short delay to allow belts to spring back
  1962. #endif
  1963. }
  1964. #endif
  1965. // Clear retracted status if homing the Z axis
  1966. #if ENABLED(FWRETRACT)
  1967. if (axis == Z_AXIS) fwretract.current_hop = 0.0;
  1968. #endif
  1969. if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("<<< homeaxis(", AS_CHAR(AXIS_CHAR(axis)), ")");
  1970. } // homeaxis()
  1971. #endif // HAS_ENDSTOPS
  1972. /**
  1973. * Set an axis' current position to its home position (after homing).
  1974. *
  1975. * For Core and Cartesian robots this applies one-to-one when an
  1976. * individual axis has been homed.
  1977. *
  1978. * DELTA should wait until all homing is done before setting the XYZ
  1979. * current_position to home, because homing is a single operation.
  1980. * In the case where the axis positions are trusted and previously
  1981. * homed, DELTA could home to X or Y individually by moving either one
  1982. * to the center. However, homing Z always homes XY and Z.
  1983. *
  1984. * SCARA should wait until all XY homing is done before setting the XY
  1985. * current_position to home, because neither X nor Y is at home until
  1986. * both are at home. Z can however be homed individually.
  1987. *
  1988. * Callers must sync the planner position after calling this!
  1989. */
  1990. void set_axis_is_at_home(const AxisEnum axis) {
  1991. if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM(">>> set_axis_is_at_home(", AS_CHAR(AXIS_CHAR(axis)), ")");
  1992. set_axis_trusted(axis);
  1993. set_axis_homed(axis);
  1994. #if ENABLED(DUAL_X_CARRIAGE)
  1995. if (axis == X_AXIS && (active_extruder == 1 || dual_x_carriage_mode == DXC_DUPLICATION_MODE)) {
  1996. current_position.x = x_home_pos(active_extruder);
  1997. return;
  1998. }
  1999. #endif
  2000. #if EITHER(MORGAN_SCARA, AXEL_TPARA)
  2001. scara_set_axis_is_at_home(axis);
  2002. #elif ENABLED(DELTA)
  2003. current_position[axis] = (axis == Z_AXIS) ? DIFF_TERN(HAS_BED_PROBE, delta_height, probe.offset.z) : base_home_pos(axis);
  2004. #else
  2005. current_position[axis] = base_home_pos(axis);
  2006. #endif
  2007. /**
  2008. * Z Probe Z Homing? Account for the probe's Z offset.
  2009. */
  2010. #if HAS_BED_PROBE && Z_HOME_TO_MIN
  2011. if (axis == Z_AXIS) {
  2012. #if HOMING_Z_WITH_PROBE
  2013. current_position.z -= probe.offset.z;
  2014. if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("*** Z HOMED WITH PROBE (Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN) ***\n> probe.offset.z = ", probe.offset.z);
  2015. #else
  2016. if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("*** Z HOMED TO ENDSTOP ***");
  2017. #endif
  2018. }
  2019. #endif
  2020. TERN_(I2C_POSITION_ENCODERS, I2CPEM.homed(axis));
  2021. TERN_(BABYSTEP_DISPLAY_TOTAL, babystep.reset_total(axis));
  2022. #if HAS_POSITION_SHIFT
  2023. position_shift[axis] = 0;
  2024. update_workspace_offset(axis);
  2025. #endif
  2026. if (DEBUGGING(LEVELING)) {
  2027. #if HAS_HOME_OFFSET
  2028. DEBUG_ECHOLNPGM("> home_offset[", AS_CHAR(AXIS_CHAR(axis)), "] = ", home_offset[axis]);
  2029. #endif
  2030. DEBUG_POS("", current_position);
  2031. DEBUG_ECHOLNPGM("<<< set_axis_is_at_home(", AS_CHAR(AXIS_CHAR(axis)), ")");
  2032. }
  2033. }
  2034. #if HAS_WORKSPACE_OFFSET
  2035. void update_workspace_offset(const AxisEnum axis) {
  2036. workspace_offset[axis] = home_offset[axis] + position_shift[axis];
  2037. if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Axis ", AS_CHAR(AXIS_CHAR(axis)), " home_offset = ", home_offset[axis], " position_shift = ", position_shift[axis]);
  2038. }
  2039. #endif
  2040. #if HAS_M206_COMMAND
  2041. /**
  2042. * Change the home offset for an axis.
  2043. * Also refreshes the workspace offset.
  2044. */
  2045. void set_home_offset(const AxisEnum axis, const float v) {
  2046. home_offset[axis] = v;
  2047. update_workspace_offset(axis);
  2048. }
  2049. #endif