My Marlin configs for Fabrikator Mini and CTC i3 Pro B
您最多选择25个主题 主题必须以字母或数字开头,可以包含连字符 (-),并且长度不得超过35个字符

G29.cpp 31KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902
  1. /**
  2. * Marlin 3D Printer Firmware
  3. * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
  4. *
  5. * Based on Sprinter and grbl.
  6. * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
  7. *
  8. * This program is free software: you can redistribute it and/or modify
  9. * it under the terms of the GNU General Public License as published by
  10. * the Free Software Foundation, either version 3 of the License, or
  11. * (at your option) any later version.
  12. *
  13. * This program is distributed in the hope that it will be useful,
  14. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  16. * GNU General Public License for more details.
  17. *
  18. * You should have received a copy of the GNU General Public License
  19. * along with this program. If not, see <https://www.gnu.org/licenses/>.
  20. *
  21. */
  22. /**
  23. * G29.cpp - Auto Bed Leveling
  24. */
  25. #include "../../../inc/MarlinConfig.h"
  26. #if HAS_ABL_NOT_UBL
  27. #include "../../gcode.h"
  28. #include "../../../feature/bedlevel/bedlevel.h"
  29. #include "../../../module/motion.h"
  30. #include "../../../module/planner.h"
  31. #include "../../../module/stepper.h"
  32. #include "../../../module/probe.h"
  33. #include "../../queue.h"
  34. #if HAS_PTC
  35. #include "../../../feature/probe_temp_comp.h"
  36. #include "../../../module/temperature.h"
  37. #endif
  38. #if HAS_STATUS_MESSAGE
  39. #include "../../../lcd/marlinui.h"
  40. #endif
  41. #if ENABLED(AUTO_BED_LEVELING_LINEAR)
  42. #include "../../../libs/least_squares_fit.h"
  43. #endif
  44. #if ABL_PLANAR
  45. #include "../../../libs/vector_3.h"
  46. #endif
  47. #define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
  48. #include "../../../core/debug_out.h"
  49. #if ENABLED(EXTENSIBLE_UI)
  50. #include "../../../lcd/extui/ui_api.h"
  51. #elif ENABLED(DWIN_CREALITY_LCD)
  52. #include "../../../lcd/e3v2/creality/dwin.h"
  53. #elif ENABLED(DWIN_CREALITY_LCD_ENHANCED)
  54. #include "../../../lcd/e3v2/enhanced/dwin.h"
  55. #endif
  56. #if HAS_MULTI_HOTEND
  57. #include "../../../module/tool_change.h"
  58. #endif
  59. #if ABL_USES_GRID
  60. #if ENABLED(PROBE_Y_FIRST)
  61. #define PR_OUTER_VAR abl.meshCount.x
  62. #define PR_OUTER_SIZE abl.grid_points.x
  63. #define PR_INNER_VAR abl.meshCount.y
  64. #define PR_INNER_SIZE abl.grid_points.y
  65. #else
  66. #define PR_OUTER_VAR abl.meshCount.y
  67. #define PR_OUTER_SIZE abl.grid_points.y
  68. #define PR_INNER_VAR abl.meshCount.x
  69. #define PR_INNER_SIZE abl.grid_points.x
  70. #endif
  71. #endif
  72. #define G29_RETURN(b) return TERN_(G29_RETRY_AND_RECOVER, b)
  73. // For manual probing values persist over multiple G29
  74. class G29_State {
  75. public:
  76. int verbose_level;
  77. xy_pos_t probePos;
  78. float measured_z;
  79. bool dryrun,
  80. reenable;
  81. #if EITHER(PROBE_MANUALLY, AUTO_BED_LEVELING_LINEAR)
  82. int abl_probe_index;
  83. #endif
  84. #if ENABLED(AUTO_BED_LEVELING_LINEAR)
  85. int abl_points;
  86. #elif ENABLED(AUTO_BED_LEVELING_3POINT)
  87. static constexpr int abl_points = 3;
  88. #elif ABL_USES_GRID
  89. static constexpr int abl_points = GRID_MAX_POINTS;
  90. #endif
  91. #if ABL_USES_GRID
  92. xy_int8_t meshCount;
  93. xy_pos_t probe_position_lf,
  94. probe_position_rb;
  95. xy_float_t gridSpacing; // = { 0.0f, 0.0f }
  96. #if ENABLED(AUTO_BED_LEVELING_LINEAR)
  97. bool topography_map;
  98. xy_uint8_t grid_points;
  99. #else // Bilinear
  100. static constexpr xy_uint8_t grid_points = { GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y };
  101. #endif
  102. #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
  103. float Z_offset;
  104. #endif
  105. #if ENABLED(AUTO_BED_LEVELING_LINEAR)
  106. int indexIntoAB[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
  107. float eqnAMatrix[(GRID_MAX_POINTS) * 3], // "A" matrix of the linear system of equations
  108. eqnBVector[GRID_MAX_POINTS], // "B" vector of Z points
  109. mean;
  110. #endif
  111. #endif
  112. };
  113. #if ABL_USES_GRID && EITHER(AUTO_BED_LEVELING_3POINT, AUTO_BED_LEVELING_BILINEAR)
  114. constexpr xy_uint8_t G29_State::grid_points;
  115. constexpr int G29_State::abl_points;
  116. #endif
  117. /**
  118. * G29: Detailed Z probe, probes the bed at 3 or more points.
  119. * Will fail if the printer has not been homed with G28.
  120. *
  121. * Enhanced G29 Auto Bed Leveling Probe Routine
  122. *
  123. * O Auto-level only if needed
  124. *
  125. * D Dry-Run mode. Just evaluate the bed Topology - Don't apply
  126. * or alter the bed level data. Useful to check the topology
  127. * after a first run of G29.
  128. *
  129. * J Jettison current bed leveling data
  130. *
  131. * V Set the verbose level (0-4). Example: "G29 V3"
  132. *
  133. * Parameters With LINEAR leveling only:
  134. *
  135. * P Set the size of the grid that will be probed (P x P points).
  136. * Example: "G29 P4"
  137. *
  138. * X Set the X size of the grid that will be probed (X x Y points).
  139. * Example: "G29 X7 Y5"
  140. *
  141. * Y Set the Y size of the grid that will be probed (X x Y points).
  142. *
  143. * T Generate a Bed Topology Report. Example: "G29 P5 T" for a detailed report.
  144. * This is useful for manual bed leveling and finding flaws in the bed (to
  145. * assist with part placement).
  146. * Not supported by non-linear delta printer bed leveling.
  147. *
  148. * Parameters With LINEAR and BILINEAR leveling only:
  149. *
  150. * S Set the XY travel speed between probe points (in units/min)
  151. *
  152. * H Set bounds to a centered square H x H units in size
  153. *
  154. * -or-
  155. *
  156. * F Set the Front limit of the probing grid
  157. * B Set the Back limit of the probing grid
  158. * L Set the Left limit of the probing grid
  159. * R Set the Right limit of the probing grid
  160. *
  161. * Parameters with DEBUG_LEVELING_FEATURE only:
  162. *
  163. * C Make a totally fake grid with no actual probing.
  164. * For use in testing when no probing is possible.
  165. *
  166. * Parameters with BILINEAR leveling only:
  167. *
  168. * Z Supply an additional Z probe offset
  169. *
  170. * Extra parameters with PROBE_MANUALLY:
  171. *
  172. * To do manual probing simply repeat G29 until the procedure is complete.
  173. * The first G29 accepts parameters. 'G29 Q' for status, 'G29 A' to abort.
  174. *
  175. * Q Query leveling and G29 state
  176. *
  177. * A Abort current leveling procedure
  178. *
  179. * Extra parameters with BILINEAR only:
  180. *
  181. * W Write a mesh point. (If G29 is idle.)
  182. * I X index for mesh point
  183. * J Y index for mesh point
  184. * X X for mesh point, overrides I
  185. * Y Y for mesh point, overrides J
  186. * Z Z for mesh point. Otherwise, raw current Z.
  187. *
  188. * Without PROBE_MANUALLY:
  189. *
  190. * E By default G29 will engage the Z probe, test the bed, then disengage.
  191. * Include "E" to engage/disengage the Z probe for each sample.
  192. * There's no extra effect if you have a fixed Z probe.
  193. */
  194. G29_TYPE GcodeSuite::G29() {
  195. DEBUG_SECTION(log_G29, "G29", DEBUGGING(LEVELING));
  196. TERN_(PROBE_MANUALLY, static) G29_State abl;
  197. TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_PROBE));
  198. reset_stepper_timeout();
  199. const bool seenQ = EITHER(DEBUG_LEVELING_FEATURE, PROBE_MANUALLY) && parser.seen_test('Q');
  200. // G29 Q is also available if debugging
  201. #if ENABLED(DEBUG_LEVELING_FEATURE)
  202. if (seenQ || DEBUGGING(LEVELING)) log_machine_info();
  203. if (DISABLED(PROBE_MANUALLY) && seenQ) G29_RETURN(false);
  204. #endif
  205. const bool seenA = TERN0(PROBE_MANUALLY, parser.seen_test('A')),
  206. no_action = seenA || seenQ,
  207. faux = ENABLED(DEBUG_LEVELING_FEATURE) && DISABLED(PROBE_MANUALLY) ? parser.boolval('C') : no_action;
  208. if (!no_action && planner.leveling_active && parser.boolval('O')) { // Auto-level only if needed
  209. if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> Auto-level not needed, skip");
  210. G29_RETURN(false);
  211. }
  212. // Send 'N' to force homing before G29 (internal only)
  213. if (parser.seen_test('N'))
  214. process_subcommands_now(TERN(CAN_SET_LEVELING_AFTER_G28, F("G28L0"), FPSTR(G28_STR)));
  215. // Don't allow auto-leveling without homing first
  216. if (homing_needed_error()) G29_RETURN(false);
  217. #if ENABLED(AUTO_BED_LEVELING_3POINT)
  218. vector_3 points[3];
  219. probe.get_three_points(points);
  220. #endif
  221. #if ENABLED(AUTO_BED_LEVELING_LINEAR)
  222. struct linear_fit_data lsf_results;
  223. #endif
  224. /**
  225. * On the initial G29 fetch command parameters.
  226. */
  227. if (!g29_in_progress) {
  228. TERN_(HAS_MULTI_HOTEND, if (active_extruder) tool_change(0));
  229. #if EITHER(PROBE_MANUALLY, AUTO_BED_LEVELING_LINEAR)
  230. abl.abl_probe_index = -1;
  231. #endif
  232. abl.reenable = planner.leveling_active;
  233. #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
  234. const bool seen_w = parser.seen_test('W');
  235. if (seen_w) {
  236. if (!leveling_is_valid()) {
  237. SERIAL_ERROR_MSG("No bilinear grid");
  238. G29_RETURN(false);
  239. }
  240. const float rz = parser.seenval('Z') ? RAW_Z_POSITION(parser.value_linear_units()) : current_position.z;
  241. if (!WITHIN(rz, -10, 10)) {
  242. SERIAL_ERROR_MSG("Bad Z value");
  243. G29_RETURN(false);
  244. }
  245. const float rx = RAW_X_POSITION(parser.linearval('X', NAN)),
  246. ry = RAW_Y_POSITION(parser.linearval('Y', NAN));
  247. int8_t i = parser.byteval('I', -1), j = parser.byteval('J', -1);
  248. #pragma GCC diagnostic push
  249. #pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
  250. if (!isnan(rx) && !isnan(ry)) {
  251. // Get nearest i / j from rx / ry
  252. i = (rx - bilinear_start.x + 0.5 * abl.gridSpacing.x) / abl.gridSpacing.x;
  253. j = (ry - bilinear_start.y + 0.5 * abl.gridSpacing.y) / abl.gridSpacing.y;
  254. LIMIT(i, 0, (GRID_MAX_POINTS_X) - 1);
  255. LIMIT(j, 0, (GRID_MAX_POINTS_Y) - 1);
  256. }
  257. #pragma GCC diagnostic pop
  258. if (WITHIN(i, 0, (GRID_MAX_POINTS_X) - 1) && WITHIN(j, 0, (GRID_MAX_POINTS_Y) - 1)) {
  259. set_bed_leveling_enabled(false);
  260. z_values[i][j] = rz;
  261. TERN_(ABL_BILINEAR_SUBDIVISION, bed_level_virt_interpolate());
  262. TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(i, j, rz));
  263. set_bed_leveling_enabled(abl.reenable);
  264. if (abl.reenable) report_current_position();
  265. }
  266. G29_RETURN(false);
  267. } // parser.seen_test('W')
  268. #else
  269. constexpr bool seen_w = false;
  270. #endif
  271. // Jettison bed leveling data
  272. if (!seen_w && parser.seen_test('J')) {
  273. reset_bed_level();
  274. G29_RETURN(false);
  275. }
  276. abl.verbose_level = parser.intval('V');
  277. if (!WITHIN(abl.verbose_level, 0, 4)) {
  278. SERIAL_ECHOLNPGM("?(V)erbose level implausible (0-4).");
  279. G29_RETURN(false);
  280. }
  281. abl.dryrun = parser.boolval('D') || TERN0(PROBE_MANUALLY, no_action);
  282. #if ENABLED(AUTO_BED_LEVELING_LINEAR)
  283. incremental_LSF_reset(&lsf_results);
  284. abl.topography_map = abl.verbose_level > 2 || parser.boolval('T');
  285. // X and Y specify points in each direction, overriding the default
  286. // These values may be saved with the completed mesh
  287. abl.grid_points.set(
  288. parser.byteval('X', GRID_MAX_POINTS_X),
  289. parser.byteval('Y', GRID_MAX_POINTS_Y)
  290. );
  291. if (parser.seenval('P')) abl.grid_points.x = abl.grid_points.y = parser.value_int();
  292. if (!WITHIN(abl.grid_points.x, 2, GRID_MAX_POINTS_X)) {
  293. SERIAL_ECHOLNPGM("?Probe points (X) implausible (2-" STRINGIFY(GRID_MAX_POINTS_X) ").");
  294. G29_RETURN(false);
  295. }
  296. if (!WITHIN(abl.grid_points.y, 2, GRID_MAX_POINTS_Y)) {
  297. SERIAL_ECHOLNPGM("?Probe points (Y) implausible (2-" STRINGIFY(GRID_MAX_POINTS_Y) ").");
  298. G29_RETURN(false);
  299. }
  300. abl.abl_points = abl.grid_points.x * abl.grid_points.y;
  301. abl.mean = 0;
  302. #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
  303. abl.Z_offset = parser.linearval('Z');
  304. #endif
  305. #if ABL_USES_GRID
  306. xy_probe_feedrate_mm_s = MMM_TO_MMS(parser.linearval('S', XY_PROBE_FEEDRATE));
  307. const float x_min = probe.min_x(), x_max = probe.max_x(),
  308. y_min = probe.min_y(), y_max = probe.max_y();
  309. if (parser.seen('H')) {
  310. const int16_t size = (int16_t)parser.value_linear_units();
  311. abl.probe_position_lf.set(_MAX((X_CENTER) - size / 2, x_min), _MAX((Y_CENTER) - size / 2, y_min));
  312. abl.probe_position_rb.set(_MIN(abl.probe_position_lf.x + size, x_max), _MIN(abl.probe_position_lf.y + size, y_max));
  313. }
  314. else {
  315. abl.probe_position_lf.set(parser.linearval('L', x_min), parser.linearval('F', y_min));
  316. abl.probe_position_rb.set(parser.linearval('R', x_max), parser.linearval('B', y_max));
  317. }
  318. if (!probe.good_bounds(abl.probe_position_lf, abl.probe_position_rb)) {
  319. if (DEBUGGING(LEVELING)) {
  320. DEBUG_ECHOLNPGM("G29 L", abl.probe_position_lf.x, " R", abl.probe_position_rb.x,
  321. " F", abl.probe_position_lf.y, " B", abl.probe_position_rb.y);
  322. }
  323. SERIAL_ECHOLNPGM("? (L,R,F,B) out of bounds.");
  324. G29_RETURN(false);
  325. }
  326. // Probe at the points of a lattice grid
  327. abl.gridSpacing.set((abl.probe_position_rb.x - abl.probe_position_lf.x) / (abl.grid_points.x - 1),
  328. (abl.probe_position_rb.y - abl.probe_position_lf.y) / (abl.grid_points.y - 1));
  329. #endif // ABL_USES_GRID
  330. if (abl.verbose_level > 0) {
  331. SERIAL_ECHOPGM("G29 Auto Bed Leveling");
  332. if (abl.dryrun) SERIAL_ECHOPGM(" (DRYRUN)");
  333. SERIAL_EOL();
  334. }
  335. planner.synchronize();
  336. #if ENABLED(AUTO_BED_LEVELING_3POINT)
  337. if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> 3-point Leveling");
  338. points[0].z = points[1].z = points[2].z = 0; // Probe at 3 arbitrary points
  339. #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
  340. TERN_(EXTENSIBLE_UI, ExtUI::onMeshLevelingStart());
  341. TERN_(DWIN_CREALITY_LCD_ENHANCED, DWIN_MeshLevelingStart());
  342. #endif
  343. if (!faux) {
  344. remember_feedrate_scaling_off();
  345. #if ENABLED(PREHEAT_BEFORE_LEVELING)
  346. if (!abl.dryrun) probe.preheat_for_probing(LEVELING_NOZZLE_TEMP, LEVELING_BED_TEMP);
  347. #endif
  348. }
  349. // Disable auto bed leveling during G29.
  350. // Be formal so G29 can be done successively without G28.
  351. if (!no_action) set_bed_leveling_enabled(false);
  352. // Deploy certain probes before starting probing
  353. #if HAS_BED_PROBE
  354. if (ENABLED(BLTOUCH))
  355. do_z_clearance(Z_CLEARANCE_DEPLOY_PROBE);
  356. else if (probe.deploy()) {
  357. set_bed_leveling_enabled(abl.reenable);
  358. G29_RETURN(false);
  359. }
  360. #endif
  361. #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
  362. if (TERN1(PROBE_MANUALLY, !no_action)
  363. && (abl.gridSpacing != bilinear_grid_spacing || abl.probe_position_lf != bilinear_start)
  364. ) {
  365. // Reset grid to 0.0 or "not probed". (Also disables ABL)
  366. reset_bed_level();
  367. // Initialize a grid with the given dimensions
  368. bilinear_grid_spacing = abl.gridSpacing;
  369. bilinear_start = abl.probe_position_lf;
  370. // Can't re-enable (on error) until the new grid is written
  371. abl.reenable = false;
  372. }
  373. #endif // AUTO_BED_LEVELING_BILINEAR
  374. } // !g29_in_progress
  375. #if ENABLED(PROBE_MANUALLY)
  376. // For manual probing, get the next index to probe now.
  377. // On the first probe this will be incremented to 0.
  378. if (!no_action) {
  379. ++abl.abl_probe_index;
  380. g29_in_progress = true;
  381. }
  382. // Abort current G29 procedure, go back to idle state
  383. if (seenA && g29_in_progress) {
  384. SERIAL_ECHOLNPGM("Manual G29 aborted");
  385. SET_SOFT_ENDSTOP_LOOSE(false);
  386. set_bed_leveling_enabled(abl.reenable);
  387. g29_in_progress = false;
  388. TERN_(LCD_BED_LEVELING, ui.wait_for_move = false);
  389. }
  390. // Query G29 status
  391. if (abl.verbose_level || seenQ) {
  392. SERIAL_ECHOPGM("Manual G29 ");
  393. if (g29_in_progress)
  394. SERIAL_ECHOLNPGM("point ", _MIN(abl.abl_probe_index + 1, abl.abl_points), " of ", abl.abl_points);
  395. else
  396. SERIAL_ECHOLNPGM("idle");
  397. }
  398. if (no_action) G29_RETURN(false);
  399. if (abl.abl_probe_index == 0) {
  400. // For the initial G29 S2 save software endstop state
  401. SET_SOFT_ENDSTOP_LOOSE(true);
  402. // Move close to the bed before the first point
  403. do_blocking_move_to_z(0);
  404. }
  405. else {
  406. #if EITHER(AUTO_BED_LEVELING_LINEAR, AUTO_BED_LEVELING_3POINT)
  407. const uint16_t index = abl.abl_probe_index - 1;
  408. #endif
  409. // For G29 after adjusting Z.
  410. // Save the previous Z before going to the next point
  411. abl.measured_z = current_position.z;
  412. #if ENABLED(AUTO_BED_LEVELING_LINEAR)
  413. abl.mean += abl.measured_z;
  414. abl.eqnBVector[index] = abl.measured_z;
  415. abl.eqnAMatrix[index + 0 * abl.abl_points] = abl.probePos.x;
  416. abl.eqnAMatrix[index + 1 * abl.abl_points] = abl.probePos.y;
  417. abl.eqnAMatrix[index + 2 * abl.abl_points] = 1;
  418. incremental_LSF(&lsf_results, abl.probePos, abl.measured_z);
  419. #elif ENABLED(AUTO_BED_LEVELING_3POINT)
  420. points[index].z = abl.measured_z;
  421. #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
  422. const float newz = abl.measured_z + abl.Z_offset;
  423. z_values[abl.meshCount.x][abl.meshCount.y] = newz;
  424. TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(abl.meshCount, newz));
  425. if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM_P(PSTR("Save X"), abl.meshCount.x, SP_Y_STR, abl.meshCount.y, SP_Z_STR, abl.measured_z + abl.Z_offset);
  426. #endif
  427. }
  428. //
  429. // If there's another point to sample, move there with optional lift.
  430. //
  431. #if ABL_USES_GRID
  432. // Skip any unreachable points
  433. while (abl.abl_probe_index < abl.abl_points) {
  434. // Set abl.meshCount.x, abl.meshCount.y based on abl.abl_probe_index, with zig-zag
  435. PR_OUTER_VAR = abl.abl_probe_index / PR_INNER_SIZE;
  436. PR_INNER_VAR = abl.abl_probe_index - (PR_OUTER_VAR * PR_INNER_SIZE);
  437. // Probe in reverse order for every other row/column
  438. const bool zig = (PR_OUTER_VAR & 1); // != ((PR_OUTER_SIZE) & 1);
  439. if (zig) PR_INNER_VAR = (PR_INNER_SIZE - 1) - PR_INNER_VAR;
  440. abl.probePos = abl.probe_position_lf + abl.gridSpacing * abl.meshCount.asFloat();
  441. TERN_(AUTO_BED_LEVELING_LINEAR, abl.indexIntoAB[abl.meshCount.x][abl.meshCount.y] = abl.abl_probe_index);
  442. // Keep looping till a reachable point is found
  443. if (position_is_reachable(abl.probePos)) break;
  444. ++abl.abl_probe_index;
  445. }
  446. // Is there a next point to move to?
  447. if (abl.abl_probe_index < abl.abl_points) {
  448. _manual_goto_xy(abl.probePos); // Can be used here too!
  449. // Disable software endstops to allow manual adjustment
  450. // If G29 is not completed, they will not be re-enabled
  451. SET_SOFT_ENDSTOP_LOOSE(true);
  452. G29_RETURN(false);
  453. }
  454. else {
  455. // Leveling done! Fall through to G29 finishing code below
  456. SERIAL_ECHOLNPGM("Grid probing done.");
  457. // Re-enable software endstops, if needed
  458. SET_SOFT_ENDSTOP_LOOSE(false);
  459. }
  460. #elif ENABLED(AUTO_BED_LEVELING_3POINT)
  461. // Probe at 3 arbitrary points
  462. if (abl.abl_probe_index < abl.abl_points) {
  463. abl.probePos = xy_pos_t(points[abl.abl_probe_index]);
  464. _manual_goto_xy(abl.probePos);
  465. // Disable software endstops to allow manual adjustment
  466. // If G29 is not completed, they will not be re-enabled
  467. SET_SOFT_ENDSTOP_LOOSE(true);
  468. G29_RETURN(false);
  469. }
  470. else {
  471. SERIAL_ECHOLNPGM("3-point probing done.");
  472. // Re-enable software endstops, if needed
  473. SET_SOFT_ENDSTOP_LOOSE(false);
  474. if (!abl.dryrun) {
  475. vector_3 planeNormal = vector_3::cross(points[0] - points[1], points[2] - points[1]).get_normal();
  476. if (planeNormal.z < 0) planeNormal *= -1;
  477. planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
  478. // Can't re-enable (on error) until the new grid is written
  479. abl.reenable = false;
  480. }
  481. }
  482. #endif // AUTO_BED_LEVELING_3POINT
  483. #else // !PROBE_MANUALLY
  484. {
  485. const ProbePtRaise raise_after = parser.boolval('E') ? PROBE_PT_STOW : PROBE_PT_RAISE;
  486. abl.measured_z = 0;
  487. #if ABL_USES_GRID
  488. bool zig = PR_OUTER_SIZE & 1; // Always end at RIGHT and BACK_PROBE_BED_POSITION
  489. abl.measured_z = 0;
  490. // Outer loop is X with PROBE_Y_FIRST enabled
  491. // Outer loop is Y with PROBE_Y_FIRST disabled
  492. for (PR_OUTER_VAR = 0; PR_OUTER_VAR < PR_OUTER_SIZE && !isnan(abl.measured_z); PR_OUTER_VAR++) {
  493. int8_t inStart, inStop, inInc;
  494. if (zig) { // Zig away from origin
  495. inStart = 0; // Left or front
  496. inStop = PR_INNER_SIZE; // Right or back
  497. inInc = 1; // Zig right
  498. }
  499. else { // Zag towards origin
  500. inStart = PR_INNER_SIZE - 1; // Right or back
  501. inStop = -1; // Left or front
  502. inInc = -1; // Zag left
  503. }
  504. zig ^= true; // zag
  505. // An index to print current state
  506. uint8_t pt_index = (PR_OUTER_VAR) * (PR_INNER_SIZE) + 1;
  507. // Inner loop is Y with PROBE_Y_FIRST enabled
  508. // Inner loop is X with PROBE_Y_FIRST disabled
  509. for (PR_INNER_VAR = inStart; PR_INNER_VAR != inStop; pt_index++, PR_INNER_VAR += inInc) {
  510. abl.probePos = abl.probe_position_lf + abl.gridSpacing * abl.meshCount.asFloat();
  511. TERN_(AUTO_BED_LEVELING_LINEAR, abl.indexIntoAB[abl.meshCount.x][abl.meshCount.y] = ++abl.abl_probe_index); // 0...
  512. // Avoid probing outside the round or hexagonal area
  513. if (TERN0(IS_KINEMATIC, !probe.can_reach(abl.probePos))) continue;
  514. if (abl.verbose_level) SERIAL_ECHOLNPGM("Probing mesh point ", pt_index, "/", abl.abl_points, ".");
  515. TERN_(HAS_STATUS_MESSAGE, ui.status_printf(0, F(S_FMT " %i/%i"), GET_TEXT(MSG_PROBING_POINT), int(pt_index), int(abl.abl_points)));
  516. abl.measured_z = faux ? 0.001f * random(-100, 101) : probe.probe_at_point(abl.probePos, raise_after, abl.verbose_level);
  517. if (isnan(abl.measured_z)) {
  518. set_bed_leveling_enabled(abl.reenable);
  519. break; // Breaks out of both loops
  520. }
  521. TERN_(PTC_BED, ptc.compensate_measurement(TSI_BED, thermalManager.degBed(), abl.measured_z));
  522. TERN_(PTC_PROBE, ptc.compensate_measurement(TSI_PROBE, thermalManager.degProbe(), abl.measured_z));
  523. TERN_(PTC_HOTEND, ptc.compensate_measurement(TSI_EXT, thermalManager.degHotend(0), abl.measured_z));
  524. #if ENABLED(AUTO_BED_LEVELING_LINEAR)
  525. abl.mean += abl.measured_z;
  526. abl.eqnBVector[abl.abl_probe_index] = abl.measured_z;
  527. abl.eqnAMatrix[abl.abl_probe_index + 0 * abl.abl_points] = abl.probePos.x;
  528. abl.eqnAMatrix[abl.abl_probe_index + 1 * abl.abl_points] = abl.probePos.y;
  529. abl.eqnAMatrix[abl.abl_probe_index + 2 * abl.abl_points] = 1;
  530. incremental_LSF(&lsf_results, abl.probePos, abl.measured_z);
  531. #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
  532. const float z = abl.measured_z + abl.Z_offset;
  533. z_values[abl.meshCount.x][abl.meshCount.y] = z PLUS_TERN0(X_AXIS_TWIST_COMPENSATION, xatc.compensation(abl.probePos));
  534. TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(abl.meshCount, z));
  535. #endif
  536. abl.reenable = false;
  537. idle_no_sleep();
  538. } // inner
  539. } // outer
  540. #elif ENABLED(AUTO_BED_LEVELING_3POINT)
  541. // Probe at 3 arbitrary points
  542. LOOP_L_N(i, 3) {
  543. if (abl.verbose_level) SERIAL_ECHOLNPGM("Probing point ", i + 1, "/3.");
  544. TERN_(HAS_STATUS_MESSAGE, ui.status_printf(0, F(S_FMT " %i/3"), GET_TEXT(MSG_PROBING_POINT), int(i + 1)));
  545. // Retain the last probe position
  546. abl.probePos = xy_pos_t(points[i]);
  547. abl.measured_z = faux ? 0.001 * random(-100, 101) : probe.probe_at_point(abl.probePos, raise_after, abl.verbose_level);
  548. if (isnan(abl.measured_z)) {
  549. set_bed_leveling_enabled(abl.reenable);
  550. break;
  551. }
  552. points[i].z = abl.measured_z;
  553. }
  554. if (!abl.dryrun && !isnan(abl.measured_z)) {
  555. vector_3 planeNormal = vector_3::cross(points[0] - points[1], points[2] - points[1]).get_normal();
  556. if (planeNormal.z < 0) planeNormal *= -1;
  557. planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
  558. // Can't re-enable (on error) until the new grid is written
  559. abl.reenable = false;
  560. }
  561. #endif // AUTO_BED_LEVELING_3POINT
  562. TERN_(HAS_STATUS_MESSAGE, ui.reset_status());
  563. // Stow the probe. No raise for FIX_MOUNTED_PROBE.
  564. if (probe.stow()) {
  565. set_bed_leveling_enabled(abl.reenable);
  566. abl.measured_z = NAN;
  567. }
  568. }
  569. #endif // !PROBE_MANUALLY
  570. //
  571. // G29 Finishing Code
  572. //
  573. // Unless this is a dry run, auto bed leveling will
  574. // definitely be enabled after this point.
  575. //
  576. // If code above wants to continue leveling, it should
  577. // return or loop before this point.
  578. //
  579. if (DEBUGGING(LEVELING)) DEBUG_POS("> probing complete", current_position);
  580. #if ENABLED(PROBE_MANUALLY)
  581. g29_in_progress = false;
  582. TERN_(LCD_BED_LEVELING, ui.wait_for_move = false);
  583. #endif
  584. // Calculate leveling, print reports, correct the position
  585. if (!isnan(abl.measured_z)) {
  586. #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
  587. if (!abl.dryrun) extrapolate_unprobed_bed_level();
  588. print_bilinear_leveling_grid();
  589. refresh_bed_level();
  590. TERN_(ABL_BILINEAR_SUBDIVISION, print_bilinear_leveling_grid_virt());
  591. #elif ENABLED(AUTO_BED_LEVELING_LINEAR)
  592. // For LINEAR leveling calculate matrix, print reports, correct the position
  593. /**
  594. * solve the plane equation ax + by + d = z
  595. * A is the matrix with rows [x y 1] for all the probed points
  596. * B is the vector of the Z positions
  597. * the normal vector to the plane is formed by the coefficients of the
  598. * plane equation in the standard form, which is Vx*x+Vy*y+Vz*z+d = 0
  599. * so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z
  600. */
  601. struct { float a, b, d; } plane_equation_coefficients;
  602. finish_incremental_LSF(&lsf_results);
  603. plane_equation_coefficients.a = -lsf_results.A; // We should be able to eliminate the '-' on these three lines and down below
  604. plane_equation_coefficients.b = -lsf_results.B; // but that is not yet tested.
  605. plane_equation_coefficients.d = -lsf_results.D;
  606. abl.mean /= abl.abl_points;
  607. if (abl.verbose_level) {
  608. SERIAL_ECHOPAIR_F("Eqn coefficients: a: ", plane_equation_coefficients.a, 8);
  609. SERIAL_ECHOPAIR_F(" b: ", plane_equation_coefficients.b, 8);
  610. SERIAL_ECHOPAIR_F(" d: ", plane_equation_coefficients.d, 8);
  611. if (abl.verbose_level > 2)
  612. SERIAL_ECHOPAIR_F("\nMean of sampled points: ", abl.mean, 8);
  613. SERIAL_EOL();
  614. }
  615. // Create the matrix but don't correct the position yet
  616. if (!abl.dryrun)
  617. planner.bed_level_matrix = matrix_3x3::create_look_at(
  618. vector_3(-plane_equation_coefficients.a, -plane_equation_coefficients.b, 1) // We can eliminate the '-' here and up above
  619. );
  620. // Show the Topography map if enabled
  621. if (abl.topography_map) {
  622. float min_diff = 999;
  623. auto print_topo_map = [&](FSTR_P const title, const bool get_min) {
  624. SERIAL_ECHOF(title);
  625. for (int8_t yy = abl.grid_points.y - 1; yy >= 0; yy--) {
  626. LOOP_L_N(xx, abl.grid_points.x) {
  627. const int ind = abl.indexIntoAB[xx][yy];
  628. xyz_float_t tmp = { abl.eqnAMatrix[ind + 0 * abl.abl_points],
  629. abl.eqnAMatrix[ind + 1 * abl.abl_points], 0 };
  630. planner.bed_level_matrix.apply_rotation_xyz(tmp.x, tmp.y, tmp.z);
  631. if (get_min) NOMORE(min_diff, abl.eqnBVector[ind] - tmp.z);
  632. const float subval = get_min ? abl.mean : tmp.z + min_diff,
  633. diff = abl.eqnBVector[ind] - subval;
  634. SERIAL_CHAR(' '); if (diff >= 0.0) SERIAL_CHAR('+'); // Include + for column alignment
  635. SERIAL_ECHO_F(diff, 5);
  636. } // xx
  637. SERIAL_EOL();
  638. } // yy
  639. SERIAL_EOL();
  640. };
  641. print_topo_map(F("\nBed Height Topography:\n"
  642. " +--- BACK --+\n"
  643. " | |\n"
  644. " L | (+) | R\n"
  645. " E | | I\n"
  646. " F | (-) N (+) | G\n"
  647. " T | | H\n"
  648. " | (-) | T\n"
  649. " | |\n"
  650. " O-- FRONT --+\n"
  651. " (0,0)\n"), true);
  652. if (abl.verbose_level > 3)
  653. print_topo_map(F("\nCorrected Bed Height vs. Bed Topology:\n"), false);
  654. } // abl.topography_map
  655. #endif // AUTO_BED_LEVELING_LINEAR
  656. #if ABL_PLANAR
  657. // For LINEAR and 3POINT leveling correct the current position
  658. if (abl.verbose_level > 0)
  659. planner.bed_level_matrix.debug(F("\n\nBed Level Correction Matrix:"));
  660. if (!abl.dryrun) {
  661. //
  662. // Correct the current XYZ position based on the tilted plane.
  663. //
  664. if (DEBUGGING(LEVELING)) DEBUG_POS("G29 uncorrected XYZ", current_position);
  665. xyze_pos_t converted = current_position;
  666. planner.force_unapply_leveling(converted); // use conversion machinery
  667. // Use the last measured distance to the bed, if possible
  668. if ( NEAR(current_position.x, abl.probePos.x - probe.offset_xy.x)
  669. && NEAR(current_position.y, abl.probePos.y - probe.offset_xy.y)
  670. ) {
  671. const float simple_z = current_position.z - abl.measured_z;
  672. if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Probed Z", simple_z, " Matrix Z", converted.z, " Discrepancy ", simple_z - converted.z);
  673. converted.z = simple_z;
  674. }
  675. // The rotated XY and corrected Z are now current_position
  676. current_position = converted;
  677. if (DEBUGGING(LEVELING)) DEBUG_POS("G29 corrected XYZ", current_position);
  678. }
  679. #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
  680. if (!abl.dryrun) {
  681. if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("G29 uncorrected Z:", current_position.z);
  682. // Unapply the offset because it is going to be immediately applied
  683. // and cause compensation movement in Z
  684. const float fade_scaling_factor = TERN(ENABLE_LEVELING_FADE_HEIGHT, planner.fade_scaling_factor_for_z(current_position.z), 1);
  685. current_position.z -= fade_scaling_factor * bilinear_z_offset(current_position);
  686. if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM(" corrected Z:", current_position.z);
  687. }
  688. #endif // ABL_PLANAR
  689. // Auto Bed Leveling is complete! Enable if possible.
  690. planner.leveling_active = !abl.dryrun || abl.reenable;
  691. } // !isnan(abl.measured_z)
  692. // Restore state after probing
  693. if (!faux) restore_feedrate_and_scaling();
  694. // Sync the planner from the current_position
  695. if (planner.leveling_active) sync_plan_position();
  696. TERN_(HAS_BED_PROBE, probe.move_z_after_probing());
  697. #ifdef Z_PROBE_END_SCRIPT
  698. if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Z Probe End Script: ", Z_PROBE_END_SCRIPT);
  699. planner.synchronize();
  700. process_subcommands_now(F(Z_PROBE_END_SCRIPT));
  701. #endif
  702. TERN_(HAS_DWIN_E3V2_BASIC, DWIN_CompletedLeveling());
  703. report_current_position();
  704. TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_IDLE));
  705. G29_RETURN(isnan(abl.measured_z));
  706. }
  707. #endif // HAS_ABL_NOT_UBL