From e58d1bf97405f89d0a720c90a63ad677ca0624b5 Mon Sep 17 00:00:00 2001 From: InsanityAutomation <38436470+InsanityAutomation@users.noreply.github.com> Date: Sat, 1 Feb 2020 05:50:44 -0500 Subject: [PATCH] G34 automatic point assignment (#16473) --- Marlin/Configuration_adv.h | 53 ++++++--- Marlin/src/feature/z_stepper_align.cpp | 137 ++++++++++++++++++++++ Marlin/src/feature/z_stepper_align.h | 41 +++++++ Marlin/src/gcode/bedlevel/G26.cpp | 2 +- Marlin/src/gcode/calibrate/G34_M422.cpp | 132 ++++++++------------- Marlin/src/module/configuration_store.cpp | 38 +++++- 6 files changed, 298 insertions(+), 105 deletions(-) create mode 100644 Marlin/src/feature/z_stepper_align.cpp create mode 100644 Marlin/src/feature/z_stepper_align.h diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index e2bda3f80..9a979f252 100644 --- a/Marlin/Configuration_adv.h +++ b/Marlin/Configuration_adv.h @@ -668,11 +668,37 @@ */ //#define Z_STEPPER_AUTO_ALIGN #if ENABLED(Z_STEPPER_AUTO_ALIGN) - // Define probe X and Y positions for Z1, Z2 [, Z3] - #define Z_STEPPER_ALIGN_XY { { 10, 190 }, { 100, 10 }, { 190, 190 } } + // Define probe X and Y positions for Z1, Z2 [, Z3 [, Z4]] + // If not defined, probe limits will be used. + // Override with 'M422 S X Y' + //#define Z_STEPPER_ALIGN_XY { { 10, 190 }, { 100, 10 }, { 190, 190 } } + + /** + * Orientation for the automatically-calculated probe positions. + * Override Z stepper align points with 'M422 S X Y' + * + * 2 Steppers: (0) (1) + * | | 2 | + * | 1 2 | | + * | | 1 | + * + * 3 Steppers: (0) (1) (2) (3) + * | 3 | 1 | 2 1 | 2 | + * | | 3 | | 3 | + * | 1 2 | 2 | 3 | 1 | + * + * 4 Steppers: (0) (1) (2) (3) + * | 4 3 | 1 4 | 2 1 | 3 2 | + * | | | | | + * | 1 2 | 2 3 | 3 4 | 4 1 | + * + */ + #ifndef Z_STEPPER_ALIGN_XY + //#define Z_STEPPERS_ORIENTATION 0 + #endif // Provide Z stepper positions for more rapid convergence in bed alignment. - // Currently requires triple stepper drivers. + // Requires triple stepper drivers (i.e., set NUM_Z_STEPPER_DRIVERS to 3) //#define Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) // Define Stepper XY positions for Z1, Z2, Z3 corresponding to @@ -680,23 +706,16 @@ // Define one position per Z stepper in stepper driver order. #define Z_STEPPER_ALIGN_STEPPER_XY { { 210.7, 102.5 }, { 152.6, 220.0 }, { 94.5, 102.5 } } #else - // Amplification factor. Used to scale the correction step up or down. - // In case the stepper (spindle) position is further out than the test point. - // Use a value > 1. NOTE: This may cause instability - #define Z_STEPPER_ALIGN_AMP 1.0 + // Amplification factor. Used to scale the correction step up or down in case + // the stepper (spindle) position is farther out than the test point. + #define Z_STEPPER_ALIGN_AMP 1.0 // Use a value > 1.0 NOTE: This may cause instability! #endif - // Set number of iterations to align - #define Z_STEPPER_ALIGN_ITERATIONS 3 - - // Enable to restore leveling setup after operation - #define RESTORE_LEVELING_AFTER_G34 - // On a 300mm bed a 5% grade would give a misalignment of ~1.5cm - #define G34_MAX_GRADE 5 // (%) Maximum incline G34 will handle - - // Stop criterion. If the accuracy is better than this stop iterating early - #define Z_STEPPER_ALIGN_ACC 0.02 + #define G34_MAX_GRADE 5 // (%) Maximum incline that G34 will handle + #define Z_STEPPER_ALIGN_ITERATIONS 5 // Number of iterations to apply during alignment + #define Z_STEPPER_ALIGN_ACC 0.02 // Stop iterating early if the accuracy is better than this + #define RESTORE_LEVELING_AFTER_G34 // Restore leveling after G34 is done? #endif // @section motion diff --git a/Marlin/src/feature/z_stepper_align.cpp b/Marlin/src/feature/z_stepper_align.cpp new file mode 100644 index 000000000..0e2b7cef2 --- /dev/null +++ b/Marlin/src/feature/z_stepper_align.cpp @@ -0,0 +1,137 @@ +/** + * 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 . + * + */ + +/** + * feature/z_stepper_align.cpp + */ + +#include "../inc/MarlinConfigPre.h" + +#if ENABLED(Z_STEPPER_AUTO_ALIGN) + +#include "z_stepper_align.h" +#include "../module/probe.h" + +ZStepperAlign z_stepper_align; + +xy_pos_t ZStepperAlign::xy[NUM_Z_STEPPER_DRIVERS]; + +#if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) + xy_pos_t ZStepperAlign::stepper_xy[NUM_Z_STEPPER_DRIVERS]; +#endif + +void ZStepperAlign::reset_to_default() { + #ifdef Z_STEPPER_ALIGN_XY + + constexpr xy_pos_t xy_init[] = Z_STEPPER_ALIGN_XY; + static_assert(COUNT(xy_init) == NUM_Z_STEPPER_DRIVERS, + "Z_STEPPER_ALIGN_XY requires " + #if NUM_Z_STEPPER_DRIVERS == 4 + "four {X,Y} entries (Z, Z2, Z3, and Z4)." + #elif NUM_Z_STEPPER_DRIVERS == 3 + "three {X,Y} entries (Z, Z2, and Z3)." + #else + "two {X,Y} entries (Z and Z2)." + #endif + ); + + constexpr xyz_pos_t dpo = NOZZLE_TO_PROBE_OFFSET; + + #define LTEST(N) (xy_init[N].x >= _MAX(X_MIN_BED + MIN_PROBE_EDGE_LEFT, X_MIN_POS + dpo.x) - 0.00001f) + #define RTEST(N) (xy_init[N].x <= _MIN(X_MAX_BED - MIN_PROBE_EDGE_RIGHT, X_MAX_POS + dpo.x) + 0.00001f) + #define FTEST(N) (xy_init[N].y >= _MAX(Y_MIN_BED + MIN_PROBE_EDGE_FRONT, Y_MIN_POS + dpo.y) - 0.00001f) + #define BTEST(N) (xy_init[N].y <= _MIN(Y_MAX_BED - MIN_PROBE_EDGE_BACK, Y_MAX_POS + dpo.y) + 0.00001f) + + static_assert(LTEST(0) && RTEST(0), "The 1st Z_STEPPER_ALIGN_XY X is unreachable with the default probe X offset."); + static_assert(FTEST(0) && BTEST(0), "The 1st Z_STEPPER_ALIGN_XY Y is unreachable with the default probe Y offset."); + static_assert(LTEST(1) && RTEST(1), "The 2nd Z_STEPPER_ALIGN_XY X is unreachable with the default probe X offset."); + static_assert(FTEST(1) && BTEST(1), "The 2nd Z_STEPPER_ALIGN_XY Y is unreachable with the default probe Y offset."); + #if NUM_Z_STEPPER_DRIVERS >= 3 + static_assert(LTEST(2) && RTEST(2), "The 3rd Z_STEPPER_ALIGN_XY X is unreachable with the default probe X offset."); + static_assert(FTEST(2) && BTEST(2), "The 3rd Z_STEPPER_ALIGN_XY Y is unreachable with the default probe Y offset."); + #if NUM_Z_STEPPER_DRIVERS >= 4 + static_assert(LTEST(3) && RTEST(3), "The 4th Z_STEPPER_ALIGN_XY X is unreachable with the default probe X offset."); + static_assert(FTEST(3) && BTEST(3), "The 4th Z_STEPPER_ALIGN_XY Y is unreachable with the default probe Y offset."); + #endif + #endif + + #else // !defined(Z_STEPPER_ALIGN_XY) + + const xy_pos_t xy_init[] = { + #if NUM_Z_STEPPER_DRIVERS >= 3 // First probe point... + #if !Z_STEPPERS_ORIENTATION + { probe.min_x(), probe.min_y() }, // SW + #elif Z_STEPPERS_ORIENTATION == 1 + { probe.min_x(), probe.max_y() }, // NW + #elif Z_STEPPERS_ORIENTATION == 2 + { probe.max_x(), probe.max_y() }, // NE + #elif Z_STEPPERS_ORIENTATION == 3 + { probe.max_x(), probe.min_y() }, // SE + #else + #error "Z_STEPPERS_ORIENTATION must be from 0 to 3 (first point SW, NW, NE, SE)." + #endif + #if NUM_Z_STEPPER_DRIVERS == 4 // 3 more points... + #if !Z_STEPPERS_ORIENTATION + { probe.min_x(), probe.max_y() }, { probe.max_x(), probe.max_y() }, { probe.max_x(), probe.min_y() } // SW + #elif Z_STEPPERS_ORIENTATION == 1 + { probe.max_x(), probe.max_y() }, { probe.max_x(), probe.min_y() }, { probe.min_x(), probe.min_y() } // NW + #elif Z_STEPPERS_ORIENTATION == 2 + { probe.max_x(), probe.min_y() }, { probe.min_x(), probe.min_y() }, { probe.min_x(), probe.max_y() } // NE + #elif Z_STEPPERS_ORIENTATION == 3 + { probe.min_x(), probe.min_y() }, { probe.min_x(), probe.max_y() }, { probe.max_x(), probe.max_y() } // SE + #endif + #elif !Z_STEPPERS_ORIENTATION // or 2 more points... + { probe.max_x(), probe.min_y() }, { X_CENTER, probe.max_y() } // SW + #elif Z_STEPPERS_ORIENTATION == 1 + { probe.min_x(), probe.min_y() }, { probe.max_x(), Y_CENTER } // NW + #elif Z_STEPPERS_ORIENTATION == 2 + { probe.min_x(), probe.max_y() }, { X_CENTER, probe.min_y() } // NE + #elif Z_STEPPERS_ORIENTATION == 3 + { probe.max_x(), probe.max_y() }, { probe.min_x(), Y_CENTER } // SE + #endif + #elif Z_STEPPERS_ORIENTATION + { X_CENTER, probe.min_y() }, { X_CENTER, probe.max_y() } + #else + { probe.min_x(), Y_CENTER }, { probe.max_x(), Y_CENTER } + #endif + }; + + #endif // !defined(Z_STEPPER_ALIGN_XY) + + COPY(xy, xy_init); + + #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) + constexpr xy_pos_t stepper_xy_init[] = Z_STEPPER_ALIGN_STEPPER_XY; + static_assert( + COUNT(stepper_xy_init) == NUM_Z_STEPPER_DRIVERS, + "Z_STEPPER_ALIGN_STEPPER_XY requires " + #if NUM_Z_STEPPER_DRIVERS == 4 + "four {X,Y} entries (Z, Z2, Z3, and Z4)." + #elif NUM_Z_STEPPER_DRIVERS == 3 + "three {X,Y} entries (Z, Z2, and Z3)." + #endif + ); + COPY(stepper_xy, stepper_xy_init); + #endif +} + +#endif // Z_STEPPER_AUTO_ALIGN diff --git a/Marlin/src/feature/z_stepper_align.h b/Marlin/src/feature/z_stepper_align.h new file mode 100644 index 000000000..8b31ac4bd --- /dev/null +++ b/Marlin/src/feature/z_stepper_align.h @@ -0,0 +1,41 @@ +/** + * 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 . + * + */ +#pragma once + +/** + * feature/z_stepper_align.h + */ + +#include "../inc/MarlinConfig.h" + +class ZStepperAlign { + public: + static xy_pos_t xy[NUM_Z_STEPPER_DRIVERS]; + + #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) + static xy_pos_t stepper_xy[NUM_Z_STEPPER_DRIVERS]; + #endif + + static void reset_to_default(); +}; + +extern ZStepperAlign z_stepper_align; diff --git a/Marlin/src/gcode/bedlevel/G26.cpp b/Marlin/src/gcode/bedlevel/G26.cpp index 611538a90..7b580b666 100644 --- a/Marlin/src/gcode/bedlevel/G26.cpp +++ b/Marlin/src/gcode/bedlevel/G26.cpp @@ -707,7 +707,7 @@ void GcodeSuite::G26() { if (location.valid()) { const xy_pos_t circle = _GET_MESH_POS(location.pos); - // If this mesh location is outside the printable_radius, skip it. + // If this mesh location is outside the printable radius, skip it. if (!position_is_reachable(circle)) continue; // Determine where to start and end the circle, diff --git a/Marlin/src/gcode/calibrate/G34_M422.cpp b/Marlin/src/gcode/calibrate/G34_M422.cpp index a35b3faef..4532da8f8 100644 --- a/Marlin/src/gcode/calibrate/G34_M422.cpp +++ b/Marlin/src/gcode/calibrate/G34_M422.cpp @@ -24,6 +24,8 @@ #if ENABLED(Z_STEPPER_AUTO_ALIGN) +#include "../../feature/z_stepper_align.h" + #include "../gcode.h" #include "../../module/planner.h" #include "../../module/stepper.h" @@ -45,68 +47,6 @@ #define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE) #include "../../core/debug_out.h" -// -// Sanity check G34 / M422 settings -// -constexpr xy_pos_t test_z_stepper_align_xy[] = Z_STEPPER_ALIGN_XY; - -#if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) - - static_assert(COUNT(test_z_stepper_align_xy) >= NUM_Z_STEPPER_DRIVERS, - "Z_STEPPER_ALIGN_XY requires at least three {X,Y} entries (Z, Z2, Z3, ...)." - ); - - constexpr float test_z_stepper_align_stepper_xy[][XY] = Z_STEPPER_ALIGN_STEPPER_XY; - static_assert( - COUNT(test_z_stepper_align_stepper_xy) == NUM_Z_STEPPER_DRIVERS, - "Z_STEPPER_ALIGN_STEPPER_XY requires three {X,Y} entries (one per Z stepper)." - ); - -#else - - static_assert(COUNT(test_z_stepper_align_xy) == NUM_Z_STEPPER_DRIVERS, - #if NUM_Z_STEPPER_DRIVERS == 4 - "Z_STEPPER_ALIGN_XY requires four {X,Y} entries (Z, Z2, Z3, and Z4)." - #elif NUM_Z_STEPPER_DRIVERS == 3 - "Z_STEPPER_ALIGN_XY requires three {X,Y} entries (Z, Z2, and Z3)." - #else - "Z_STEPPER_ALIGN_XY requires two {X,Y} entries (Z and Z2)." - #endif - ); - -#endif - -constexpr xyz_pos_t dpo = NOZZLE_TO_PROBE_OFFSET; - -#define LTEST(N) (test_z_stepper_align_xy[N].x >= _MAX(X_MIN_BED + MIN_PROBE_EDGE_LEFT, X_MIN_POS + dpo.x) - 0.00001f) -#define RTEST(N) (test_z_stepper_align_xy[N].x <= _MIN(X_MAX_BED - MIN_PROBE_EDGE_RIGHT, X_MAX_POS + dpo.x) + 0.00001f) -#define FTEST(N) (test_z_stepper_align_xy[N].y >= _MAX(Y_MIN_BED + MIN_PROBE_EDGE_FRONT, Y_MIN_POS + dpo.y) - 0.00001f) -#define BTEST(N) (test_z_stepper_align_xy[N].y <= _MIN(Y_MAX_BED - MIN_PROBE_EDGE_BACK, Y_MAX_POS + dpo.y) + 0.00001f) - -static_assert(LTEST(0) && RTEST(0), "The 1st Z_STEPPER_ALIGN_XY X is unreachable with the default probe X offset."); -static_assert(FTEST(0) && BTEST(0), "The 1st Z_STEPPER_ALIGN_XY Y is unreachable with the default probe Y offset."); -static_assert(LTEST(1) && RTEST(1), "The 2nd Z_STEPPER_ALIGN_XY X is unreachable with the default probe X offset."); -static_assert(FTEST(1) && BTEST(1), "The 2nd Z_STEPPER_ALIGN_XY Y is unreachable with the default probe Y offset."); -#if NUM_Z_STEPPER_DRIVERS >= 3 - static_assert(LTEST(2) && RTEST(2), "The 3rd Z_STEPPER_ALIGN_XY X is unreachable with the default probe X offset."); - static_assert(FTEST(2) && BTEST(2), "The 3rd Z_STEPPER_ALIGN_XY Y is unreachable with the default probe Y offset."); - #if NUM_Z_STEPPER_DRIVERS >= 4 - static_assert(LTEST(3) && RTEST(3), "The 4th Z_STEPPER_ALIGN_XY X is unreachable with the default probe X offset."); - static_assert(FTEST(3) && BTEST(3), "The 4th Z_STEPPER_ALIGN_XY Y is unreachable with the default probe Y offset."); - #endif -#endif - -// -// G34 / M422 shared data -// -static xy_pos_t z_stepper_align_pos[] = Z_STEPPER_ALIGN_XY; - -#if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) - static xy_pos_t z_stepper_align_stepper_pos[] = Z_STEPPER_ALIGN_STEPPER_XY; -#endif - -#define G34_PROBE_COUNT COUNT(z_stepper_align_pos) - inline void set_all_z_lock(const bool lock) { stepper.set_z_lock(lock); stepper.set_z2_lock(lock); @@ -201,11 +141,11 @@ void GcodeSuite::G34() { // iteration this will be re-calculated based on the actual bed position float z_probe = Z_BASIC_CLEARANCE + (G34_MAX_GRADE) * 0.01f * ( #if NUM_Z_STEPPER_DRIVERS == 3 - SQRT(_MAX(HYPOT2(z_stepper_align_pos[0].x - z_stepper_align_pos[0].y, z_stepper_align_pos[1].x - z_stepper_align_pos[1].y), - HYPOT2(z_stepper_align_pos[1].x - z_stepper_align_pos[1].y, z_stepper_align_pos[2].x - z_stepper_align_pos[2].y), - HYPOT2(z_stepper_align_pos[2].x - z_stepper_align_pos[2].y, z_stepper_align_pos[0].x - z_stepper_align_pos[0].y))) + SQRT(_MAX(HYPOT2(z_stepper_align.xy[0].x - z_stepper_align.xy[0].y, z_stepper_align.xy[1].x - z_stepper_align.xy[1].y), + HYPOT2(z_stepper_align.xy[1].x - z_stepper_align.xy[1].y, z_stepper_align.xy[2].x - z_stepper_align.xy[2].y), + HYPOT2(z_stepper_align.xy[2].x - z_stepper_align.xy[2].y, z_stepper_align.xy[0].x - z_stepper_align.xy[0].y))) #else - HYPOT(z_stepper_align_pos[0].x - z_stepper_align_pos[0].y, z_stepper_align_pos[1].x - z_stepper_align_pos[1].y) + HYPOT(z_stepper_align.xy[0].x - z_stepper_align.xy[0].y, z_stepper_align.xy[1].x - z_stepper_align.xy[1].y) #endif ); @@ -216,31 +156,39 @@ void GcodeSuite::G34() { current_position.z -= z_probe * 0.5f; float last_z_align_move[NUM_Z_STEPPER_DRIVERS] = ARRAY_N(NUM_Z_STEPPER_DRIVERS, 10000.0f, 10000.0f, 10000.0f), - z_measured[G34_PROBE_COUNT] = { 0 }, + z_measured[NUM_Z_STEPPER_DRIVERS] = { 0 }, z_maxdiff = 0.0f, amplification = z_auto_align_amplification; uint8_t iteration; bool err_break = false; + + #if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) + bool adjustment_reverse = false; + #endif + for (iteration = 0; iteration < z_auto_align_iterations; ++iteration) { if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> probing all positions."); SERIAL_ECHOLNPAIR("\nITERATION: ", int(iteration + 1)); // Initialize minimum value - float z_measured_min = 100000.0f, + float z_measured_min = 100000.0f, z_measured_max = -100000.0f; // Probe all positions (one per Z-Stepper) - for (uint8_t i = 0; i < G34_PROBE_COUNT; ++i) { + for (uint8_t i = 0; i < NUM_Z_STEPPER_DRIVERS; ++i) { // iteration odd/even --> downward / upward stepper sequence - const uint8_t iprobe = (iteration & 1) ? G34_PROBE_COUNT - 1 - i : i; + const uint8_t iprobe = (iteration & 1) ? NUM_Z_STEPPER_DRIVERS - 1 - i : i; // Safe clearance even on an incline if (iteration == 0 || i > 0) do_blocking_move_to_z(z_probe); + if (DEBUGGING(LEVELING)) + DEBUG_ECHOLNPAIR_P(PSTR("Probing X"), z_stepper_align.xy[iprobe].x, SP_Y_STR, z_stepper_align.xy[iprobe].y); + // Probe a Z height for each stepper. - const float z_probed_height = probe.probe_at_point(z_stepper_align_pos[iprobe], raise_after, 0, true); + const float z_probed_height = probe.probe_at_point(z_stepper_align.xy[iprobe], raise_after, 0, true); if (isnan(z_probed_height)) { SERIAL_ECHOLNPGM("Probing failed."); err_break = true; @@ -279,15 +227,15 @@ void GcodeSuite::G34() { // This allows the actual adjustment logic to be shared by both algorithms. linear_fit_data lfd; incremental_LSF_reset(&lfd); - for (uint8_t i = 0; i < G34_PROBE_COUNT; ++i) { - SERIAL_ECHOLNPAIR("PROBEPT_", int(i + 1), ": ", z_measured[i]); - incremental_LSF(&lfd, z_stepper_align_pos[i], z_measured[i]); + for (uint8_t i = 0; i < NUM_Z_STEPPER_DRIVERS; ++i) { + SERIAL_ECHOLNPAIR("PROBEPT_", i + '1', ": ", z_measured[i]); + incremental_LSF(&lfd, z_stepper_align.xy[i], z_measured[i]); } finish_incremental_LSF(&lfd); z_measured_min = 100000.0f; for (uint8_t i = 0; i < NUM_Z_STEPPER_DRIVERS; ++i) { - z_measured[i] = -(lfd.A * z_stepper_align_stepper_pos[i].x + lfd.B * z_stepper_align_stepper_pos[i].y); + z_measured[i] = -(lfd.A * z_stepper_align.stepper_xy[i].x + lfd.B * z_stepper_align.stepper_xy[i].y); z_measured_min = _MIN(z_measured_min, z_measured[i]); } @@ -309,8 +257,8 @@ void GcodeSuite::G34() { // Correct the individual stepper offsets for (uint8_t zstepper = 0; zstepper < NUM_Z_STEPPER_DRIVERS; ++zstepper) { // Calculate current stepper move - const float z_align_move = z_measured[zstepper] - z_measured_min, - z_align_abs = ABS(z_align_move); + float z_align_move = z_measured[zstepper] - z_measured_min; + const float z_align_abs = ABS(z_align_move); #if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) // Optimize one iteration's correction based on the first measurements @@ -318,10 +266,14 @@ void GcodeSuite::G34() { #endif // Check for less accuracy compared to last move - if (last_z_align_move[zstepper] < z_align_abs - 1.0) { + if (last_z_align_move[zstepper] < z_align_abs * 0.7f) { SERIAL_ECHOLNPGM("Decreasing accuracy detected."); - err_break = true; - break; + #if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) + adjustment_reverse = !adjustment_reverse; + #else + err_break = true; + break; + #endif } // Remember the alignment for the next iteration @@ -342,6 +294,13 @@ void GcodeSuite::G34() { #endif } + #if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) + // Decreasing accuracy was detected so move was inverted. + // Will match reversed Z steppers on dual steppers. Triple will need more work to map. + if (adjustment_reverse) + z_align_move = -z_align_move; + #endif + // Do a move to correct part of the misalignment for the current stepper do_blocking_move_to_z(amplification * z_align_move + current_position.z); } // for (zstepper) @@ -406,12 +365,13 @@ void GcodeSuite::G34() { * Y : Y position to set (Unchanged if omitted) */ void GcodeSuite::M422() { + if (!parser.seen_any()) { - for (uint8_t i = 0; i < G34_PROBE_COUNT; ++i) - SERIAL_ECHOLNPAIR_P(PSTR("M422 S"), i + 1, SP_X_STR, z_stepper_align_pos[i].x, SP_Y_STR, z_stepper_align_pos[i].y); + for (uint8_t i = 0; i < NUM_Z_STEPPER_DRIVERS; ++i) + SERIAL_ECHOLNPAIR_P(PSTR("M422 S"), i + '1', SP_X_STR, z_stepper_align.xy[i].x, SP_Y_STR, z_stepper_align.xy[i].y); #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) for (uint8_t i = 0; i < NUM_Z_STEPPER_DRIVERS; ++i) - SERIAL_ECHOLNPAIR_P(PSTR("M422 W"), i + 1, SP_X_STR, z_stepper_align_stepper_pos[i].x, SP_Y_STR, z_stepper_align_stepper_pos[i].y); + SERIAL_ECHOLNPAIR_P(PSTR("M422 W"), i + '1', SP_X_STR, z_stepper_align.stepper_xy[i].x, SP_Y_STR, z_stepper_align.stepper_xy[i].y); #endif return; } @@ -427,9 +387,9 @@ void GcodeSuite::M422() { xy_pos_t *pos_dest = ( #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) - !is_probe_point ? z_stepper_align_stepper_pos : + !is_probe_point ? z_stepper_align.stepper_xy : #endif - z_stepper_align_pos + z_stepper_align.xy ); if (!is_probe_point @@ -451,7 +411,7 @@ void GcodeSuite::M422() { int8_t position_index; if (is_probe_point) { position_index = parser.intval('S') - 1; - if (!WITHIN(position_index, 0, int8_t(G34_PROBE_COUNT) - 1)) { + if (!WITHIN(position_index, 0, int8_t(NUM_Z_STEPPER_DRIVERS) - 1)) { SERIAL_ECHOLNPGM("?(S) Z-ProbePosition index invalid."); return; } diff --git a/Marlin/src/module/configuration_store.cpp b/Marlin/src/module/configuration_store.cpp index 05b67c984..0e4e6cef3 100644 --- a/Marlin/src/module/configuration_store.cpp +++ b/Marlin/src/module/configuration_store.cpp @@ -37,7 +37,7 @@ */ // Change EEPROM version if the structure changes -#define EEPROM_VERSION "V75" +#define EEPROM_VERSION "V76" #define EEPROM_OFFSET 100 // Check the integrity of data offsets. @@ -66,6 +66,10 @@ #include "../feature/bedlevel/bedlevel.h" #endif +#if ENABLED(Z_STEPPER_AUTO_ALIGN) + #include "../feature/z_stepper_align.h" +#endif + #if ENABLED(EXTENSIBLE_UI) #include "../lcd/extensible_ui/ui_api.h" #endif @@ -251,6 +255,16 @@ typedef struct SettingsDataStruct { z4_endstop_adj; // M666 (S4) Z #endif + // + // Z_STEPPER_AUTO_ALIGN, Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS + // + #if ENABLED(Z_STEPPER_AUTO_ALIGN) + xy_pos_t z_stepper_align_xy[NUM_Z_STEPPER_DRIVERS]; // M422 S X Y + #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) + xy_pos_t z_stepper_align_stepper_xy[NUM_Z_STEPPER_DRIVERS]; // M422 W X Y + #endif + #endif + // // ULTIPANEL // @@ -801,6 +815,13 @@ void MarlinSettings::postprocess() { #endif } + #if ENABLED(Z_STEPPER_AUTO_ALIGN) + EEPROM_WRITE(z_stepper_align.xy); + #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) + EEPROM_WRITE(z_stepper_align.stepper_xy); + #endif + #endif + // // LCD Preheat settings // @@ -1669,6 +1690,13 @@ void MarlinSettings::postprocess() { #endif } + #if ENABLED(Z_STEPPER_AUTO_ALIGN) + EEPROM_READ(z_stepper_align.xy); + #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) + EEPROM_READ(z_stepper_align.stepper_xy); + #endif + #endif + // // LCD Preheat settings // @@ -2473,6 +2501,14 @@ void MarlinSettings::reset() { #endif #endif + // + // Z Stepper Auto-alignment points + // + + #if ENABLED(Z_STEPPER_AUTO_ALIGN) + z_stepper_align.reset_to_default(); + #endif + // // Servo Angles //