Quad Z leveling, G34 (R)ecalculate (#17122)

This commit is contained in:
InsanityAutomation 2020-03-14 00:22:04 -04:00 committed by GitHub
parent 118bd2f8b2
commit cee07f3f6b
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 45 additions and 32 deletions

View File

@ -64,6 +64,7 @@ inline void set_all_z_lock(const bool lock) {
* I<iterations> * I<iterations>
* T<accuracy> * T<accuracy>
* A<amplification> * A<amplification>
* R<recalculate> points based on current probe offsets
*/ */
void GcodeSuite::G34() { void GcodeSuite::G34() {
if (DEBUGGING(LEVELING)) { if (DEBUGGING(LEVELING)) {
@ -73,8 +74,10 @@ void GcodeSuite::G34() {
do { // break out on error do { // break out on error
#if NUM_Z_STEPPER_DRIVERS >= 4 #if NUM_Z_STEPPER_DRIVERS == 4
SERIAL_ECHOLNPGM("Alignment not supported for over 3 steppers"); SERIAL_ECHOLNPGM("Alignment for 4 steppers is Experimental!");
#elif NUM_Z_STEPPER_DRIVERS > 4
SERIAL_ECHOLNPGM("Alignment not supported for over 4 steppers");
break; break;
#endif #endif
@ -101,6 +104,8 @@ void GcodeSuite::G34() {
} }
#endif #endif
if (parser.seen('R')) z_stepper_align.reset_to_default();
const ProbePtRaise raise_after = parser.boolval('E') ? PROBE_PT_STOW : PROBE_PT_RAISE; const ProbePtRaise raise_after = parser.boolval('E') ? PROBE_PT_STOW : PROBE_PT_RAISE;
// Wait for planner moves to finish! // 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 // Compute a worst-case clearance height to probe from. After the first
// iteration this will be re-calculated based on the actual bed position // 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 #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), _MAX(magnitude2(0, 1), magnitude2(1, 2), magnitude2(2, 0))
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), #elif NUM_Z_STEPPER_DRIVERS == 4
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, 3),
magnitude2(3, 0), magnitude2(0, 2), magnitude2(1, 3))
#else #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 #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. // 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) #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 #else
float last_z_align_level_indicator = 10000.0f; float last_z_align_level_indicator = 10000.0f;
#endif #endif
@ -241,7 +251,7 @@ void GcodeSuite::G34() {
linear_fit_data lfd; linear_fit_data lfd;
incremental_LSF_reset(&lfd); incremental_LSF_reset(&lfd);
LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS) { 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]); incremental_LSF(&lfd, z_stepper_align.xy[i], z_measured[i]);
} }
finish_incremental_LSF(&lfd); finish_incremental_LSF(&lfd);
@ -322,9 +332,12 @@ void GcodeSuite::G34() {
switch (zstepper) { switch (zstepper) {
case 0: stepper.set_z_lock(false); break; case 0: stepper.set_z_lock(false); break;
case 1: stepper.set_z2_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; case 2: stepper.set_z3_lock(false); break;
#endif #endif
#if NUM_Z_STEPPER_DRIVERS == 4
case 3: stepper.set_z4_lock(false); break;
#endif
} }
#if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) #if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
@ -348,27 +361,12 @@ void GcodeSuite::G34() {
} // for (iteration) } // for (iteration)
if (err_break) { if (err_break)
SERIAL_ECHOLNPGM("G34 aborted."); SERIAL_ECHOLNPGM("G34 aborted.");
set_axis_not_trusted(Z_AXIS); // The Z coordinate is messed up now else {
break; SERIAL_ECHOLNPAIR("Did ", int(iteration + (iteration != z_auto_align_iterations)), " of ", int(z_auto_align_iterations));
}
SERIAL_ECHOLNPAIR("Did ", int(iteration + (iteration != z_auto_align_iterations)), " iterations of ", int(z_auto_align_iterations));
SERIAL_ECHOLNPAIR_F("Accuracy: ", z_maxdiff); 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 // Stow the probe, as the last call to probe.probe_at_point(...) left
// the probe deployed if it was successful. // the probe deployed if it was successful.
@ -377,7 +375,6 @@ void GcodeSuite::G34() {
#if ENABLED(HOME_AFTER_G34) #if ENABLED(HOME_AFTER_G34)
// After this operation the z position needs correction // After this operation the z position needs correction
set_axis_not_trusted(Z_AXIS); set_axis_not_trusted(Z_AXIS);
// Home Z after the alignment procedure // Home Z after the alignment procedure
process_subcommands_now_P(PSTR("G28Z")); process_subcommands_now_P(PSTR("G28Z"));
#else #else
@ -388,6 +385,15 @@ void GcodeSuite::G34() {
sync_plan_position(); sync_plan_position();
#endif #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); }while(0);
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("<<< G34"); if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("<<< G34");
@ -406,9 +412,16 @@ void GcodeSuite::G34() {
* S and W require an X and/or Y parameter * S and W require an X and/or Y parameter
* X<pos> : X position to set (Unchanged if omitted) * X<pos> : X position to set (Unchanged if omitted)
* Y<pos> : Y position to set (Unchanged if omitted) * Y<pos> : Y position to set (Unchanged if omitted)
*
* R : Recalculate points based on current probe offsets
*/ */
void GcodeSuite::M422() { void GcodeSuite::M422() {
if (parser.seen('R')) {
z_stepper_align.reset_to_default();
return;
}
if (!parser.seen_any()) { if (!parser.seen_any()) {
LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS) 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); 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);

View File

@ -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." #error "Z_STEPPER_AUTO_ALIGN requires NUM_Z_STEPPER_DRIVERS greater than 1."
#elif !HAS_BED_PROBE #elif !HAS_BED_PROBE
#error "Z_STEPPER_AUTO_ALIGN requires a Z-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 #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." #error "Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS requires NUM_Z_STEPPER_DRIVERS to be 3 or 4."
#endif #endif
#endif #endif