From 05ebde38127ca6c3bc056cb4068a414bade766b4 Mon Sep 17 00:00:00 2001 From: lujios <83166168+lujios@users.noreply.github.com> Date: Tue, 13 Jul 2021 02:19:29 +0200 Subject: [PATCH] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Improve=20Sensorless=20hom?= =?UTF-8?q?ing/probing=20for=20G28,=20G33=20(#21899)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Marlin/src/MarlinCore.cpp | 6 +- Marlin/src/feature/tmc_util.h | 7 -- Marlin/src/gcode/calibrate/G28.cpp | 18 ++++- Marlin/src/gcode/calibrate/G33.cpp | 16 ++++- Marlin/src/module/delta.cpp | 1 + Marlin/src/module/endstops.cpp | 12 +++- Marlin/src/module/planner.cpp | 28 ++++++++ Marlin/src/module/planner.h | 13 ++++ Marlin/src/module/probe.cpp | 109 +++++++++++++++++++++++++++-- Marlin/src/module/probe.h | 12 ++++ 10 files changed, 197 insertions(+), 25 deletions(-) diff --git a/Marlin/src/MarlinCore.cpp b/Marlin/src/MarlinCore.cpp index c595e8b13664..3ec4a8193c86 100644 --- a/Marlin/src/MarlinCore.cpp +++ b/Marlin/src/MarlinCore.cpp @@ -828,10 +828,8 @@ void idle(bool no_stepper_sleep/*=false*/) { // Run StallGuard endstop checks #if ENABLED(SPI_ENDSTOPS) - if (endstops.tmc_spi_homing.any - && TERN1(IMPROVE_HOMING_RELIABILITY, ELAPSED(millis(), sg_guard_period)) - ) LOOP_L_N(i, 4) // Read SGT 4 times per idle loop - if (endstops.tmc_spi_homing_check()) break; + if (endstops.tmc_spi_homing.any && TERN1(IMPROVE_HOMING_RELIABILITY, ELAPSED(millis(), sg_guard_period))) + LOOP_L_N(i, 4) if (endstops.tmc_spi_homing_check()) break; // Read SGT 4 times per idle loop #endif // Handle SD Card insert / remove diff --git a/Marlin/src/feature/tmc_util.h b/Marlin/src/feature/tmc_util.h index 4753f78d91fe..c878d86faedd 100644 --- a/Marlin/src/feature/tmc_util.h +++ b/Marlin/src/feature/tmc_util.h @@ -360,13 +360,6 @@ void test_tmc_connection(LOGICAL_AXIS_DECL(const bool, true)); #if ENABLED(IMPROVE_HOMING_RELIABILITY) extern millis_t sg_guard_period; constexpr uint16_t default_sg_guard_duration = 400; - - struct motion_state_t { - xy_ulong_t acceleration; - #if HAS_CLASSIC_JERK - xy_float_t jerk_state; - #endif - }; #endif bool tmc_enable_stallguard(TMC2130Stepper &st); diff --git a/Marlin/src/gcode/calibrate/G28.cpp b/Marlin/src/gcode/calibrate/G28.cpp index 7cd1f65fbf36..ca9cbb8cc9b8 100644 --- a/Marlin/src/gcode/calibrate/G28.cpp +++ b/Marlin/src/gcode/calibrate/G28.cpp @@ -167,12 +167,15 @@ motion_state_t begin_slow_homing() { motion_state_t motion_state{0}; motion_state.acceleration.set(planner.settings.max_acceleration_mm_per_s2[X_AXIS], - planner.settings.max_acceleration_mm_per_s2[Y_AXIS]); + planner.settings.max_acceleration_mm_per_s2[Y_AXIS] + OPTARG(DELTA, planner.settings.max_acceleration_mm_per_s2[Z_AXIS]) + ); planner.settings.max_acceleration_mm_per_s2[X_AXIS] = 100; planner.settings.max_acceleration_mm_per_s2[Y_AXIS] = 100; + TERN_(DELTA, planner.settings.max_acceleration_mm_per_s2[Z_AXIS] = 100); #if HAS_CLASSIC_JERK motion_state.jerk_state = planner.max_jerk; - planner.max_jerk.set(0, 0); + planner.max_jerk.set(0, 0 OPTARG(DELTA, 0)); #endif planner.reset_acceleration_rates(); return motion_state; @@ -181,6 +184,7 @@ void end_slow_homing(const motion_state_t &motion_state) { planner.settings.max_acceleration_mm_per_s2[X_AXIS] = motion_state.acceleration.x; planner.settings.max_acceleration_mm_per_s2[Y_AXIS] = motion_state.acceleration.y; + TERN_(DELTA, planner.settings.max_acceleration_mm_per_s2[Z_AXIS] = motion_state.acceleration.z); TERN_(HAS_CLASSIC_JERK, planner.max_jerk = motion_state.jerk_state); planner.reset_acceleration_rates(); } @@ -259,7 +263,7 @@ void GcodeSuite::G28() { reset_stepper_timeout(); #define HAS_CURRENT_HOME(N) (defined(N##_CURRENT_HOME) && N##_CURRENT_HOME != N##_CURRENT) - #if HAS_CURRENT_HOME(X) || HAS_CURRENT_HOME(X2) || HAS_CURRENT_HOME(Y) || HAS_CURRENT_HOME(Y2) + #if HAS_CURRENT_HOME(X) || HAS_CURRENT_HOME(X2) || HAS_CURRENT_HOME(Y) || HAS_CURRENT_HOME(Y2) || (ENABLED(DELTA) && HAS_CURRENT_HOME(Z)) #define HAS_HOMING_CURRENT 1 #endif @@ -287,6 +291,11 @@ void GcodeSuite::G28() { stepperY2.rms_current(Y2_CURRENT_HOME); if (DEBUGGING(LEVELING)) debug_current(PSTR("Y2"), tmc_save_current_Y2, Y2_CURRENT_HOME); #endif + #if HAS_CURRENT_HOME(Z) && ENABLED(DELTA) + const int16_t tmc_save_current_Z = stepperZ.getMilliamps(); + stepperZ.rms_current(Z_CURRENT_HOME); + if (DEBUGGING(LEVELING)) debug_current(PSTR("Z"), tmc_save_current_Z, Z_CURRENT_HOME); + #endif #endif #if ENABLED(IMPROVE_HOMING_RELIABILITY) @@ -497,6 +506,9 @@ void GcodeSuite::G28() { #if HAS_CURRENT_HOME(Y2) stepperY2.rms_current(tmc_save_current_Y2); #endif + #if HAS_CURRENT_HOME(Z) && ENABLED(DELTA) + stepperZ.rms_current(tmc_save_current_Z); + #endif #if HAS_CURRENT_HOME(I) stepperI.rms_current(tmc_save_current_I); #endif diff --git a/Marlin/src/gcode/calibrate/G33.cpp b/Marlin/src/gcode/calibrate/G33.cpp index 0737c9166850..a897a101157c 100644 --- a/Marlin/src/gcode/calibrate/G33.cpp +++ b/Marlin/src/gcode/calibrate/G33.cpp @@ -71,7 +71,9 @@ float lcd_probe_pt(const xy_pos_t &xy); void ac_home() { endstops.enable(true); + TERN_(SENSORLESS_HOMING, probe.set_homing_current(true)); home_delta(); + TERN_(SENSORLESS_HOMING, probe.set_homing_current(false)); endstops.not_homing(); } @@ -384,6 +386,12 @@ static float auto_tune_a() { * V3 Report settings and probe results * * E Engage the probe for each point + * + * With SENSORLESS_PROBING: + * Use these flags to calibrate stall sensitivity: (e.g., `G33 P1 Y Z` to calibrate X only.) + * X Don't activate stallguard on X. + * Y Don't activate stallguard on Y. + * Z Don't activate stallguard on Z. */ void GcodeSuite::G33() { @@ -417,6 +425,12 @@ void GcodeSuite::G33() { const bool stow_after_each = parser.seen_test('E'); + #if ENABLED(SENSORLESS_PROBING) + probe.test_sensitivity.x = !parser.seen_test('X'); + TERN_(HAS_Y_AXIS, probe.test_sensitivity.y = !parser.seen_test('Y')); + TERN_(HAS_Z_AXIS, probe.test_sensitivity.z = !parser.seen_test('Z')); + #endif + const bool _0p_calibration = probe_points == 0, _1p_calibration = probe_points == 1 || probe_points == -1, _4p_calibration = probe_points == 2, @@ -587,7 +601,7 @@ void GcodeSuite::G33() { // print report - if (verbose_level == 3) + if (verbose_level == 3 || verbose_level == 0) print_calibration_results(z_at_pt, _tower_results, _opposite_results); if (verbose_level != 0) { // !dry run diff --git a/Marlin/src/module/delta.cpp b/Marlin/src/module/delta.cpp index 96d8841f1312..992c3a09b469 100644 --- a/Marlin/src/module/delta.cpp +++ b/Marlin/src/module/delta.cpp @@ -254,6 +254,7 @@ void home_delta() { current_position.z = DIFF_TERN(HAS_BED_PROBE, delta_height + 10, probe.offset.z); line_to_current_position(homing_feedrate(Z_AXIS)); planner.synchronize(); + TERN_(SENSORLESS_PROBING,endstops.report_states()); // Re-enable stealthChop if used. Disable diag1 pin on driver. #if ENABLED(SENSORLESS_HOMING) && DISABLED(ENDSTOPS_ALWAYS_ON_DEFAULT) diff --git a/Marlin/src/module/endstops.cpp b/Marlin/src/module/endstops.cpp index 8f6827de2776..420acccb58b0 100644 --- a/Marlin/src/module/endstops.cpp +++ b/Marlin/src/module/endstops.cpp @@ -595,9 +595,15 @@ void _O2 Endstops::report_states() { // The following routines are called from an ISR context. It could be the temperature ISR, the // endstop ISR or the Stepper ISR. -#define _ENDSTOP(AXIS, MINMAX) AXIS ##_## MINMAX -#define _ENDSTOP_PIN(AXIS, MINMAX) AXIS ##_## MINMAX ##_PIN -#define _ENDSTOP_INVERTING(AXIS, MINMAX) AXIS ##_## MINMAX ##_ENDSTOP_INVERTING +#if BOTH(DELTA, SENSORLESS_PROBING) + #define _ENDSTOP(AXIS, MINMAX) AXIS ##_MAX + #define _ENDSTOP_PIN(AXIS, MINMAX) AXIS ##_MAX_PIN + #define _ENDSTOP_INVERTING(AXIS, MINMAX) AXIS ##_MAX_ENDSTOP_INVERTING +#else + #define _ENDSTOP(AXIS, MINMAX) AXIS ##_## MINMAX + #define _ENDSTOP_PIN(AXIS, MINMAX) AXIS ##_## MINMAX ##_PIN + #define _ENDSTOP_INVERTING(AXIS, MINMAX) AXIS ##_## MINMAX ##_ENDSTOP_INVERTING +#endif // Check endstops - Could be called from Temperature ISR! void Endstops::update() { diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index 49b2d60b20b8..e48d05b09f6d 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -1527,6 +1527,34 @@ void Planner::check_axes_activity() { } #endif +#if ENABLED(IMPROVE_HOMING_RELIABILITY) + + void Planner::enable_stall_prevention(const bool onoff) { + static motion_state_t saved_motion_state; + if (onoff) { + saved_motion_state.acceleration.x = settings.max_acceleration_mm_per_s2[X_AXIS]; + saved_motion_state.acceleration.y = settings.max_acceleration_mm_per_s2[Y_AXIS]; + settings.max_acceleration_mm_per_s2[X_AXIS] = settings.max_acceleration_mm_per_s2[Y_AXIS] = 100; + #if ENABLED(DELTA) + saved_motion_state.acceleration.z = settings.max_acceleration_mm_per_s2[Z_AXIS]; + settings.max_acceleration_mm_per_s2[Z_AXIS] = 100; + #endif + #if HAS_CLASSIC_JERK + saved_motion_state.jerk_state = max_jerk; + max_jerk.set(0, 0 OPTARG(DELTA, 0)); + #endif + } + else { + settings.max_acceleration_mm_per_s2[X_AXIS] = saved_motion_state.acceleration.x; + settings.max_acceleration_mm_per_s2[Y_AXIS] = saved_motion_state.acceleration.y; + TERN_(DELTA, settings.max_acceleration_mm_per_s2[Z_AXIS] = saved_motion_state.acceleration.z); + TERN_(HAS_CLASSIC_JERK, max_jerk = saved_motion_state.jerk_state); + } + reset_acceleration_rates(); + } + +#endif + #if HAS_LEVELING constexpr xy_pos_t level_fulcrum = { diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index 10114ebfc637..9b104615f6f9 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -281,6 +281,15 @@ typedef struct { min_travel_feedrate_mm_s; // (mm/s) M205 T - Minimum travel feedrate } planner_settings_t; +#if ENABLED(IMPROVE_HOMING_RELIABILITY) + struct motion_state_t { + TERN(DELTA, xyz_ulong_t, xy_ulong_t) acceleration; + #if HAS_CLASSIC_JERK + TERN(DELTA, xyz_float_t, xy_float_t) jerk_state; + #endif + }; +#endif + #if DISABLED(SKEW_CORRECTION) #define XY_SKEW_FACTOR 0 #define XZ_SKEW_FACTOR 0 @@ -532,6 +541,10 @@ class Planner { } #endif + #if ENABLED(IMPROVE_HOMING_RELIABILITY) + void enable_stall_prevention(const bool onoff); + #endif + #if DISABLED(NO_VOLUMETRICS) // Update multipliers based on new diameter measurements diff --git a/Marlin/src/module/probe.cpp b/Marlin/src/module/probe.cpp index dae25feea3eb..6831d151f9ec 100644 --- a/Marlin/src/module/probe.cpp +++ b/Marlin/src/module/probe.cpp @@ -68,7 +68,7 @@ #include "servo.h" #endif -#if ENABLED(SENSORLESS_PROBING) +#if EITHER(SENSORLESS_PROBING, SENSORLESS_HOMING) #include "stepper.h" #include "../feature/tmc_util.h" #endif @@ -92,6 +92,10 @@ xyz_pos_t Probe::offset; // Initialized by settings.load() const xy_pos_t &Probe::offset_xy = Probe::offset; #endif +#if ENABLED(SENSORLESS_PROBING) + Probe::sense_bool_t Probe::test_sensitivity; +#endif + #if ENABLED(Z_PROBE_SLED) #ifndef SLED_DOCKING_OFFSET @@ -493,11 +497,12 @@ bool Probe::probe_down_to_z(const_float_t z, const_feedRate_t fr_mm_s) { #if ENABLED(SENSORLESS_PROBING) sensorless_t stealth_states { false }; #if ENABLED(DELTA) - stealth_states.x = tmc_enable_stallguard(stepperX); - stealth_states.y = tmc_enable_stallguard(stepperY); + if (probe.test_sensitivity.x) stealth_states.x = tmc_enable_stallguard(stepperX); // Delta watches all DIAG pins for a stall + if (probe.test_sensitivity.y) stealth_states.y = tmc_enable_stallguard(stepperY); #endif - stealth_states.z = tmc_enable_stallguard(stepperZ); + if (probe.test_sensitivity.z) stealth_states.z = tmc_enable_stallguard(stepperZ); // All machines will check Z-DIAG for stall endstops.enable(true); + set_homing_current(true); // The "homing" current also applies to probing #endif TERN_(HAS_QUIET_PROBING, set_probing_paused(true)); @@ -520,10 +525,11 @@ bool Probe::probe_down_to_z(const_float_t z, const_feedRate_t fr_mm_s) { #if ENABLED(SENSORLESS_PROBING) endstops.not_homing(); #if ENABLED(DELTA) - tmc_disable_stallguard(stepperX, stealth_states.x); - tmc_disable_stallguard(stepperY, stealth_states.y); + if (probe.test_sensitivity.x) tmc_disable_stallguard(stepperX, stealth_states.x); + if (probe.test_sensitivity.y) tmc_disable_stallguard(stepperY, stealth_states.y); #endif - tmc_disable_stallguard(stepperZ, stealth_states.z); + if (probe.test_sensitivity.z) tmc_disable_stallguard(stepperZ, stealth_states.z); + set_homing_current(false); #endif if (probe_triggered && TERN0(BLTOUCH_SLOW_MODE, bltouch.stow())) // Stow in LOW SPEED MODE on every trigger @@ -815,4 +821,93 @@ float Probe::probe_at_point(const_float_t rx, const_float_t ry, const ProbePtRai #endif // HAS_Z_SERVO_PROBE +#if EITHER(SENSORLESS_PROBING, SENSORLESS_HOMING) + + sensorless_t stealth_states { false }; + + /** + * Disable stealthChop if used. Enable diag1 pin on driver. + */ + void Probe::enable_stallguard_diag1() { + #if ENABLED(SENSORLESS_PROBING) + #if ENABLED(DELTA) + stealth_states.x = tmc_enable_stallguard(stepperX); + stealth_states.y = tmc_enable_stallguard(stepperY); + #endif + stealth_states.z = tmc_enable_stallguard(stepperZ); + endstops.enable(true); + #endif + } + + /** + * Re-enable stealthChop if used. Disable diag1 pin on driver. + */ + void Probe::disable_stallguard_diag1() { + #if ENABLED(SENSORLESS_PROBING) + endstops.not_homing(); + #if ENABLED(DELTA) + tmc_disable_stallguard(stepperX, stealth_states.x); + tmc_disable_stallguard(stepperY, stealth_states.y); + #endif + tmc_disable_stallguard(stepperZ, stealth_states.z); + #endif + } + + /** + * Change the current in the TMC drivers to N##_CURRENT_HOME. And we save the current configuration of each TMC driver. + */ + void Probe::set_homing_current(const bool onoff) { + #define HAS_CURRENT_HOME(N) (defined(N##_CURRENT_HOME) && N##_CURRENT_HOME != N##_CURRENT) + #if HAS_CURRENT_HOME(X) || HAS_CURRENT_HOME(Y) || HAS_CURRENT_HOME(Z) + #if ENABLED(DELTA) + static int16_t saved_current_X, saved_current_Y; + #endif + #if HAS_CURRENT_HOME(Z) + static int16_t saved_current_Z; + #endif + auto debug_current_on = [](PGM_P const s, const int16_t a, const int16_t b) { + if (DEBUGGING(LEVELING)) { DEBUG_ECHOPGM_P(s); DEBUG_ECHOLNPAIR(" current: ", a, " -> ", b); } + }; + if (onoff) { + #if ENABLED(DELTA) + #if HAS_CURRENT_HOME(X) + saved_current_X = stepperX.getMilliamps(); + stepperX.rms_current(X_CURRENT_HOME); + debug_current_on(PSTR("X"), saved_current_X, X_CURRENT_HOME); + #endif + #if HAS_CURRENT_HOME(Y) + saved_current_Y = stepperY.getMilliamps(); + stepperY.rms_current(Y_CURRENT_HOME); + debug_current_on(PSTR("Y"), saved_current_Y, Y_CURRENT_HOME); + #endif + #endif + #if HAS_CURRENT_HOME(Z) + saved_current_Z = stepperZ.getMilliamps(); + stepperZ.rms_current(Z_CURRENT_HOME); + debug_current_on(PSTR("Z"), saved_current_Z, Z_CURRENT_HOME); + #endif + TERN_(IMPROVE_HOMING_RELIABILITY, planner.enable_stall_prevention(true)); + } + else { + #if ENABLED(DELTA) + #if HAS_CURRENT_HOME(X) + stepperX.rms_current(saved_current_X); + debug_current_on(PSTR("X"), X_CURRENT_HOME, saved_current_X); + #endif + #if HAS_CURRENT_HOME(Y) + stepperY.rms_current(saved_current_Y); + debug_current_on(PSTR("Y"), Y_CURRENT_HOME, saved_current_Y); + #endif + #endif + #if HAS_CURRENT_HOME(Z) + stepperZ.rms_current(saved_current_Z); + debug_current_on(PSTR("Z"), Z_CURRENT_HOME, saved_current_Z); + #endif + TERN_(IMPROVE_HOMING_RELIABILITY, planner.enable_stall_prevention(false)); + } + #endif + } + +#endif // SENSORLESS_PROBING + #endif // HAS_BED_PROBE diff --git a/Marlin/src/module/probe.h b/Marlin/src/module/probe.h index da46c830f699..62880c865f71 100644 --- a/Marlin/src/module/probe.h +++ b/Marlin/src/module/probe.h @@ -56,6 +56,11 @@ class Probe { public: + #if ENABLED(SENSORLESS_PROBING) + typedef struct { bool x:1, y:1, z:1; } sense_bool_t; + static sense_bool_t test_sensitivity; + #endif + #if HAS_BED_PROBE static xyz_pos_t offset; @@ -256,6 +261,13 @@ class Probe { static bool tare(); #endif + // Basic functions for Sensorless Homing and Probing + #if EITHER(SENSORLESS_HOMING, SENSORLESS_PROBING) + static void enable_stallguard_diag1(); + static void disable_stallguard_diag1(); + static void set_homing_current(const bool onoff); + #endif + private: static bool probe_down_to_z(const_float_t z, const_feedRate_t fr_mm_s); static void do_z_raise(const float z_raise);