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

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312
  1. /**
  2. * Marlin 3D Printer Firmware
  3. * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
  4. *
  5. * Based on Sprinter and grbl.
  6. * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
  7. *
  8. * This program is free software: you can redistribute it and/or modify
  9. * it under the terms of the GNU General Public License as published by
  10. * the Free Software Foundation, either version 3 of the License, or
  11. * (at your option) any later version.
  12. *
  13. * This program is distributed in the hope that it will be useful,
  14. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  16. * GNU General Public License for more details.
  17. *
  18. * You should have received a copy of the GNU General Public License
  19. * along with this program. If not, see <http://www.gnu.org/licenses/>.
  20. *
  21. */
  22. /**
  23. * 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 "../inc/MarlinConfig.h"
  32. #if IS_SCARA
  33. #include "../libs/buzzer.h"
  34. #include "../lcd/ultralcd.h"
  35. #endif
  36. #if HAS_BED_PROBE
  37. #include "probe.h"
  38. #endif
  39. #if HAS_LEVELING
  40. #include "../feature/bedlevel/bedlevel.h"
  41. #endif
  42. #if HAS_AXIS_UNHOMED_ERR && ENABLED(ULTRA_LCD)
  43. #include "../lcd/ultralcd.h"
  44. #endif
  45. #if ENABLED(SENSORLESS_HOMING)
  46. #include "../feature/tmc2130.h"
  47. #endif
  48. #define XYZ_CONSTS(type, array, CONFIG) const PROGMEM type array##_P[XYZ] = { X_##CONFIG, Y_##CONFIG, Z_##CONFIG }
  49. XYZ_CONSTS(float, base_min_pos, MIN_POS);
  50. XYZ_CONSTS(float, base_max_pos, MAX_POS);
  51. XYZ_CONSTS(float, base_home_pos, HOME_POS);
  52. XYZ_CONSTS(float, max_length, MAX_LENGTH);
  53. XYZ_CONSTS(float, home_bump_mm, HOME_BUMP_MM);
  54. XYZ_CONSTS(signed char, home_dir, HOME_DIR);
  55. // Relative Mode. Enable with G91, disable with G90.
  56. bool relative_mode = false;
  57. /**
  58. * Cartesian Current Position
  59. * Used to track the native machine position as moves are queued.
  60. * Used by 'buffer_line_to_current_position' to do a move after changing it.
  61. * Used by 'SYNC_PLAN_POSITION_KINEMATIC' to update 'planner.position'.
  62. */
  63. float current_position[XYZE] = { 0.0 };
  64. /**
  65. * Cartesian Destination
  66. * The destination for a move, filled in by G-code movement commands,
  67. * and expected by functions like 'prepare_move_to_destination'.
  68. * Set with 'gcode_get_destination' or 'set_destination_from_current'.
  69. */
  70. float destination[XYZE] = { 0.0 };
  71. // The active extruder (tool). Set with T<extruder> command.
  72. uint8_t active_extruder = 0;
  73. // Extruder offsets
  74. #if HOTENDS > 1
  75. float hotend_offset[XYZ][HOTENDS]; // Initialized by settings.load()
  76. #endif
  77. // The feedrate for the current move, often used as the default if
  78. // no other feedrate is specified. Overridden for special moves.
  79. // Set by the last G0 through G5 command's "F" parameter.
  80. // Functions that override this for custom moves *must always* restore it!
  81. float feedrate_mm_s = MMM_TO_MMS(1500.0);
  82. int16_t feedrate_percentage = 100;
  83. // Homing feedrate is const progmem - compare to constexpr in the header
  84. const float homing_feedrate_mm_s[4] PROGMEM = {
  85. #if ENABLED(DELTA)
  86. MMM_TO_MMS(HOMING_FEEDRATE_Z), MMM_TO_MMS(HOMING_FEEDRATE_Z),
  87. #else
  88. MMM_TO_MMS(HOMING_FEEDRATE_XY), MMM_TO_MMS(HOMING_FEEDRATE_XY),
  89. #endif
  90. MMM_TO_MMS(HOMING_FEEDRATE_Z), 0
  91. };
  92. // Cartesian conversion result goes here:
  93. float cartes[XYZ];
  94. // Until kinematics.cpp is created, create this here
  95. #if IS_KINEMATIC
  96. float delta[ABC];
  97. #endif
  98. /**
  99. * The workspace can be offset by some commands, or
  100. * these offsets may be omitted to save on computation.
  101. */
  102. #if HAS_WORKSPACE_OFFSET
  103. #if HAS_POSITION_SHIFT
  104. // The distance that XYZ has been offset by G92. Reset by G28.
  105. float position_shift[XYZ] = { 0 };
  106. #endif
  107. #if HAS_HOME_OFFSET
  108. // This offset is added to the configured home position.
  109. // Set by M206, M428, or menu item. Saved to EEPROM.
  110. float home_offset[XYZ] = { 0 };
  111. #endif
  112. #if HAS_HOME_OFFSET && HAS_POSITION_SHIFT
  113. // The above two are combined to save on computes
  114. float workspace_offset[XYZ] = { 0 };
  115. #endif
  116. #endif
  117. #if OLDSCHOOL_ABL
  118. float xy_probe_feedrate_mm_s = MMM_TO_MMS(XY_PROBE_SPEED);
  119. #endif
  120. /**
  121. * Output the current position to serial
  122. */
  123. void report_current_position() {
  124. SERIAL_PROTOCOLPGM("X:");
  125. SERIAL_PROTOCOL(current_position[X_AXIS]);
  126. SERIAL_PROTOCOLPGM(" Y:");
  127. SERIAL_PROTOCOL(current_position[Y_AXIS]);
  128. SERIAL_PROTOCOLPGM(" Z:");
  129. SERIAL_PROTOCOL(current_position[Z_AXIS]);
  130. SERIAL_PROTOCOLPGM(" E:");
  131. SERIAL_PROTOCOL(current_position[E_AXIS]);
  132. stepper.report_positions();
  133. #if IS_SCARA
  134. scara_report_positions();
  135. #endif
  136. }
  137. /**
  138. * sync_plan_position
  139. *
  140. * Set the planner/stepper positions directly from current_position with
  141. * no kinematic translation. Used for homing axes and cartesian/core syncing.
  142. */
  143. void sync_plan_position() {
  144. #if ENABLED(DEBUG_LEVELING_FEATURE)
  145. if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position", current_position);
  146. #endif
  147. planner.set_position_mm(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  148. }
  149. void sync_plan_position_e() { planner.set_e_position_mm(current_position[E_AXIS]); }
  150. /**
  151. * Get the stepper positions in the cartes[] array.
  152. * Forward kinematics are applied for DELTA and SCARA.
  153. *
  154. * The result is in the current coordinate space with
  155. * leveling applied. The coordinates need to be run through
  156. * unapply_leveling to obtain the "ideal" coordinates
  157. * suitable for current_position, etc.
  158. */
  159. void get_cartesian_from_steppers() {
  160. #if ENABLED(DELTA)
  161. forward_kinematics_DELTA(
  162. stepper.get_axis_position_mm(A_AXIS),
  163. stepper.get_axis_position_mm(B_AXIS),
  164. stepper.get_axis_position_mm(C_AXIS)
  165. );
  166. #else
  167. #if IS_SCARA
  168. forward_kinematics_SCARA(
  169. stepper.get_axis_position_degrees(A_AXIS),
  170. stepper.get_axis_position_degrees(B_AXIS)
  171. );
  172. #else
  173. cartes[X_AXIS] = stepper.get_axis_position_mm(X_AXIS);
  174. cartes[Y_AXIS] = stepper.get_axis_position_mm(Y_AXIS);
  175. #endif
  176. cartes[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
  177. #endif
  178. }
  179. /**
  180. * Set the current_position for an axis based on
  181. * the stepper positions, removing any leveling that
  182. * may have been applied.
  183. */
  184. void set_current_from_steppers_for_axis(const AxisEnum axis) {
  185. get_cartesian_from_steppers();
  186. #if PLANNER_LEVELING
  187. planner.unapply_leveling(cartes);
  188. #endif
  189. if (axis == ALL_AXES)
  190. COPY(current_position, cartes);
  191. else
  192. current_position[axis] = cartes[axis];
  193. }
  194. /**
  195. * Move the planner to the current position from wherever it last moved
  196. * (or from wherever it has been told it is located).
  197. */
  198. void line_to_current_position() {
  199. planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate_mm_s, active_extruder);
  200. }
  201. /**
  202. * Move the planner to the position stored in the destination array, which is
  203. * used by G0/G1/G2/G3/G5 and many other functions to set a destination.
  204. */
  205. void buffer_line_to_destination(const float fr_mm_s) {
  206. planner.buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], fr_mm_s, active_extruder);
  207. }
  208. #if IS_KINEMATIC
  209. void sync_plan_position_kinematic() {
  210. #if ENABLED(DEBUG_LEVELING_FEATURE)
  211. if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position_kinematic", current_position);
  212. #endif
  213. planner.set_position_mm_kinematic(current_position);
  214. }
  215. /**
  216. * Calculate delta, start a line, and set current_position to destination
  217. */
  218. void prepare_uninterpolated_move_to_destination(const float fr_mm_s/*=0.0*/) {
  219. #if ENABLED(DEBUG_LEVELING_FEATURE)
  220. if (DEBUGGING(LEVELING)) DEBUG_POS("prepare_uninterpolated_move_to_destination", destination);
  221. #endif
  222. gcode.refresh_cmd_timeout();
  223. #if UBL_DELTA
  224. // ubl segmented line will do z-only moves in single segment
  225. ubl.prepare_segmented_line_to(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s));
  226. #else
  227. if ( current_position[X_AXIS] == destination[X_AXIS]
  228. && current_position[Y_AXIS] == destination[Y_AXIS]
  229. && current_position[Z_AXIS] == destination[Z_AXIS]
  230. && current_position[E_AXIS] == destination[E_AXIS]
  231. ) return;
  232. planner.buffer_line_kinematic(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s), active_extruder);
  233. #endif
  234. set_current_from_destination();
  235. }
  236. #endif // IS_KINEMATIC
  237. /**
  238. * Plan a move to (X, Y, Z) and set the current_position
  239. * The final current_position may not be the one that was requested
  240. */
  241. void do_blocking_move_to(const float &rx, const float &ry, const float &rz, const float &fr_mm_s/*=0.0*/) {
  242. const float old_feedrate_mm_s = feedrate_mm_s;
  243. #if ENABLED(DEBUG_LEVELING_FEATURE)
  244. if (DEBUGGING(LEVELING)) print_xyz(PSTR(">>> do_blocking_move_to"), NULL, rx, ry, rz);
  245. #endif
  246. #if ENABLED(DELTA)
  247. if (!position_is_reachable(rx, ry)) return;
  248. feedrate_mm_s = fr_mm_s ? fr_mm_s : XY_PROBE_FEEDRATE_MM_S;
  249. set_destination_from_current(); // sync destination at the start
  250. #if ENABLED(DEBUG_LEVELING_FEATURE)
  251. if (DEBUGGING(LEVELING)) DEBUG_POS("set_destination_from_current", destination);
  252. #endif
  253. // when in the danger zone
  254. if (current_position[Z_AXIS] > delta_clip_start_height) {
  255. if (rz > delta_clip_start_height) { // staying in the danger zone
  256. destination[X_AXIS] = rx; // move directly (uninterpolated)
  257. destination[Y_AXIS] = ry;
  258. destination[Z_AXIS] = rz;
  259. prepare_uninterpolated_move_to_destination(); // set_current_from_destination()
  260. #if ENABLED(DEBUG_LEVELING_FEATURE)
  261. if (DEBUGGING(LEVELING)) DEBUG_POS("danger zone move", current_position);
  262. #endif
  263. return;
  264. }
  265. else {
  266. destination[Z_AXIS] = delta_clip_start_height;
  267. prepare_uninterpolated_move_to_destination(); // set_current_from_destination()
  268. #if ENABLED(DEBUG_LEVELING_FEATURE)
  269. if (DEBUGGING(LEVELING)) DEBUG_POS("zone border move", current_position);
  270. #endif
  271. }
  272. }
  273. if (rz > current_position[Z_AXIS]) { // raising?
  274. destination[Z_AXIS] = rz;
  275. prepare_uninterpolated_move_to_destination(); // set_current_from_destination()
  276. #if ENABLED(DEBUG_LEVELING_FEATURE)
  277. if (DEBUGGING(LEVELING)) DEBUG_POS("z raise move", current_position);
  278. #endif
  279. }
  280. destination[X_AXIS] = rx;
  281. destination[Y_AXIS] = ry;
  282. prepare_move_to_destination(); // set_current_from_destination()
  283. #if ENABLED(DEBUG_LEVELING_FEATURE)
  284. if (DEBUGGING(LEVELING)) DEBUG_POS("xy move", current_position);
  285. #endif
  286. if (rz < current_position[Z_AXIS]) { // lowering?
  287. destination[Z_AXIS] = rz;
  288. prepare_uninterpolated_move_to_destination(); // set_current_from_destination()
  289. #if ENABLED(DEBUG_LEVELING_FEATURE)
  290. if (DEBUGGING(LEVELING)) DEBUG_POS("z lower move", current_position);
  291. #endif
  292. }
  293. #elif IS_SCARA
  294. if (!position_is_reachable(rx, ry)) return;
  295. set_destination_from_current();
  296. // If Z needs to raise, do it before moving XY
  297. if (destination[Z_AXIS] < rz) {
  298. destination[Z_AXIS] = rz;
  299. prepare_uninterpolated_move_to_destination(fr_mm_s ? fr_mm_s : homing_feedrate(Z_AXIS));
  300. }
  301. destination[X_AXIS] = rx;
  302. destination[Y_AXIS] = ry;
  303. prepare_uninterpolated_move_to_destination(fr_mm_s ? fr_mm_s : XY_PROBE_FEEDRATE_MM_S);
  304. // If Z needs to lower, do it after moving XY
  305. if (destination[Z_AXIS] > rz) {
  306. destination[Z_AXIS] = rz;
  307. prepare_uninterpolated_move_to_destination(fr_mm_s ? fr_mm_s : homing_feedrate(Z_AXIS));
  308. }
  309. #else
  310. // If Z needs to raise, do it before moving XY
  311. if (current_position[Z_AXIS] < rz) {
  312. feedrate_mm_s = fr_mm_s ? fr_mm_s : homing_feedrate(Z_AXIS);
  313. current_position[Z_AXIS] = rz;
  314. line_to_current_position();
  315. }
  316. feedrate_mm_s = fr_mm_s ? fr_mm_s : XY_PROBE_FEEDRATE_MM_S;
  317. current_position[X_AXIS] = rx;
  318. current_position[Y_AXIS] = ry;
  319. line_to_current_position();
  320. // If Z needs to lower, do it after moving XY
  321. if (current_position[Z_AXIS] > rz) {
  322. feedrate_mm_s = fr_mm_s ? fr_mm_s : homing_feedrate(Z_AXIS);
  323. current_position[Z_AXIS] = rz;
  324. line_to_current_position();
  325. }
  326. #endif
  327. stepper.synchronize();
  328. feedrate_mm_s = old_feedrate_mm_s;
  329. #if ENABLED(DEBUG_LEVELING_FEATURE)
  330. if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("<<< do_blocking_move_to");
  331. #endif
  332. }
  333. void do_blocking_move_to_x(const float &rx, const float &fr_mm_s/*=0.0*/) {
  334. do_blocking_move_to(rx, current_position[Y_AXIS], current_position[Z_AXIS], fr_mm_s);
  335. }
  336. void do_blocking_move_to_z(const float &rz, const float &fr_mm_s/*=0.0*/) {
  337. do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], rz, fr_mm_s);
  338. }
  339. void do_blocking_move_to_xy(const float &rx, const float &ry, const float &fr_mm_s/*=0.0*/) {
  340. do_blocking_move_to(rx, ry, current_position[Z_AXIS], fr_mm_s);
  341. }
  342. //
  343. // Prepare to do endstop or probe moves
  344. // with custom feedrates.
  345. //
  346. // - Save current feedrates
  347. // - Reset the rate multiplier
  348. // - Reset the command timeout
  349. // - Enable the endstops (for endstop moves)
  350. //
  351. void bracket_probe_move(const bool before) {
  352. static float saved_feedrate_mm_s;
  353. static int16_t saved_feedrate_percentage;
  354. #if ENABLED(DEBUG_LEVELING_FEATURE)
  355. if (DEBUGGING(LEVELING)) DEBUG_POS("bracket_probe_move", current_position);
  356. #endif
  357. if (before) {
  358. saved_feedrate_mm_s = feedrate_mm_s;
  359. saved_feedrate_percentage = feedrate_percentage;
  360. feedrate_percentage = 100;
  361. gcode.refresh_cmd_timeout();
  362. }
  363. else {
  364. feedrate_mm_s = saved_feedrate_mm_s;
  365. feedrate_percentage = saved_feedrate_percentage;
  366. gcode.refresh_cmd_timeout();
  367. }
  368. }
  369. void setup_for_endstop_or_probe_move() { bracket_probe_move(true); }
  370. void clean_up_after_endstop_or_probe_move() { bracket_probe_move(false); }
  371. // Software Endstops are based on the configured limits.
  372. float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
  373. soft_endstop_max[XYZ] = { X_MAX_BED, Y_MAX_BED, Z_MAX_POS };
  374. #if HAS_SOFTWARE_ENDSTOPS
  375. // Software Endstops are based on the configured limits.
  376. bool soft_endstops_enabled = true;
  377. #if IS_KINEMATIC
  378. float soft_endstop_radius, soft_endstop_radius_2;
  379. #endif
  380. /**
  381. * Constrain the given coordinates to the software endstops.
  382. *
  383. * For DELTA/SCARA the XY constraint is based on the smallest
  384. * radius within the set software endstops.
  385. */
  386. void clamp_to_software_endstops(float target[XYZ]) {
  387. if (!soft_endstops_enabled) return;
  388. #if IS_KINEMATIC
  389. const float dist_2 = HYPOT2(target[X_AXIS], target[Y_AXIS]);
  390. if (dist_2 > soft_endstop_radius_2) {
  391. const float ratio = soft_endstop_radius / SQRT(dist_2); // 200 / 300 = 0.66
  392. target[X_AXIS] *= ratio;
  393. target[Y_AXIS] *= ratio;
  394. }
  395. #else
  396. #if ENABLED(MIN_SOFTWARE_ENDSTOP_X)
  397. NOLESS(target[X_AXIS], soft_endstop_min[X_AXIS]);
  398. #endif
  399. #if ENABLED(MIN_SOFTWARE_ENDSTOP_Y)
  400. NOLESS(target[Y_AXIS], soft_endstop_min[Y_AXIS]);
  401. #endif
  402. #if ENABLED(MAX_SOFTWARE_ENDSTOP_X)
  403. NOMORE(target[X_AXIS], soft_endstop_max[X_AXIS]);
  404. #endif
  405. #if ENABLED(MAX_SOFTWARE_ENDSTOP_Y)
  406. NOMORE(target[Y_AXIS], soft_endstop_max[Y_AXIS]);
  407. #endif
  408. #endif
  409. #if ENABLED(MIN_SOFTWARE_ENDSTOP_Z)
  410. NOLESS(target[Z_AXIS], soft_endstop_min[Z_AXIS]);
  411. #endif
  412. #if ENABLED(MAX_SOFTWARE_ENDSTOP_Z)
  413. NOMORE(target[Z_AXIS], soft_endstop_max[Z_AXIS]);
  414. #endif
  415. }
  416. #endif
  417. #if !UBL_DELTA
  418. #if IS_KINEMATIC
  419. #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
  420. #if ENABLED(DELTA)
  421. #define ADJUST_DELTA(V) \
  422. if (planner.leveling_active) { \
  423. const float zadj = bilinear_z_offset(V); \
  424. delta[A_AXIS] += zadj; \
  425. delta[B_AXIS] += zadj; \
  426. delta[C_AXIS] += zadj; \
  427. }
  428. #else
  429. #define ADJUST_DELTA(V) if (planner.leveling_active) { delta[Z_AXIS] += bilinear_z_offset(V); }
  430. #endif
  431. #else
  432. #define ADJUST_DELTA(V) NOOP
  433. #endif
  434. /**
  435. * Prepare a linear move in a DELTA or SCARA setup.
  436. *
  437. * This calls planner.buffer_line several times, adding
  438. * small incremental moves for DELTA or SCARA.
  439. *
  440. * For Unified Bed Leveling (Delta or Segmented Cartesian)
  441. * the ubl.prepare_segmented_line_to method replaces this.
  442. */
  443. inline bool prepare_kinematic_move_to(float rtarget[XYZE]) {
  444. // Get the top feedrate of the move in the XY plane
  445. const float _feedrate_mm_s = MMS_SCALED(feedrate_mm_s);
  446. // If the move is only in Z/E don't split up the move
  447. if (rtarget[X_AXIS] == current_position[X_AXIS] && rtarget[Y_AXIS] == current_position[Y_AXIS]) {
  448. planner.buffer_line_kinematic(rtarget, _feedrate_mm_s, active_extruder);
  449. return false;
  450. }
  451. // Fail if attempting move outside printable radius
  452. if (!position_is_reachable(rtarget[X_AXIS], rtarget[Y_AXIS])) return true;
  453. // Get the cartesian distances moved in XYZE
  454. const float difference[XYZE] = {
  455. rtarget[X_AXIS] - current_position[X_AXIS],
  456. rtarget[Y_AXIS] - current_position[Y_AXIS],
  457. rtarget[Z_AXIS] - current_position[Z_AXIS],
  458. rtarget[E_AXIS] - current_position[E_AXIS]
  459. };
  460. // Get the linear distance in XYZ
  461. float cartesian_mm = SQRT(sq(difference[X_AXIS]) + sq(difference[Y_AXIS]) + sq(difference[Z_AXIS]));
  462. // If the move is very short, check the E move distance
  463. if (UNEAR_ZERO(cartesian_mm)) cartesian_mm = FABS(difference[E_AXIS]);
  464. // No E move either? Game over.
  465. if (UNEAR_ZERO(cartesian_mm)) return true;
  466. // Minimum number of seconds to move the given distance
  467. const float seconds = cartesian_mm / _feedrate_mm_s;
  468. // The number of segments-per-second times the duration
  469. // gives the number of segments
  470. uint16_t segments = delta_segments_per_second * seconds;
  471. // For SCARA minimum segment size is 0.25mm
  472. #if IS_SCARA
  473. NOMORE(segments, cartesian_mm * 4);
  474. #endif
  475. // At least one segment is required
  476. NOLESS(segments, 1);
  477. // The approximate length of each segment
  478. const float inv_segments = 1.0 / float(segments),
  479. segment_distance[XYZE] = {
  480. difference[X_AXIS] * inv_segments,
  481. difference[Y_AXIS] * inv_segments,
  482. difference[Z_AXIS] * inv_segments,
  483. difference[E_AXIS] * inv_segments
  484. };
  485. // SERIAL_ECHOPAIR("mm=", cartesian_mm);
  486. // SERIAL_ECHOPAIR(" seconds=", seconds);
  487. // SERIAL_ECHOLNPAIR(" segments=", segments);
  488. #if IS_SCARA && ENABLED(SCARA_FEEDRATE_SCALING)
  489. // SCARA needs to scale the feed rate from mm/s to degrees/s
  490. const float inv_segment_length = min(10.0, float(segments) / cartesian_mm), // 1/mm/segs
  491. feed_factor = inv_segment_length * _feedrate_mm_s;
  492. float oldA = stepper.get_axis_position_degrees(A_AXIS),
  493. oldB = stepper.get_axis_position_degrees(B_AXIS);
  494. #endif
  495. // Get the current position as starting point
  496. float raw[XYZE];
  497. COPY(raw, current_position);
  498. // Drop one segment so the last move is to the exact target.
  499. // If there's only 1 segment, loops will be skipped entirely.
  500. --segments;
  501. // Calculate and execute the segments
  502. for (uint16_t s = segments + 1; --s;) {
  503. LOOP_XYZE(i) raw[i] += segment_distance[i];
  504. #if ENABLED(DELTA)
  505. DELTA_RAW_IK(); // Delta can inline its kinematics
  506. #else
  507. inverse_kinematics(raw);
  508. #endif
  509. ADJUST_DELTA(raw); // Adjust Z if bed leveling is enabled
  510. #if IS_SCARA && ENABLED(SCARA_FEEDRATE_SCALING)
  511. // For SCARA scale the feed rate from mm/s to degrees/s
  512. // Use ratio between the length of the move and the larger angle change
  513. const float adiff = abs(delta[A_AXIS] - oldA),
  514. bdiff = abs(delta[B_AXIS] - oldB);
  515. planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], max(adiff, bdiff) * feed_factor, active_extruder);
  516. oldA = delta[A_AXIS];
  517. oldB = delta[B_AXIS];
  518. #else
  519. planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], _feedrate_mm_s, active_extruder);
  520. #endif
  521. }
  522. // Since segment_distance is only approximate,
  523. // the final move must be to the exact destination.
  524. #if IS_SCARA && ENABLED(SCARA_FEEDRATE_SCALING)
  525. // For SCARA scale the feed rate from mm/s to degrees/s
  526. // With segments > 1 length is 1 segment, otherwise total length
  527. inverse_kinematics(rtarget);
  528. ADJUST_DELTA(rtarget);
  529. const float adiff = abs(delta[A_AXIS] - oldA),
  530. bdiff = abs(delta[B_AXIS] - oldB);
  531. planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], max(adiff, bdiff) * feed_factor, active_extruder);
  532. #else
  533. planner.buffer_line_kinematic(rtarget, _feedrate_mm_s, active_extruder);
  534. #endif
  535. return false;
  536. }
  537. #else // !IS_KINEMATIC
  538. /**
  539. * Prepare a linear move in a Cartesian setup.
  540. *
  541. * When a mesh-based leveling system is active, moves are segmented
  542. * according to the configuration of the leveling system.
  543. *
  544. * Returns true if current_position[] was set to destination[]
  545. */
  546. inline bool prepare_move_to_destination_cartesian() {
  547. #if HAS_MESH
  548. if (planner.leveling_active) {
  549. #if ENABLED(AUTO_BED_LEVELING_UBL)
  550. ubl.line_to_destination_cartesian(MMS_SCALED(feedrate_mm_s), active_extruder); // UBL's motion routine needs to know about
  551. return true; // all moves, including Z-only moves.
  552. #else
  553. /**
  554. * For MBL and ABL-BILINEAR only segment moves when X or Y are involved.
  555. * Otherwise fall through to do a direct single move.
  556. */
  557. if (current_position[X_AXIS] != destination[X_AXIS] || current_position[Y_AXIS] != destination[Y_AXIS]) {
  558. #if ENABLED(MESH_BED_LEVELING)
  559. mesh_line_to_destination(MMS_SCALED(feedrate_mm_s));
  560. #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
  561. bilinear_line_to_destination(MMS_SCALED(feedrate_mm_s));
  562. #endif
  563. return true;
  564. }
  565. #endif
  566. }
  567. #endif // HAS_MESH
  568. buffer_line_to_destination(MMS_SCALED(feedrate_mm_s));
  569. return false;
  570. }
  571. #endif // !IS_KINEMATIC
  572. #endif // !UBL_DELTA
  573. #if ENABLED(DUAL_X_CARRIAGE) || ENABLED(DUAL_NOZZLE_DUPLICATION_MODE)
  574. bool extruder_duplication_enabled = false; // Used in Dual X mode 2
  575. #endif
  576. #if ENABLED(DUAL_X_CARRIAGE)
  577. DualXMode dual_x_carriage_mode = DEFAULT_DUAL_X_CARRIAGE_MODE;
  578. float inactive_extruder_x_pos = X2_MAX_POS, // used in mode 0 & 1
  579. raised_parked_position[XYZE], // used in mode 1
  580. duplicate_extruder_x_offset = DEFAULT_DUPLICATION_X_OFFSET; // used in mode 2
  581. bool active_extruder_parked = false; // used in mode 1 & 2
  582. millis_t delayed_move_time = 0; // used in mode 1
  583. int16_t duplicate_extruder_temp_offset = 0; // used in mode 2
  584. float x_home_pos(const int extruder) {
  585. if (extruder == 0)
  586. return base_home_pos(X_AXIS);
  587. else
  588. /**
  589. * In dual carriage mode the extruder offset provides an override of the
  590. * second X-carriage position when homed - otherwise X2_HOME_POS is used.
  591. * This allows soft recalibration of the second extruder home position
  592. * without firmware reflash (through the M218 command).
  593. */
  594. return hotend_offset[X_AXIS][1] > 0 ? hotend_offset[X_AXIS][1] : X2_HOME_POS;
  595. }
  596. /**
  597. * Prepare a linear move in a dual X axis setup
  598. *
  599. * Return true if current_position[] was set to destination[]
  600. */
  601. inline bool prepare_move_to_destination_dualx() {
  602. if (active_extruder_parked) {
  603. switch (dual_x_carriage_mode) {
  604. case DXC_FULL_CONTROL_MODE:
  605. break;
  606. case DXC_AUTO_PARK_MODE:
  607. if (current_position[E_AXIS] == destination[E_AXIS]) {
  608. // This is a travel move (with no extrusion)
  609. // Skip it, but keep track of the current position
  610. // (so it can be used as the start of the next non-travel move)
  611. if (delayed_move_time != 0xFFFFFFFFUL) {
  612. set_current_from_destination();
  613. NOLESS(raised_parked_position[Z_AXIS], destination[Z_AXIS]);
  614. delayed_move_time = millis();
  615. return true;
  616. }
  617. }
  618. // unpark extruder: 1) raise, 2) move into starting XY position, 3) lower
  619. for (uint8_t i = 0; i < 3; i++)
  620. planner.buffer_line(
  621. i == 0 ? raised_parked_position[X_AXIS] : current_position[X_AXIS],
  622. i == 0 ? raised_parked_position[Y_AXIS] : current_position[Y_AXIS],
  623. i == 2 ? current_position[Z_AXIS] : raised_parked_position[Z_AXIS],
  624. current_position[E_AXIS],
  625. i == 1 ? PLANNER_XY_FEEDRATE() : planner.max_feedrate_mm_s[Z_AXIS],
  626. active_extruder
  627. );
  628. delayed_move_time = 0;
  629. active_extruder_parked = false;
  630. #if ENABLED(DEBUG_LEVELING_FEATURE)
  631. if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("Clear active_extruder_parked");
  632. #endif
  633. break;
  634. case DXC_DUPLICATION_MODE:
  635. if (active_extruder == 0) {
  636. #if ENABLED(DEBUG_LEVELING_FEATURE)
  637. if (DEBUGGING(LEVELING)) {
  638. SERIAL_ECHOPAIR("Set planner X", inactive_extruder_x_pos);
  639. SERIAL_ECHOLNPAIR(" ... Line to X", current_position[X_AXIS] + duplicate_extruder_x_offset);
  640. }
  641. #endif
  642. // move duplicate extruder into correct duplication position.
  643. planner.set_position_mm(
  644. inactive_extruder_x_pos,
  645. current_position[Y_AXIS],
  646. current_position[Z_AXIS],
  647. current_position[E_AXIS]
  648. );
  649. planner.buffer_line(
  650. current_position[X_AXIS] + duplicate_extruder_x_offset,
  651. current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS],
  652. planner.max_feedrate_mm_s[X_AXIS], 1
  653. );
  654. SYNC_PLAN_POSITION_KINEMATIC();
  655. stepper.synchronize();
  656. extruder_duplication_enabled = true;
  657. active_extruder_parked = false;
  658. #if ENABLED(DEBUG_LEVELING_FEATURE)
  659. if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("Set extruder_duplication_enabled\nClear active_extruder_parked");
  660. #endif
  661. }
  662. else {
  663. #if ENABLED(DEBUG_LEVELING_FEATURE)
  664. if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("Active extruder not 0");
  665. #endif
  666. }
  667. break;
  668. }
  669. }
  670. return prepare_move_to_destination_cartesian();
  671. }
  672. #endif // DUAL_X_CARRIAGE
  673. /**
  674. * Prepare a single move and get ready for the next one
  675. *
  676. * This may result in several calls to planner.buffer_line to
  677. * do smaller moves for DELTA, SCARA, mesh moves, etc.
  678. */
  679. void prepare_move_to_destination() {
  680. clamp_to_software_endstops(destination);
  681. gcode.refresh_cmd_timeout();
  682. #if ENABLED(PREVENT_COLD_EXTRUSION) || ENABLED(PREVENT_LENGTHY_EXTRUDE)
  683. if (!DEBUGGING(DRYRUN)) {
  684. if (destination[E_AXIS] != current_position[E_AXIS]) {
  685. #if ENABLED(PREVENT_COLD_EXTRUSION)
  686. if (thermalManager.tooColdToExtrude(active_extruder)) {
  687. current_position[E_AXIS] = destination[E_AXIS]; // Behave as if the move really took place, but ignore E part
  688. SERIAL_ECHO_START();
  689. SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
  690. }
  691. #endif // PREVENT_COLD_EXTRUSION
  692. #if ENABLED(PREVENT_LENGTHY_EXTRUDE)
  693. if (FABS(destination[E_AXIS] - current_position[E_AXIS]) > (EXTRUDE_MAXLENGTH) / planner.volumetric_multiplier[active_extruder]) {
  694. current_position[E_AXIS] = destination[E_AXIS]; // Behave as if the move really took place, but ignore E part
  695. SERIAL_ECHO_START();
  696. SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
  697. }
  698. #endif // PREVENT_LENGTHY_EXTRUDE
  699. }
  700. }
  701. #endif // PREVENT_COLD_EXTRUSION || PREVENT_LENGTHY_EXTRUDE
  702. if (
  703. #if UBL_DELTA // Also works for CARTESIAN (smaller segments follow mesh more closely)
  704. ubl.prepare_segmented_line_to(destination, MMS_SCALED(feedrate_mm_s))
  705. #elif IS_KINEMATIC
  706. prepare_kinematic_move_to(destination)
  707. #elif ENABLED(DUAL_X_CARRIAGE)
  708. prepare_move_to_destination_dualx()
  709. #else
  710. prepare_move_to_destination_cartesian()
  711. #endif
  712. ) return;
  713. set_current_from_destination();
  714. }
  715. #if HAS_AXIS_UNHOMED_ERR
  716. bool axis_unhomed_error(const bool x/*=true*/, const bool y/*=true*/, const bool z/*=true*/) {
  717. #if ENABLED(HOME_AFTER_DEACTIVATE)
  718. const bool xx = x && !axis_known_position[X_AXIS],
  719. yy = y && !axis_known_position[Y_AXIS],
  720. zz = z && !axis_known_position[Z_AXIS];
  721. #else
  722. const bool xx = x && !axis_homed[X_AXIS],
  723. yy = y && !axis_homed[Y_AXIS],
  724. zz = z && !axis_homed[Z_AXIS];
  725. #endif
  726. if (xx || yy || zz) {
  727. SERIAL_ECHO_START();
  728. SERIAL_ECHOPGM(MSG_HOME " ");
  729. if (xx) SERIAL_ECHOPGM(MSG_X);
  730. if (yy) SERIAL_ECHOPGM(MSG_Y);
  731. if (zz) SERIAL_ECHOPGM(MSG_Z);
  732. SERIAL_ECHOLNPGM(" " MSG_FIRST);
  733. #if ENABLED(ULTRA_LCD)
  734. lcd_status_printf_P(0, PSTR(MSG_HOME " %s%s%s " MSG_FIRST), xx ? MSG_X : "", yy ? MSG_Y : "", zz ? MSG_Z : "");
  735. #endif
  736. return true;
  737. }
  738. return false;
  739. }
  740. #endif // HAS_AXIS_UNHOMED_ERR
  741. /**
  742. * The homing feedrate may vary
  743. */
  744. inline float get_homing_bump_feedrate(const AxisEnum axis) {
  745. static const uint8_t homing_bump_divisor[] PROGMEM = HOMING_BUMP_DIVISOR;
  746. uint8_t hbd = pgm_read_byte(&homing_bump_divisor[axis]);
  747. if (hbd < 1) {
  748. hbd = 10;
  749. SERIAL_ECHO_START();
  750. SERIAL_ECHOLNPGM("Warning: Homing Bump Divisor < 1");
  751. }
  752. return homing_feedrate(axis) / hbd;
  753. }
  754. /**
  755. * Home an individual linear axis
  756. */
  757. static void do_homing_move(const AxisEnum axis, const float distance, const float fr_mm_s=0.0) {
  758. #if ENABLED(DEBUG_LEVELING_FEATURE)
  759. if (DEBUGGING(LEVELING)) {
  760. SERIAL_ECHOPAIR(">>> do_homing_move(", axis_codes[axis]);
  761. SERIAL_ECHOPAIR(", ", distance);
  762. SERIAL_ECHOPAIR(", ", fr_mm_s);
  763. SERIAL_CHAR(')');
  764. SERIAL_EOL();
  765. }
  766. #endif
  767. #if HOMING_Z_WITH_PROBE && ENABLED(BLTOUCH)
  768. const bool deploy_bltouch = (axis == Z_AXIS && distance < 0);
  769. if (deploy_bltouch) set_bltouch_deployed(true);
  770. #endif
  771. #if QUIET_PROBING
  772. if (axis == Z_AXIS) probing_pause(true);
  773. #endif
  774. // Tell the planner we're at Z=0
  775. current_position[axis] = 0;
  776. #if IS_SCARA
  777. SYNC_PLAN_POSITION_KINEMATIC();
  778. current_position[axis] = distance;
  779. inverse_kinematics(current_position);
  780. planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], current_position[E_AXIS], fr_mm_s ? fr_mm_s : homing_feedrate(axis), active_extruder);
  781. #else
  782. sync_plan_position();
  783. current_position[axis] = distance;
  784. planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], fr_mm_s ? fr_mm_s : homing_feedrate(axis), active_extruder);
  785. #endif
  786. stepper.synchronize();
  787. #if QUIET_PROBING
  788. if (axis == Z_AXIS) probing_pause(false);
  789. #endif
  790. #if HOMING_Z_WITH_PROBE && ENABLED(BLTOUCH)
  791. if (deploy_bltouch) set_bltouch_deployed(false);
  792. #endif
  793. endstops.hit_on_purpose();
  794. #if ENABLED(DEBUG_LEVELING_FEATURE)
  795. if (DEBUGGING(LEVELING)) {
  796. SERIAL_ECHOPAIR("<<< do_homing_move(", axis_codes[axis]);
  797. SERIAL_CHAR(')');
  798. SERIAL_EOL();
  799. }
  800. #endif
  801. }
  802. /**
  803. * Set an axis' current position to its home position (after homing).
  804. *
  805. * For Core and Cartesian robots this applies one-to-one when an
  806. * individual axis has been homed.
  807. *
  808. * DELTA should wait until all homing is done before setting the XYZ
  809. * current_position to home, because homing is a single operation.
  810. * In the case where the axis positions are already known and previously
  811. * homed, DELTA could home to X or Y individually by moving either one
  812. * to the center. However, homing Z always homes XY and Z.
  813. *
  814. * SCARA should wait until all XY homing is done before setting the XY
  815. * current_position to home, because neither X nor Y is at home until
  816. * both are at home. Z can however be homed individually.
  817. *
  818. * Callers must sync the planner position after calling this!
  819. */
  820. void set_axis_is_at_home(const AxisEnum axis) {
  821. #if ENABLED(DEBUG_LEVELING_FEATURE)
  822. if (DEBUGGING(LEVELING)) {
  823. SERIAL_ECHOPAIR(">>> set_axis_is_at_home(", axis_codes[axis]);
  824. SERIAL_CHAR(')');
  825. SERIAL_EOL();
  826. }
  827. #endif
  828. axis_known_position[axis] = axis_homed[axis] = true;
  829. #if HAS_POSITION_SHIFT
  830. position_shift[axis] = 0;
  831. update_software_endstops(axis);
  832. #endif
  833. #if ENABLED(DUAL_X_CARRIAGE)
  834. if (axis == X_AXIS && (active_extruder == 1 || dual_x_carriage_mode == DXC_DUPLICATION_MODE)) {
  835. current_position[X_AXIS] = x_home_pos(active_extruder);
  836. return;
  837. }
  838. #endif
  839. #if ENABLED(MORGAN_SCARA)
  840. scara_set_axis_is_at_home(axis);
  841. #elif ENABLED(DELTA)
  842. current_position[axis] = (axis == Z_AXIS ? delta_height : base_home_pos(axis));
  843. #else
  844. current_position[axis] = base_home_pos(axis);
  845. #endif
  846. /**
  847. * Z Probe Z Homing? Account for the probe's Z offset.
  848. */
  849. #if HAS_BED_PROBE && Z_HOME_DIR < 0
  850. if (axis == Z_AXIS) {
  851. #if HOMING_Z_WITH_PROBE
  852. current_position[Z_AXIS] -= zprobe_zoffset;
  853. #if ENABLED(DEBUG_LEVELING_FEATURE)
  854. if (DEBUGGING(LEVELING)) {
  855. SERIAL_ECHOLNPGM("*** Z HOMED WITH PROBE (Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN) ***");
  856. SERIAL_ECHOLNPAIR("> zprobe_zoffset = ", zprobe_zoffset);
  857. }
  858. #endif
  859. #elif ENABLED(DEBUG_LEVELING_FEATURE)
  860. if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("*** Z HOMED TO ENDSTOP (Z_MIN_PROBE_ENDSTOP) ***");
  861. #endif
  862. }
  863. #endif
  864. #if ENABLED(DEBUG_LEVELING_FEATURE)
  865. if (DEBUGGING(LEVELING)) {
  866. #if HAS_HOME_OFFSET
  867. SERIAL_ECHOPAIR("> home_offset[", axis_codes[axis]);
  868. SERIAL_ECHOLNPAIR("] = ", home_offset[axis]);
  869. #endif
  870. DEBUG_POS("", current_position);
  871. SERIAL_ECHOPAIR("<<< set_axis_is_at_home(", axis_codes[axis]);
  872. SERIAL_CHAR(')');
  873. SERIAL_EOL();
  874. }
  875. #endif
  876. #if ENABLED(I2C_POSITION_ENCODERS)
  877. I2CPEM.homed(axis);
  878. #endif
  879. }
  880. /**
  881. * Home an individual "raw axis" to its endstop.
  882. * This applies to XYZ on Cartesian and Core robots, and
  883. * to the individual ABC steppers on DELTA and SCARA.
  884. *
  885. * At the end of the procedure the axis is marked as
  886. * homed and the current position of that axis is updated.
  887. * Kinematic robots should wait till all axes are homed
  888. * before updating the current position.
  889. */
  890. void homeaxis(const AxisEnum axis) {
  891. #if IS_SCARA
  892. // Only Z homing (with probe) is permitted
  893. if (axis != Z_AXIS) { BUZZ(100, 880); return; }
  894. #else
  895. #define CAN_HOME(A) \
  896. (axis == A##_AXIS && ((A##_MIN_PIN > -1 && A##_HOME_DIR < 0) || (A##_MAX_PIN > -1 && A##_HOME_DIR > 0)))
  897. if (!CAN_HOME(X) && !CAN_HOME(Y) && !CAN_HOME(Z)) return;
  898. #endif
  899. #if ENABLED(DEBUG_LEVELING_FEATURE)
  900. if (DEBUGGING(LEVELING)) {
  901. SERIAL_ECHOPAIR(">>> homeaxis(", axis_codes[axis]);
  902. SERIAL_CHAR(')');
  903. SERIAL_EOL();
  904. }
  905. #endif
  906. const int axis_home_dir =
  907. #if ENABLED(DUAL_X_CARRIAGE)
  908. (axis == X_AXIS) ? x_home_dir(active_extruder) :
  909. #endif
  910. home_dir(axis);
  911. // Homing Z towards the bed? Deploy the Z probe or endstop.
  912. #if HOMING_Z_WITH_PROBE
  913. if (axis == Z_AXIS && DEPLOY_PROBE()) return;
  914. #endif
  915. // Set flags for X, Y, Z motor locking
  916. #if ENABLED(X_DUAL_ENDSTOPS)
  917. if (axis == X_AXIS) stepper.set_homing_flag_x(true);
  918. #endif
  919. #if ENABLED(Y_DUAL_ENDSTOPS)
  920. if (axis == Y_AXIS) stepper.set_homing_flag_y(true);
  921. #endif
  922. #if ENABLED(Z_DUAL_ENDSTOPS)
  923. if (axis == Z_AXIS) stepper.set_homing_flag_z(true);
  924. #endif
  925. // Disable stealthChop if used. Enable diag1 pin on driver.
  926. #if ENABLED(SENSORLESS_HOMING)
  927. #if ENABLED(X_IS_TMC2130)
  928. if (axis == X_AXIS) tmc2130_sensorless_homing(stepperX);
  929. #endif
  930. #if ENABLED(Y_IS_TMC2130)
  931. if (axis == Y_AXIS) tmc2130_sensorless_homing(stepperY);
  932. #endif
  933. #endif
  934. // Fast move towards endstop until triggered
  935. #if ENABLED(DEBUG_LEVELING_FEATURE)
  936. if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("Home 1 Fast:");
  937. #endif
  938. do_homing_move(axis, 1.5 * max_length(axis) * axis_home_dir);
  939. // When homing Z with probe respect probe clearance
  940. const float bump = axis_home_dir * (
  941. #if HOMING_Z_WITH_PROBE
  942. (axis == Z_AXIS) ? max(Z_CLEARANCE_BETWEEN_PROBES, home_bump_mm(Z_AXIS)) :
  943. #endif
  944. home_bump_mm(axis)
  945. );
  946. // If a second homing move is configured...
  947. if (bump) {
  948. // Move away from the endstop by the axis HOME_BUMP_MM
  949. #if ENABLED(DEBUG_LEVELING_FEATURE)
  950. if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("Move Away:");
  951. #endif
  952. do_homing_move(axis, -bump);
  953. // Slow move towards endstop until triggered
  954. #if ENABLED(DEBUG_LEVELING_FEATURE)
  955. if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("Home 2 Slow:");
  956. #endif
  957. do_homing_move(axis, 2 * bump, get_homing_bump_feedrate(axis));
  958. }
  959. #if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS)
  960. const bool pos_dir = axis_home_dir > 0;
  961. #if ENABLED(X_DUAL_ENDSTOPS)
  962. if (axis == X_AXIS) {
  963. const bool lock_x1 = pos_dir ? (endstops.x_endstop_adj > 0) : (endstops.x_endstop_adj < 0);
  964. float adj = FABS(endstops.x_endstop_adj);
  965. if (pos_dir) adj = -adj;
  966. if (lock_x1) stepper.set_x_lock(true); else stepper.set_x2_lock(true);
  967. do_homing_move(axis, adj);
  968. if (lock_x1) stepper.set_x_lock(false); else stepper.set_x2_lock(false);
  969. stepper.set_homing_flag_x(false);
  970. }
  971. #endif
  972. #if ENABLED(Y_DUAL_ENDSTOPS)
  973. if (axis == Y_AXIS) {
  974. const bool lock_y1 = pos_dir ? (endstops.y_endstop_adj > 0) : (endstops.y_endstop_adj < 0);
  975. float adj = FABS(endstops.y_endstop_adj);
  976. if (pos_dir) adj = -adj;
  977. if (lock_y1) stepper.set_y_lock(true); else stepper.set_y2_lock(true);
  978. do_homing_move(axis, adj);
  979. if (lock_y1) stepper.set_y_lock(false); else stepper.set_y2_lock(false);
  980. stepper.set_homing_flag_y(false);
  981. }
  982. #endif
  983. #if ENABLED(Z_DUAL_ENDSTOPS)
  984. if (axis == Z_AXIS) {
  985. const bool lock_z1 = pos_dir ? (endstops.z_endstop_adj > 0) : (endstops.z_endstop_adj < 0);
  986. float adj = FABS(endstops.z_endstop_adj);
  987. if (pos_dir) adj = -adj;
  988. if (lock_z1) stepper.set_z_lock(true); else stepper.set_z2_lock(true);
  989. do_homing_move(axis, adj);
  990. if (lock_z1) stepper.set_z_lock(false); else stepper.set_z2_lock(false);
  991. stepper.set_homing_flag_z(false);
  992. }
  993. #endif
  994. #endif
  995. #if IS_SCARA
  996. set_axis_is_at_home(axis);
  997. SYNC_PLAN_POSITION_KINEMATIC();
  998. #elif ENABLED(DELTA)
  999. // Delta has already moved all three towers up in G28
  1000. // so here it re-homes each tower in turn.
  1001. // Delta homing treats the axes as normal linear axes.
  1002. // retrace by the amount specified in delta_endstop_adj + additional 0.1mm in order to have minimum steps
  1003. if (delta_endstop_adj[axis] * Z_HOME_DIR <= 0) {
  1004. #if ENABLED(DEBUG_LEVELING_FEATURE)
  1005. if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("delta_endstop_adj:");
  1006. #endif
  1007. do_homing_move(axis, delta_endstop_adj[axis] - 0.1 * Z_HOME_DIR);
  1008. }
  1009. #else
  1010. // For cartesian/core machines,
  1011. // set the axis to its home position
  1012. set_axis_is_at_home(axis);
  1013. sync_plan_position();
  1014. destination[axis] = current_position[axis];
  1015. #if ENABLED(DEBUG_LEVELING_FEATURE)
  1016. if (DEBUGGING(LEVELING)) DEBUG_POS("> AFTER set_axis_is_at_home", current_position);
  1017. #endif
  1018. #endif
  1019. // Re-enable stealthChop if used. Disable diag1 pin on driver.
  1020. #if ENABLED(SENSORLESS_HOMING)
  1021. #if ENABLED(X_IS_TMC2130)
  1022. if (axis == X_AXIS) tmc2130_sensorless_homing(stepperX, false);
  1023. #endif
  1024. #if ENABLED(Y_IS_TMC2130)
  1025. if (axis == Y_AXIS) tmc2130_sensorless_homing(stepperY, false);
  1026. #endif
  1027. #endif
  1028. // Put away the Z probe
  1029. #if HOMING_Z_WITH_PROBE
  1030. if (axis == Z_AXIS && STOW_PROBE()) return;
  1031. #endif
  1032. #if ENABLED(DEBUG_LEVELING_FEATURE)
  1033. if (DEBUGGING(LEVELING)) {
  1034. SERIAL_ECHOPAIR("<<< homeaxis(", axis_codes[axis]);
  1035. SERIAL_CHAR(')');
  1036. SERIAL_EOL();
  1037. }
  1038. #endif
  1039. } // homeaxis()
  1040. #if HAS_WORKSPACE_OFFSET || ENABLED(DUAL_X_CARRIAGE)
  1041. /**
  1042. * Software endstops can be used to monitor the open end of
  1043. * an axis that has a hardware endstop on the other end. Or
  1044. * they can prevent axes from moving past endstops and grinding.
  1045. *
  1046. * To keep doing their job as the coordinate system changes,
  1047. * the software endstop positions must be refreshed to remain
  1048. * at the same positions relative to the machine.
  1049. */
  1050. void update_software_endstops(const AxisEnum axis) {
  1051. const float offs = 0.0
  1052. #if HAS_HOME_OFFSET
  1053. + home_offset[axis]
  1054. #endif
  1055. #if HAS_POSITION_SHIFT
  1056. + position_shift[axis]
  1057. #endif
  1058. ;
  1059. #if HAS_HOME_OFFSET && HAS_POSITION_SHIFT
  1060. workspace_offset[axis] = offs;
  1061. #endif
  1062. #if ENABLED(DUAL_X_CARRIAGE)
  1063. if (axis == X_AXIS) {
  1064. // In Dual X mode hotend_offset[X] is T1's home position
  1065. float dual_max_x = max(hotend_offset[X_AXIS][1], X2_MAX_POS);
  1066. if (active_extruder != 0) {
  1067. // T1 can move from X2_MIN_POS to X2_MAX_POS or X2 home position (whichever is larger)
  1068. soft_endstop_min[X_AXIS] = X2_MIN_POS + offs;
  1069. soft_endstop_max[X_AXIS] = dual_max_x + offs;
  1070. }
  1071. else if (dual_x_carriage_mode == DXC_DUPLICATION_MODE) {
  1072. // In Duplication Mode, T0 can move as far left as X_MIN_POS
  1073. // but not so far to the right that T1 would move past the end
  1074. soft_endstop_min[X_AXIS] = base_min_pos(X_AXIS) + offs;
  1075. soft_endstop_max[X_AXIS] = min(base_max_pos(X_AXIS), dual_max_x - duplicate_extruder_x_offset) + offs;
  1076. }
  1077. else {
  1078. // In other modes, T0 can move from X_MIN_POS to X_MAX_POS
  1079. soft_endstop_min[axis] = base_min_pos(axis) + offs;
  1080. soft_endstop_max[axis] = base_max_pos(axis) + offs;
  1081. }
  1082. }
  1083. #elif ENABLED(DELTA)
  1084. soft_endstop_min[axis] = base_min_pos(axis) + offs;
  1085. soft_endstop_max[axis] = (axis == Z_AXIS ? delta_height : base_max_pos(axis)) + offs;
  1086. #else
  1087. soft_endstop_min[axis] = base_min_pos(axis) + offs;
  1088. soft_endstop_max[axis] = base_max_pos(axis) + offs;
  1089. #endif
  1090. #if ENABLED(DEBUG_LEVELING_FEATURE)
  1091. if (DEBUGGING(LEVELING)) {
  1092. SERIAL_ECHOPAIR("For ", axis_codes[axis]);
  1093. #if HAS_HOME_OFFSET
  1094. SERIAL_ECHOPAIR(" axis:\n home_offset = ", home_offset[axis]);
  1095. #endif
  1096. #if HAS_POSITION_SHIFT
  1097. SERIAL_ECHOPAIR("\n position_shift = ", position_shift[axis]);
  1098. #endif
  1099. SERIAL_ECHOPAIR("\n soft_endstop_min = ", soft_endstop_min[axis]);
  1100. SERIAL_ECHOLNPAIR("\n soft_endstop_max = ", soft_endstop_max[axis]);
  1101. }
  1102. #endif
  1103. #if ENABLED(DELTA)
  1104. switch(axis) {
  1105. case X_AXIS:
  1106. case Y_AXIS:
  1107. // Get a minimum radius for clamping
  1108. soft_endstop_radius = MIN3(FABS(max(soft_endstop_min[X_AXIS], soft_endstop_min[Y_AXIS])), soft_endstop_max[X_AXIS], soft_endstop_max[Y_AXIS]);
  1109. soft_endstop_radius_2 = sq(soft_endstop_radius);
  1110. break;
  1111. case Z_AXIS:
  1112. delta_clip_start_height = soft_endstop_max[axis] - delta_safe_distance_from_top();
  1113. default: break;
  1114. }
  1115. #endif
  1116. }
  1117. #endif // HAS_WORKSPACE_OFFSET || DUAL_X_CARRIAGE
  1118. #if HAS_M206_COMMAND
  1119. /**
  1120. * Change the home offset for an axis, update the current
  1121. * position and the software endstops to retain the same
  1122. * relative distance to the new home.
  1123. *
  1124. * Since this changes the current_position, code should
  1125. * call sync_plan_position soon after this.
  1126. */
  1127. void set_home_offset(const AxisEnum axis, const float v) {
  1128. current_position[axis] += v - home_offset[axis];
  1129. home_offset[axis] = v;
  1130. update_software_endstops(axis);
  1131. }
  1132. #endif // HAS_M206_COMMAND