From cee07f3f6b3fad54919283529daa65a964266000 Mon Sep 17 00:00:00 2001 From: InsanityAutomation <38436470+InsanityAutomation@users.noreply.github.com> Date: Sat, 14 Mar 2020 00:22:04 -0400 Subject: [PATCH] Quad Z leveling, G34 (R)ecalculate (#17122) --- Marlin/src/gcode/calibrate/G34_M422.cpp | 73 +++++++++++++++---------- Marlin/src/inc/SanityCheck.h | 4 +- 2 files changed, 45 insertions(+), 32 deletions(-) diff --git a/Marlin/src/gcode/calibrate/G34_M422.cpp b/Marlin/src/gcode/calibrate/G34_M422.cpp index fecbb4be68..672f40737a 100644 --- a/Marlin/src/gcode/calibrate/G34_M422.cpp +++ b/Marlin/src/gcode/calibrate/G34_M422.cpp @@ -64,6 +64,7 @@ inline void set_all_z_lock(const bool lock) { * I * T * A + * R points based on current probe offsets */ void GcodeSuite::G34() { if (DEBUGGING(LEVELING)) { @@ -73,8 +74,10 @@ void GcodeSuite::G34() { do { // break out on error - #if NUM_Z_STEPPER_DRIVERS >= 4 - SERIAL_ECHOLNPGM("Alignment not supported for over 3 steppers"); + #if NUM_Z_STEPPER_DRIVERS == 4 + SERIAL_ECHOLNPGM("Alignment for 4 steppers is Experimental!"); + #elif NUM_Z_STEPPER_DRIVERS > 4 + SERIAL_ECHOLNPGM("Alignment not supported for over 4 steppers"); break; #endif @@ -101,6 +104,8 @@ void GcodeSuite::G34() { } #endif + if (parser.seen('R')) z_stepper_align.reset_to_default(); + const ProbePtRaise raise_after = parser.boolval('E') ? PROBE_PT_STOW : PROBE_PT_RAISE; // Wait for planner moves to finish! @@ -139,13 +144,18 @@ void GcodeSuite::G34() { // Compute a worst-case clearance height to probe from. After the first // iteration this will be re-calculated based on the actual bed position - float z_probe = Z_BASIC_CLEARANCE + (G34_MAX_GRADE) * 0.01f * ( + auto magnitude2 = [&](const uint8_t i, const uint8_t j) { + const xy_pos_t diff = z_stepper_align.xy[i] - z_stepper_align.xy[j]; + return HYPOT2(diff.x, diff.y); + }; + float z_probe = Z_BASIC_CLEARANCE + (G34_MAX_GRADE) * 0.01f * SQRT( #if NUM_Z_STEPPER_DRIVERS == 3 - SQRT(_MAX(HYPOT2(z_stepper_align.xy[0].x - z_stepper_align.xy[1].x, z_stepper_align.xy[0].y - z_stepper_align.xy[1].y), - HYPOT2(z_stepper_align.xy[1].x - z_stepper_align.xy[2].x, z_stepper_align.xy[1].y - z_stepper_align.xy[2].y), - HYPOT2(z_stepper_align.xy[2].x - z_stepper_align.xy[0].x, z_stepper_align.xy[2].y - z_stepper_align.xy[0].y))) + _MAX(magnitude2(0, 1), magnitude2(1, 2), magnitude2(2, 0)) + #elif NUM_Z_STEPPER_DRIVERS == 4 + _MAX(magnitude2(0, 1), magnitude2(1, 2), magnitude2(2, 3), + magnitude2(3, 0), magnitude2(0, 2), magnitude2(1, 3)) #else - HYPOT(z_stepper_align.xy[0].x - z_stepper_align.xy[1].x, z_stepper_align.xy[0].y - z_stepper_align.xy[1].y) + magnitude2(0, 1) #endif ); @@ -159,7 +169,7 @@ void GcodeSuite::G34() { // This hack is un-done at the end of G34 - either by re-homing, or by using the probed heights of the last iteration. #if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) - float last_z_align_move[NUM_Z_STEPPER_DRIVERS] = ARRAY_N(NUM_Z_STEPPER_DRIVERS, 10000.0f, 10000.0f, 10000.0f); + float last_z_align_move[NUM_Z_STEPPER_DRIVERS] = ARRAY_N(NUM_Z_STEPPER_DRIVERS, 10000.0f, 10000.0f, 10000.0f, 10000.0f); #else float last_z_align_level_indicator = 10000.0f; #endif @@ -241,7 +251,7 @@ void GcodeSuite::G34() { linear_fit_data lfd; incremental_LSF_reset(&lfd); LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS) { - SERIAL_ECHOLNPAIR("PROBEPT_", ('0' + i), ": ", z_measured[i]); + SERIAL_ECHOLNPAIR("PROBEPT_", int(i), ": ", z_measured[i]); incremental_LSF(&lfd, z_stepper_align.xy[i], z_measured[i]); } finish_incremental_LSF(&lfd); @@ -322,9 +332,12 @@ void GcodeSuite::G34() { switch (zstepper) { case 0: stepper.set_z_lock(false); break; case 1: stepper.set_z2_lock(false); break; - #if NUM_Z_STEPPER_DRIVERS == 3 + #if NUM_Z_STEPPER_DRIVERS >= 3 case 2: stepper.set_z3_lock(false); break; #endif + #if NUM_Z_STEPPER_DRIVERS == 4 + case 3: stepper.set_z4_lock(false); break; + #endif } #if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) @@ -348,28 +361,13 @@ void GcodeSuite::G34() { } // for (iteration) - if (err_break) { + if (err_break) SERIAL_ECHOLNPGM("G34 aborted."); - set_axis_not_trusted(Z_AXIS); // The Z coordinate is messed up now - break; + else { + SERIAL_ECHOLNPAIR("Did ", int(iteration + (iteration != z_auto_align_iterations)), " of ", int(z_auto_align_iterations)); + SERIAL_ECHOLNPAIR_F("Accuracy: ", z_maxdiff); } - SERIAL_ECHOLNPAIR("Did ", int(iteration + (iteration != z_auto_align_iterations)), " iterations of ", int(z_auto_align_iterations)); - SERIAL_ECHOLNPAIR_F("Accuracy: ", z_maxdiff); - - // Restore the active tool after homing - #if HOTENDS > 1 - tool_change(old_tool_index, (true - #if ENABLED(PARKING_EXTRUDER) - && false // Fetch the previous toolhead - #endif - )); - #endif - - #if HAS_LEVELING && ENABLED(RESTORE_LEVELING_AFTER_G34) - set_bed_leveling_enabled(leveling_was_active); - #endif - // Stow the probe, as the last call to probe.probe_at_point(...) left // the probe deployed if it was successful. probe.stow(); @@ -377,7 +375,6 @@ void GcodeSuite::G34() { #if ENABLED(HOME_AFTER_G34) // After this operation the z position needs correction set_axis_not_trusted(Z_AXIS); - // Home Z after the alignment procedure process_subcommands_now_P(PSTR("G28Z")); #else @@ -388,6 +385,15 @@ void GcodeSuite::G34() { sync_plan_position(); #endif + // Restore the active tool after homing + #if HOTENDS > 1 + tool_change(old_tool_index, DISABLED(PARKING_EXTRUDER)); // Fetch previous tool for parking extruder + #endif + + #if HAS_LEVELING && ENABLED(RESTORE_LEVELING_AFTER_G34) + set_bed_leveling_enabled(leveling_was_active); + #endif + }while(0); if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("<<< G34"); @@ -406,9 +412,16 @@ void GcodeSuite::G34() { * S and W require an X and/or Y parameter * X : X position to set (Unchanged if omitted) * Y : Y position to set (Unchanged if omitted) + * + * R : Recalculate points based on current probe offsets */ void GcodeSuite::M422() { + if (parser.seen('R')) { + z_stepper_align.reset_to_default(); + return; + } + if (!parser.seen_any()) { LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS) SERIAL_ECHOLNPAIR_P(PSTR("M422 S"), int(i + 1), SP_X_STR, z_stepper_align.xy[i].x, SP_Y_STR, z_stepper_align.xy[i].y); diff --git a/Marlin/src/inc/SanityCheck.h b/Marlin/src/inc/SanityCheck.h index d017dbf2db..454183e548 100644 --- a/Marlin/src/inc/SanityCheck.h +++ b/Marlin/src/inc/SanityCheck.h @@ -2509,8 +2509,8 @@ static_assert( _ARR_TEST(3,0) && _ARR_TEST(3,1) && _ARR_TEST(3,2) #error "Z_STEPPER_AUTO_ALIGN requires NUM_Z_STEPPER_DRIVERS greater than 1." #elif !HAS_BED_PROBE #error "Z_STEPPER_AUTO_ALIGN requires a Z-bed probe." - #elif ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) && NUM_Z_STEPPER_DRIVERS != 3 - #error "Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS requires NUM_Z_STEPPER_DRIVERS to be 3." + #elif ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) && NUM_Z_STEPPER_DRIVERS < 3 + #error "Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS requires NUM_Z_STEPPER_DRIVERS to be 3 or 4." #endif #endif