123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017 |
- /**
- * Marlin 3D Printer Firmware
- * Copyright (C) 2019 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
- *
- * Based on Sprinter and grbl.
- * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- */
-
- /**
- * G29.cpp - Auto Bed Leveling
- */
-
- #include "../../../inc/MarlinConfig.h"
-
- #if HAS_ABL_NOT_UBL
-
- #include "../../gcode.h"
- #include "../../../feature/bedlevel/bedlevel.h"
- #include "../../../module/motion.h"
- #include "../../../module/planner.h"
- #include "../../../module/stepper.h"
- #include "../../../module/probe.h"
- #include "../../queue.h"
-
- #if ENABLED(LCD_BED_LEVELING) && ENABLED(PROBE_MANUALLY)
- #include "../../../lcd/ultralcd.h"
- #endif
-
- #if ENABLED(AUTO_BED_LEVELING_LINEAR)
- #include "../../../libs/least_squares_fit.h"
- #endif
-
- #if ABL_PLANAR
- #include "../../../libs/vector_3.h"
- #endif
-
- #if ABL_GRID
- #if ENABLED(PROBE_Y_FIRST)
- #define PR_OUTER_VAR xCount
- #define PR_OUTER_END abl_grid_points_x
- #define PR_INNER_VAR yCount
- #define PR_INNER_END abl_grid_points_y
- #else
- #define PR_OUTER_VAR yCount
- #define PR_OUTER_END abl_grid_points_y
- #define PR_INNER_VAR xCount
- #define PR_INNER_END abl_grid_points_x
- #endif
- #endif
-
- #if ENABLED(G29_RETRY_AND_RECOVER)
- #define G29_RETURN(b) return b;
- #else
- #define G29_RETURN(b) return;
- #endif
-
- /**
- * G29: Detailed Z probe, probes the bed at 3 or more points.
- * Will fail if the printer has not been homed with G28.
- *
- * Enhanced G29 Auto Bed Leveling Probe Routine
- *
- * O Auto-level only if needed
- *
- * D Dry-Run mode. Just evaluate the bed Topology - Don't apply
- * or alter the bed level data. Useful to check the topology
- * after a first run of G29.
- *
- * J Jettison current bed leveling data
- *
- * V Set the verbose level (0-4). Example: "G29 V3"
- *
- * Parameters With LINEAR leveling only:
- *
- * P Set the size of the grid that will be probed (P x P points).
- * Example: "G29 P4"
- *
- * X Set the X size of the grid that will be probed (X x Y points).
- * Example: "G29 X7 Y5"
- *
- * Y Set the Y size of the grid that will be probed (X x Y points).
- *
- * T Generate a Bed Topology Report. Example: "G29 P5 T" for a detailed report.
- * This is useful for manual bed leveling and finding flaws in the bed (to
- * assist with part placement).
- * Not supported by non-linear delta printer bed leveling.
- *
- * Parameters With LINEAR and BILINEAR leveling only:
- *
- * S Set the XY travel speed between probe points (in units/min)
- *
- * H Set bounds to a centered square H x H units in size
- *
- * -or-
- *
- * F Set the Front limit of the probing grid
- * B Set the Back limit of the probing grid
- * L Set the Left limit of the probing grid
- * R Set the Right limit of the probing grid
- *
- * Parameters with DEBUG_LEVELING_FEATURE only:
- *
- * C Make a totally fake grid with no actual probing.
- * For use in testing when no probing is possible.
- *
- * Parameters with BILINEAR leveling only:
- *
- * Z Supply an additional Z probe offset
- *
- * Extra parameters with PROBE_MANUALLY:
- *
- * To do manual probing simply repeat G29 until the procedure is complete.
- * The first G29 accepts parameters. 'G29 Q' for status, 'G29 A' to abort.
- *
- * Q Query leveling and G29 state
- *
- * A Abort current leveling procedure
- *
- * Extra parameters with BILINEAR only:
- *
- * W Write a mesh point. (If G29 is idle.)
- * I X index for mesh point
- * J Y index for mesh point
- * X X for mesh point, overrides I
- * Y Y for mesh point, overrides J
- * Z Z for mesh point. Otherwise, raw current Z.
- *
- * Without PROBE_MANUALLY:
- *
- * E By default G29 will engage the Z probe, test the bed, then disengage.
- * Include "E" to engage/disengage the Z probe for each sample.
- * There's no extra effect if you have a fixed Z probe.
- *
- */
- G29_TYPE GcodeSuite::G29() {
-
- #if ENABLED(DEBUG_LEVELING_FEATURE) || ENABLED(PROBE_MANUALLY)
- const bool seenQ = parser.seen('Q');
- #else
- constexpr bool seenQ = false;
- #endif
-
- // G29 Q is also available if debugging
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- const uint8_t old_debug_flags = marlin_debug_flags;
- if (seenQ) marlin_debug_flags |= MARLIN_DEBUG_LEVELING;
- if (DEBUGGING(LEVELING)) {
- DEBUG_POS(">>> G29", current_position);
- log_machine_info();
- }
- marlin_debug_flags = old_debug_flags;
- #if DISABLED(PROBE_MANUALLY)
- if (seenQ) G29_RETURN(false);
- #endif
- #endif
-
- #if ENABLED(PROBE_MANUALLY)
- const bool seenA = parser.seen('A');
- #else
- constexpr bool seenA = false;
- #endif
-
- const bool no_action = seenA || seenQ,
- faux =
- #if ENABLED(DEBUG_LEVELING_FEATURE) && DISABLED(PROBE_MANUALLY)
- parser.boolval('C')
- #else
- no_action
- #endif
- ;
-
- // Don't allow auto-leveling without homing first
- if (axis_unhomed_error()) G29_RETURN(false);
-
- if (!no_action && planner.leveling_active && parser.boolval('O')) { // Auto-level only if needed
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) {
- SERIAL_ECHOLNPGM("> Auto-level not needed, skip");
- SERIAL_ECHOLNPGM("<<< G29");
- }
- #endif
- G29_RETURN(false);
- }
-
- // Define local vars 'static' for manual probing, 'auto' otherwise
- #if ENABLED(PROBE_MANUALLY)
- #define ABL_VAR static
- #else
- #define ABL_VAR
- #endif
-
- ABL_VAR int verbose_level;
- ABL_VAR float xProbe, yProbe, measured_z;
- ABL_VAR bool dryrun, abl_should_enable;
-
- #if ENABLED(PROBE_MANUALLY) || ENABLED(AUTO_BED_LEVELING_LINEAR)
- ABL_VAR int abl_probe_index;
- #endif
-
- #if HAS_SOFTWARE_ENDSTOPS && ENABLED(PROBE_MANUALLY)
- ABL_VAR bool enable_soft_endstops = true;
- #endif
-
- #if ABL_GRID
-
- #if ENABLED(PROBE_MANUALLY)
- ABL_VAR uint8_t PR_OUTER_VAR;
- ABL_VAR int8_t PR_INNER_VAR;
- #endif
-
- ABL_VAR int left_probe_bed_position, right_probe_bed_position, front_probe_bed_position, back_probe_bed_position;
- ABL_VAR float xGridSpacing = 0, yGridSpacing = 0;
-
- #if ENABLED(AUTO_BED_LEVELING_LINEAR)
- ABL_VAR uint8_t abl_grid_points_x = GRID_MAX_POINTS_X,
- abl_grid_points_y = GRID_MAX_POINTS_Y;
- ABL_VAR bool do_topography_map;
- #else // Bilinear
- uint8_t constexpr abl_grid_points_x = GRID_MAX_POINTS_X,
- abl_grid_points_y = GRID_MAX_POINTS_Y;
- #endif
-
- #if ENABLED(AUTO_BED_LEVELING_LINEAR)
- ABL_VAR int abl_points;
- #elif ENABLED(PROBE_MANUALLY) // Bilinear
- int constexpr abl_points = GRID_MAX_POINTS;
- #endif
-
- #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
-
- ABL_VAR float zoffset;
-
- #elif ENABLED(AUTO_BED_LEVELING_LINEAR)
-
- ABL_VAR int indexIntoAB[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
-
- ABL_VAR float eqnAMatrix[GRID_MAX_POINTS * 3], // "A" matrix of the linear system of equations
- eqnBVector[GRID_MAX_POINTS], // "B" vector of Z points
- mean;
- #endif
-
- #elif ENABLED(AUTO_BED_LEVELING_3POINT)
-
- #if ENABLED(PROBE_MANUALLY)
- int constexpr abl_points = 3; // used to show total points
- #endif
-
- // Probe at 3 arbitrary points
- ABL_VAR vector_3 points[3] = {
- vector_3(PROBE_PT_1_X, PROBE_PT_1_Y, 0),
- vector_3(PROBE_PT_2_X, PROBE_PT_2_Y, 0),
- vector_3(PROBE_PT_3_X, PROBE_PT_3_Y, 0)
- };
-
- #endif // AUTO_BED_LEVELING_3POINT
-
- #if ENABLED(AUTO_BED_LEVELING_LINEAR)
- struct linear_fit_data lsf_results;
- incremental_LSF_reset(&lsf_results);
- #endif
-
- /**
- * On the initial G29 fetch command parameters.
- */
- if (!g29_in_progress) {
-
- #if ENABLED(DUAL_X_CARRIAGE)
- if (active_extruder != 0) tool_change(0);
- #endif
-
- #if ENABLED(PROBE_MANUALLY) || ENABLED(AUTO_BED_LEVELING_LINEAR)
- abl_probe_index = -1;
- #endif
-
- abl_should_enable = planner.leveling_active;
-
- #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
-
- const bool seen_w = parser.seen('W');
- if (seen_w) {
- if (!leveling_is_valid()) {
- SERIAL_ERROR_MSG("No bilinear grid");
- G29_RETURN(false);
- }
-
- const float rz = parser.seenval('Z') ? RAW_Z_POSITION(parser.value_linear_units()) : current_position[Z_AXIS];
- if (!WITHIN(rz, -10, 10)) {
- SERIAL_ERROR_MSG("Bad Z value");
- G29_RETURN(false);
- }
-
- const float rx = RAW_X_POSITION(parser.linearval('X', NAN)),
- ry = RAW_Y_POSITION(parser.linearval('Y', NAN));
- int8_t i = parser.byteval('I', -1),
- j = parser.byteval('J', -1);
-
- if (!isnan(rx) && !isnan(ry)) {
- // Get nearest i / j from rx / ry
- i = (rx - bilinear_start[X_AXIS] + 0.5 * xGridSpacing) / xGridSpacing;
- j = (ry - bilinear_start[Y_AXIS] + 0.5 * yGridSpacing) / yGridSpacing;
- i = constrain(i, 0, GRID_MAX_POINTS_X - 1);
- j = constrain(j, 0, GRID_MAX_POINTS_Y - 1);
- }
- if (WITHIN(i, 0, GRID_MAX_POINTS_X - 1) && WITHIN(j, 0, GRID_MAX_POINTS_Y)) {
- set_bed_leveling_enabled(false);
- z_values[i][j] = rz;
- #if ENABLED(ABL_BILINEAR_SUBDIVISION)
- bed_level_virt_interpolate();
- #endif
- #if ENABLED(EXTENSIBLE_UI)
- ExtUI::onMeshUpdate(i, j, rz);
- #endif
- set_bed_leveling_enabled(abl_should_enable);
- if (abl_should_enable) report_current_position();
- }
- G29_RETURN(false);
- } // parser.seen('W')
-
- #else
-
- constexpr bool seen_w = false;
-
- #endif
-
- // Jettison bed leveling data
- if (!seen_w && parser.seen('J')) {
- reset_bed_level();
- G29_RETURN(false);
- }
-
- verbose_level = parser.intval('V');
- if (!WITHIN(verbose_level, 0, 4)) {
- SERIAL_ECHOLNPGM("?(V)erbose level is implausible (0-4).");
- G29_RETURN(false);
- }
-
- dryrun = parser.boolval('D')
- #if ENABLED(PROBE_MANUALLY)
- || no_action
- #endif
- ;
-
- #if ENABLED(AUTO_BED_LEVELING_LINEAR)
-
- do_topography_map = verbose_level > 2 || parser.boolval('T');
-
- // X and Y specify points in each direction, overriding the default
- // These values may be saved with the completed mesh
- abl_grid_points_x = parser.intval('X', GRID_MAX_POINTS_X);
- abl_grid_points_y = parser.intval('Y', GRID_MAX_POINTS_Y);
- if (parser.seenval('P')) abl_grid_points_x = abl_grid_points_y = parser.value_int();
-
- if (!WITHIN(abl_grid_points_x, 2, GRID_MAX_POINTS_X)) {
- SERIAL_ECHOLNPGM("?Probe points (X) is implausible (2-" STRINGIFY(GRID_MAX_POINTS_X) ").");
- G29_RETURN(false);
- }
- if (!WITHIN(abl_grid_points_y, 2, GRID_MAX_POINTS_Y)) {
- SERIAL_ECHOLNPGM("?Probe points (Y) is implausible (2-" STRINGIFY(GRID_MAX_POINTS_Y) ").");
- G29_RETURN(false);
- }
-
- abl_points = abl_grid_points_x * abl_grid_points_y;
- mean = 0;
-
- #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
-
- zoffset = parser.linearval('Z');
-
- #endif
-
- #if ABL_GRID
-
- xy_probe_feedrate_mm_s = MMM_TO_MMS(parser.linearval('S', XY_PROBE_SPEED));
-
- if (parser.seen('H')) {
- const int16_t size = (int16_t)parser.value_linear_units();
- left_probe_bed_position = MAX(X_CENTER - size / 2, MIN_PROBE_X);
- right_probe_bed_position = MIN(left_probe_bed_position + size, MAX_PROBE_X);
- front_probe_bed_position = MAX(Y_CENTER - size / 2, MIN_PROBE_Y);
- back_probe_bed_position = MIN(front_probe_bed_position + size, MAX_PROBE_Y);
- }
- else {
- left_probe_bed_position = parser.seenval('L') ? (int)RAW_X_POSITION(parser.value_linear_units()) : LEFT_PROBE_BED_POSITION;
- right_probe_bed_position = parser.seenval('R') ? (int)RAW_X_POSITION(parser.value_linear_units()) : RIGHT_PROBE_BED_POSITION;
- front_probe_bed_position = parser.seenval('F') ? (int)RAW_Y_POSITION(parser.value_linear_units()) : FRONT_PROBE_BED_POSITION;
- back_probe_bed_position = parser.seenval('B') ? (int)RAW_Y_POSITION(parser.value_linear_units()) : BACK_PROBE_BED_POSITION;
- }
-
- if (
- #if IS_SCARA || ENABLED(DELTA)
- !position_is_reachable_by_probe(left_probe_bed_position, 0)
- || !position_is_reachable_by_probe(right_probe_bed_position, 0)
- || !position_is_reachable_by_probe(0, front_probe_bed_position)
- || !position_is_reachable_by_probe(0, back_probe_bed_position)
- #else
- !position_is_reachable_by_probe(left_probe_bed_position, front_probe_bed_position)
- || !position_is_reachable_by_probe(right_probe_bed_position, back_probe_bed_position)
- #endif
- ) {
- SERIAL_ECHOLNPGM("? (L,R,F,B) out of bounds.");
- G29_RETURN(false);
- }
-
- // probe at the points of a lattice grid
- xGridSpacing = (right_probe_bed_position - left_probe_bed_position) / (abl_grid_points_x - 1);
- yGridSpacing = (back_probe_bed_position - front_probe_bed_position) / (abl_grid_points_y - 1);
-
- #endif // ABL_GRID
-
- if (verbose_level > 0) {
- SERIAL_ECHOPGM("G29 Auto Bed Leveling");
- if (dryrun) SERIAL_ECHOPGM(" (DRYRUN)");
- SERIAL_EOL();
- }
-
- planner.synchronize();
-
- // Disable auto bed leveling during G29.
- // Be formal so G29 can be done successively without G28.
- if (!no_action) set_bed_leveling_enabled(false);
-
- #if HAS_BED_PROBE
- // Deploy the probe. Probe will raise if needed.
- if (DEPLOY_PROBE()) {
- set_bed_leveling_enabled(abl_should_enable);
- G29_RETURN(false);
- }
- #endif
-
- if (!faux) setup_for_endstop_or_probe_move();
-
- #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
-
- #if ENABLED(PROBE_MANUALLY)
- if (!no_action)
- #endif
- if ( xGridSpacing != bilinear_grid_spacing[X_AXIS]
- || yGridSpacing != bilinear_grid_spacing[Y_AXIS]
- || left_probe_bed_position != bilinear_start[X_AXIS]
- || front_probe_bed_position != bilinear_start[Y_AXIS]
- ) {
- // Reset grid to 0.0 or "not probed". (Also disables ABL)
- reset_bed_level();
-
- // Initialize a grid with the given dimensions
- bilinear_grid_spacing[X_AXIS] = xGridSpacing;
- bilinear_grid_spacing[Y_AXIS] = yGridSpacing;
- bilinear_start[X_AXIS] = left_probe_bed_position;
- bilinear_start[Y_AXIS] = front_probe_bed_position;
-
- // Can't re-enable (on error) until the new grid is written
- abl_should_enable = false;
- }
-
- #endif // AUTO_BED_LEVELING_BILINEAR
-
- #if ENABLED(AUTO_BED_LEVELING_3POINT)
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("> 3-point Leveling");
- #endif
-
- // Probe at 3 arbitrary points
- points[0].z = points[1].z = points[2].z = 0;
-
- #endif // AUTO_BED_LEVELING_3POINT
-
- } // !g29_in_progress
-
- #if ENABLED(PROBE_MANUALLY)
-
- // For manual probing, get the next index to probe now.
- // On the first probe this will be incremented to 0.
- if (!no_action) {
- ++abl_probe_index;
- g29_in_progress = true;
- }
-
- // Abort current G29 procedure, go back to idle state
- if (seenA && g29_in_progress) {
- SERIAL_ECHOLNPGM("Manual G29 aborted");
- #if HAS_SOFTWARE_ENDSTOPS
- soft_endstops_enabled = enable_soft_endstops;
- #endif
- set_bed_leveling_enabled(abl_should_enable);
- g29_in_progress = false;
- #if ENABLED(LCD_BED_LEVELING)
- ui.wait_for_bl_move = false;
- #endif
- }
-
- // Query G29 status
- if (verbose_level || seenQ) {
- SERIAL_ECHOPGM("Manual G29 ");
- if (g29_in_progress) {
- SERIAL_ECHOPAIR("point ", MIN(abl_probe_index + 1, abl_points));
- SERIAL_ECHOLNPAIR(" of ", abl_points);
- }
- else
- SERIAL_ECHOLNPGM("idle");
- }
-
- if (no_action) G29_RETURN(false);
-
- if (abl_probe_index == 0) {
- // For the initial G29 S2 save software endstop state
- #if HAS_SOFTWARE_ENDSTOPS
- enable_soft_endstops = soft_endstops_enabled;
- #endif
- // Move close to the bed before the first point
- do_blocking_move_to_z(0);
- }
- else {
-
- #if ENABLED(AUTO_BED_LEVELING_LINEAR) || ENABLED(AUTO_BED_LEVELING_3POINT)
- const uint16_t index = abl_probe_index - 1;
- #endif
-
- // For G29 after adjusting Z.
- // Save the previous Z before going to the next point
- measured_z = current_position[Z_AXIS];
-
- #if ENABLED(AUTO_BED_LEVELING_LINEAR)
-
- mean += measured_z;
- eqnBVector[index] = measured_z;
- eqnAMatrix[index + 0 * abl_points] = xProbe;
- eqnAMatrix[index + 1 * abl_points] = yProbe;
- eqnAMatrix[index + 2 * abl_points] = 1;
-
- incremental_LSF(&lsf_results, xProbe, yProbe, measured_z);
-
- #elif ENABLED(AUTO_BED_LEVELING_3POINT)
-
- points[index].z = measured_z;
-
- #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
-
- z_values[xCount][yCount] = measured_z + zoffset;
- #if ENABLED(EXTENSIBLE_UI)
- ExtUI::onMeshUpdate(xCount, yCount, z_values[xCount][yCount]);
- #endif
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) {
- SERIAL_ECHOPAIR("Save X", xCount);
- SERIAL_ECHOPAIR(" Y", yCount);
- SERIAL_ECHOLNPAIR(" Z", measured_z + zoffset);
- }
- #endif
-
- #endif
- }
-
- //
- // If there's another point to sample, move there with optional lift.
- //
-
- #if ABL_GRID
-
- // Skip any unreachable points
- while (abl_probe_index < abl_points) {
-
- // Set xCount, yCount based on abl_probe_index, with zig-zag
- PR_OUTER_VAR = abl_probe_index / PR_INNER_END;
- PR_INNER_VAR = abl_probe_index - (PR_OUTER_VAR * PR_INNER_END);
-
- // Probe in reverse order for every other row/column
- bool zig = (PR_OUTER_VAR & 1); // != ((PR_OUTER_END) & 1);
-
- if (zig) PR_INNER_VAR = (PR_INNER_END - 1) - PR_INNER_VAR;
-
- const float xBase = xCount * xGridSpacing + left_probe_bed_position,
- yBase = yCount * yGridSpacing + front_probe_bed_position;
-
- xProbe = FLOOR(xBase + (xBase < 0 ? 0 : 0.5));
- yProbe = FLOOR(yBase + (yBase < 0 ? 0 : 0.5));
-
- #if ENABLED(AUTO_BED_LEVELING_LINEAR)
- indexIntoAB[xCount][yCount] = abl_probe_index;
- #endif
-
- // Keep looping till a reachable point is found
- if (position_is_reachable(xProbe, yProbe)) break;
- ++abl_probe_index;
- }
-
- // Is there a next point to move to?
- if (abl_probe_index < abl_points) {
- _manual_goto_xy(xProbe, yProbe); // Can be used here too!
- #if HAS_SOFTWARE_ENDSTOPS
- // Disable software endstops to allow manual adjustment
- // If G29 is not completed, they will not be re-enabled
- soft_endstops_enabled = false;
- #endif
- G29_RETURN(false);
- }
- else {
-
- // Leveling done! Fall through to G29 finishing code below
-
- SERIAL_ECHOLNPGM("Grid probing done.");
-
- // Re-enable software endstops, if needed
- #if HAS_SOFTWARE_ENDSTOPS
- soft_endstops_enabled = enable_soft_endstops;
- #endif
- }
-
- #elif ENABLED(AUTO_BED_LEVELING_3POINT)
-
- // Probe at 3 arbitrary points
- if (abl_probe_index < abl_points) {
- xProbe = points[abl_probe_index].x;
- yProbe = points[abl_probe_index].y;
- _manual_goto_xy(xProbe, yProbe);
- #if HAS_SOFTWARE_ENDSTOPS
- // Disable software endstops to allow manual adjustment
- // If G29 is not completed, they will not be re-enabled
- soft_endstops_enabled = false;
- #endif
- G29_RETURN(false);
- }
- else {
-
- SERIAL_ECHOLNPGM("3-point probing done.");
-
- // Re-enable software endstops, if needed
- #if HAS_SOFTWARE_ENDSTOPS
- soft_endstops_enabled = enable_soft_endstops;
- #endif
-
- if (!dryrun) {
- vector_3 planeNormal = vector_3::cross(points[0] - points[1], points[2] - points[1]).get_normal();
- if (planeNormal.z < 0) {
- planeNormal.x *= -1;
- planeNormal.y *= -1;
- planeNormal.z *= -1;
- }
- planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
-
- // Can't re-enable (on error) until the new grid is written
- abl_should_enable = false;
- }
-
- }
-
- #endif // AUTO_BED_LEVELING_3POINT
-
- #else // !PROBE_MANUALLY
- {
- const ProbePtRaise raise_after = parser.boolval('E') ? PROBE_PT_STOW : PROBE_PT_RAISE;
-
- measured_z = 0;
-
- #if ABL_GRID
-
- bool zig = PR_OUTER_END & 1; // Always end at RIGHT and BACK_PROBE_BED_POSITION
-
- measured_z = 0;
-
- // Outer loop is Y with PROBE_Y_FIRST disabled
- for (uint8_t PR_OUTER_VAR = 0; PR_OUTER_VAR < PR_OUTER_END && !isnan(measured_z); PR_OUTER_VAR++) {
-
- int8_t inStart, inStop, inInc;
-
- if (zig) { // away from origin
- inStart = 0;
- inStop = PR_INNER_END;
- inInc = 1;
- }
- else { // towards origin
- inStart = PR_INNER_END - 1;
- inStop = -1;
- inInc = -1;
- }
-
- zig ^= true; // zag
-
- // Inner loop is Y with PROBE_Y_FIRST enabled
- for (int8_t PR_INNER_VAR = inStart; PR_INNER_VAR != inStop; PR_INNER_VAR += inInc) {
-
- const float xBase = left_probe_bed_position + xGridSpacing * xCount,
- yBase = front_probe_bed_position + yGridSpacing * yCount;
-
- xProbe = FLOOR(xBase + (xBase < 0 ? 0 : 0.5));
- yProbe = FLOOR(yBase + (yBase < 0 ? 0 : 0.5));
-
- #if ENABLED(AUTO_BED_LEVELING_LINEAR)
- indexIntoAB[xCount][yCount] = ++abl_probe_index; // 0...
- #endif
-
- #if IS_KINEMATIC
- // Avoid probing outside the round or hexagonal area
- if (!position_is_reachable_by_probe(xProbe, yProbe)) continue;
- #endif
-
- measured_z = faux ? 0.001 * random(-100, 101) : probe_pt(xProbe, yProbe, raise_after, verbose_level);
-
- if (isnan(measured_z)) {
- set_bed_leveling_enabled(abl_should_enable);
- break;
- }
-
- #if ENABLED(AUTO_BED_LEVELING_LINEAR)
-
- mean += measured_z;
- eqnBVector[abl_probe_index] = measured_z;
- eqnAMatrix[abl_probe_index + 0 * abl_points] = xProbe;
- eqnAMatrix[abl_probe_index + 1 * abl_points] = yProbe;
- eqnAMatrix[abl_probe_index + 2 * abl_points] = 1;
-
- incremental_LSF(&lsf_results, xProbe, yProbe, measured_z);
-
- #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
-
- z_values[xCount][yCount] = measured_z + zoffset;
- #if ENABLED(EXTENSIBLE_UI)
- ExtUI::onMeshUpdate(xCount, yCount, z_values[xCount][yCount]);
- #endif
-
- #endif
-
- abl_should_enable = false;
- idle();
-
- } // inner
- } // outer
-
- #elif ENABLED(AUTO_BED_LEVELING_3POINT)
-
- // Probe at 3 arbitrary points
-
- for (uint8_t i = 0; i < 3; ++i) {
- // Retain the last probe position
- xProbe = points[i].x;
- yProbe = points[i].y;
- measured_z = faux ? 0.001 * random(-100, 101) : probe_pt(xProbe, yProbe, raise_after, verbose_level);
- if (isnan(measured_z)) {
- set_bed_leveling_enabled(abl_should_enable);
- break;
- }
- points[i].z = measured_z;
- }
-
- if (!dryrun && !isnan(measured_z)) {
- vector_3 planeNormal = vector_3::cross(points[0] - points[1], points[2] - points[1]).get_normal();
- if (planeNormal.z < 0) {
- planeNormal.x *= -1;
- planeNormal.y *= -1;
- planeNormal.z *= -1;
- }
- planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
-
- // Can't re-enable (on error) until the new grid is written
- abl_should_enable = false;
- }
-
- #endif // AUTO_BED_LEVELING_3POINT
-
- // Stow the probe. No raise for FIX_MOUNTED_PROBE.
- if (STOW_PROBE()) {
- set_bed_leveling_enabled(abl_should_enable);
- measured_z = NAN;
- }
- }
- #endif // !PROBE_MANUALLY
-
- //
- // G29 Finishing Code
- //
- // Unless this is a dry run, auto bed leveling will
- // definitely be enabled after this point.
- //
- // If code above wants to continue leveling, it should
- // return or loop before this point.
- //
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) DEBUG_POS("> probing complete", current_position);
- #endif
-
- #if ENABLED(PROBE_MANUALLY)
- g29_in_progress = false;
- #if ENABLED(LCD_BED_LEVELING)
- ui.wait_for_bl_move = false;
- #endif
- #endif
-
- // Calculate leveling, print reports, correct the position
- if (!isnan(measured_z)) {
- #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
-
- if (!dryrun) extrapolate_unprobed_bed_level();
- print_bilinear_leveling_grid();
-
- refresh_bed_level();
-
- #if ENABLED(ABL_BILINEAR_SUBDIVISION)
- print_bilinear_leveling_grid_virt();
- #endif
-
- #elif ENABLED(AUTO_BED_LEVELING_LINEAR)
-
- // For LINEAR leveling calculate matrix, print reports, correct the position
-
- /**
- * solve the plane equation ax + by + d = z
- * A is the matrix with rows [x y 1] for all the probed points
- * B is the vector of the Z positions
- * the normal vector to the plane is formed by the coefficients of the
- * plane equation in the standard form, which is Vx*x+Vy*y+Vz*z+d = 0
- * so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z
- */
- float plane_equation_coefficients[3];
-
- finish_incremental_LSF(&lsf_results);
- plane_equation_coefficients[0] = -lsf_results.A; // We should be able to eliminate the '-' on these three lines and down below
- plane_equation_coefficients[1] = -lsf_results.B; // but that is not yet tested.
- plane_equation_coefficients[2] = -lsf_results.D;
-
- mean /= abl_points;
-
- if (verbose_level) {
- SERIAL_ECHOPAIR_F("Eqn coefficients: a: ", plane_equation_coefficients[0], 8);
- SERIAL_ECHOPAIR_F(" b: ", plane_equation_coefficients[1], 8);
- SERIAL_ECHOPAIR_F(" d: ", plane_equation_coefficients[2], 8);
- if (verbose_level > 2)
- SERIAL_ECHOPAIR_F("\nMean of sampled points: ", mean, 8);
- SERIAL_EOL();
- }
-
- // Create the matrix but don't correct the position yet
- if (!dryrun)
- planner.bed_level_matrix = matrix_3x3::create_look_at(
- vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1) // We can eliminate the '-' here and up above
- );
-
- // Show the Topography map if enabled
- if (do_topography_map) {
-
- SERIAL_ECHOLNPGM("\nBed Height Topography:\n"
- " +--- BACK --+\n"
- " | |\n"
- " L | (+) | R\n"
- " E | | I\n"
- " F | (-) N (+) | G\n"
- " T | | H\n"
- " | (-) | T\n"
- " | |\n"
- " O-- FRONT --+\n"
- " (0,0)");
-
- float min_diff = 999;
-
- for (int8_t yy = abl_grid_points_y - 1; yy >= 0; yy--) {
- for (uint8_t xx = 0; xx < abl_grid_points_x; xx++) {
- int ind = indexIntoAB[xx][yy];
- float diff = eqnBVector[ind] - mean,
- x_tmp = eqnAMatrix[ind + 0 * abl_points],
- y_tmp = eqnAMatrix[ind + 1 * abl_points],
- z_tmp = 0;
-
- apply_rotation_xyz(planner.bed_level_matrix, x_tmp, y_tmp, z_tmp);
-
- NOMORE(min_diff, eqnBVector[ind] - z_tmp);
-
- if (diff >= 0.0)
- SERIAL_ECHOPGM(" +"); // Include + for column alignment
- else
- SERIAL_CHAR(' ');
- SERIAL_ECHO_F(diff, 5);
- } // xx
- SERIAL_EOL();
- } // yy
- SERIAL_EOL();
-
- if (verbose_level > 3) {
- SERIAL_ECHOLNPGM("\nCorrected Bed Height vs. Bed Topology:");
-
- for (int8_t yy = abl_grid_points_y - 1; yy >= 0; yy--) {
- for (uint8_t xx = 0; xx < abl_grid_points_x; xx++) {
- int ind = indexIntoAB[xx][yy];
- float x_tmp = eqnAMatrix[ind + 0 * abl_points],
- y_tmp = eqnAMatrix[ind + 1 * abl_points],
- z_tmp = 0;
-
- apply_rotation_xyz(planner.bed_level_matrix, x_tmp, y_tmp, z_tmp);
-
- float diff = eqnBVector[ind] - z_tmp - min_diff;
- if (diff >= 0.0)
- SERIAL_ECHOPGM(" +");
- // Include + for column alignment
- else
- SERIAL_CHAR(' ');
- SERIAL_ECHO_F(diff, 5);
- } // xx
- SERIAL_EOL();
- } // yy
- SERIAL_EOL();
- }
- } //do_topography_map
-
- #endif // AUTO_BED_LEVELING_LINEAR
-
- #if ABL_PLANAR
-
- // For LINEAR and 3POINT leveling correct the current position
-
- if (verbose_level > 0)
- planner.bed_level_matrix.debug(PSTR("\n\nBed Level Correction Matrix:"));
-
- if (!dryrun) {
- //
- // Correct the current XYZ position based on the tilted plane.
- //
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) DEBUG_POS("G29 uncorrected XYZ", current_position);
- #endif
-
- float converted[XYZ];
- COPY(converted, current_position);
-
- planner.leveling_active = true;
- planner.unapply_leveling(converted); // use conversion machinery
- planner.leveling_active = false;
-
- // Use the last measured distance to the bed, if possible
- if ( NEAR(current_position[X_AXIS], xProbe - (X_PROBE_OFFSET_FROM_EXTRUDER))
- && NEAR(current_position[Y_AXIS], yProbe - (Y_PROBE_OFFSET_FROM_EXTRUDER))
- ) {
- const float simple_z = current_position[Z_AXIS] - measured_z;
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) {
- SERIAL_ECHOPAIR("Z from Probe:", simple_z);
- SERIAL_ECHOPAIR(" Matrix:", converted[Z_AXIS]);
- SERIAL_ECHOLNPAIR(" Discrepancy:", simple_z - converted[Z_AXIS]);
- }
- #endif
- converted[Z_AXIS] = simple_z;
- }
-
- // The rotated XY and corrected Z are now current_position
- COPY(current_position, converted);
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) DEBUG_POS("G29 corrected XYZ", current_position);
- #endif
- }
-
- #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
-
- if (!dryrun) {
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPAIR("G29 uncorrected Z:", current_position[Z_AXIS]);
- #endif
-
- // Unapply the offset because it is going to be immediately applied
- // and cause compensation movement in Z
- current_position[Z_AXIS] -= bilinear_z_offset(current_position);
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPAIR(" corrected Z:", current_position[Z_AXIS]);
- #endif
- }
-
- #endif // ABL_PLANAR
-
- #ifdef Z_PROBE_END_SCRIPT
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPAIR("Z Probe End Script: ", Z_PROBE_END_SCRIPT);
- #endif
- planner.synchronize();
- enqueue_and_echo_commands_P(PSTR(Z_PROBE_END_SCRIPT));
- #endif
-
- // Auto Bed Leveling is complete! Enable if possible.
- planner.leveling_active = dryrun ? abl_should_enable : true;
- } // !isnan(measured_z)
-
- // Restore state after probing
- if (!faux) clean_up_after_endstop_or_probe_move();
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("<<< G29");
- #endif
-
- KEEPALIVE_STATE(IN_HANDLER);
-
- if (planner.leveling_active)
- sync_plan_position();
-
- #if HAS_BED_PROBE && defined(Z_AFTER_PROBING)
- move_z_after_probing();
- #endif
-
- report_current_position();
-
- G29_RETURN(isnan(measured_z));
- }
-
- #endif // HAS_ABL_NOT_UBL
|