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

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