|
|
@ -155,8 +155,12 @@ void GcodeSuite::G34() {
|
|
|
|
// Move the Z coordinate realm towards the positive - dirty trick
|
|
|
|
// Move the Z coordinate realm towards the positive - dirty trick
|
|
|
|
current_position.z -= z_probe * 0.5f;
|
|
|
|
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),
|
|
|
|
#if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
|
|
|
|
z_measured[NUM_Z_STEPPER_DRIVERS] = { 0 },
|
|
|
|
float last_z_align_move[NUM_Z_STEPPER_DRIVERS] = ARRAY_N(NUM_Z_STEPPER_DRIVERS, 10000.0f, 10000.0f, 10000.0f);
|
|
|
|
|
|
|
|
#else
|
|
|
|
|
|
|
|
float last_z_align_level_indicator = 10000.0f;
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
float z_measured[NUM_Z_STEPPER_DRIVERS] = { 0 },
|
|
|
|
z_maxdiff = 0.0f,
|
|
|
|
z_maxdiff = 0.0f,
|
|
|
|
amplification = z_auto_align_amplification;
|
|
|
|
amplification = z_auto_align_amplification;
|
|
|
|
|
|
|
|
|
|
|
@ -167,7 +171,7 @@ void GcodeSuite::G34() {
|
|
|
|
bool adjustment_reverse = false;
|
|
|
|
bool adjustment_reverse = false;
|
|
|
|
#endif
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
for (iteration = 0; iteration < z_auto_align_iterations; ++iteration) {
|
|
|
|
LOOP_L_N(iteration, z_auto_align_iterations) {
|
|
|
|
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> probing all positions.");
|
|
|
|
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> probing all positions.");
|
|
|
|
|
|
|
|
|
|
|
|
SERIAL_ECHOLNPAIR("\nITERATION: ", int(iteration + 1));
|
|
|
|
SERIAL_ECHOLNPAIR("\nITERATION: ", int(iteration + 1));
|
|
|
@ -177,7 +181,7 @@ void GcodeSuite::G34() {
|
|
|
|
z_measured_max = -100000.0f;
|
|
|
|
z_measured_max = -100000.0f;
|
|
|
|
|
|
|
|
|
|
|
|
// Probe all positions (one per Z-Stepper)
|
|
|
|
// Probe all positions (one per Z-Stepper)
|
|
|
|
for (uint8_t i = 0; i < NUM_Z_STEPPER_DRIVERS; ++i) {
|
|
|
|
LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS) {
|
|
|
|
// iteration odd/even --> downward / upward stepper sequence
|
|
|
|
// iteration odd/even --> downward / upward stepper sequence
|
|
|
|
const uint8_t iprobe = (iteration & 1) ? NUM_Z_STEPPER_DRIVERS - 1 - i : i;
|
|
|
|
const uint8_t iprobe = (iteration & 1) ? NUM_Z_STEPPER_DRIVERS - 1 - i : i;
|
|
|
|
|
|
|
|
|
|
|
@ -227,14 +231,14 @@ void GcodeSuite::G34() {
|
|
|
|
// This allows the actual adjustment logic to be shared by both algorithms.
|
|
|
|
// This allows the actual adjustment logic to be shared by both algorithms.
|
|
|
|
linear_fit_data lfd;
|
|
|
|
linear_fit_data lfd;
|
|
|
|
incremental_LSF_reset(&lfd);
|
|
|
|
incremental_LSF_reset(&lfd);
|
|
|
|
for (uint8_t i = 0; i < NUM_Z_STEPPER_DRIVERS; ++i) {
|
|
|
|
LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS) {
|
|
|
|
SERIAL_ECHOLNPAIR("PROBEPT_", i + '1', ": ", z_measured[i]);
|
|
|
|
SERIAL_ECHOLNPAIR("PROBEPT_", i + '1', ": ", 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);
|
|
|
|
|
|
|
|
|
|
|
|
z_measured_min = 100000.0f;
|
|
|
|
z_measured_min = 100000.0f;
|
|
|
|
for (uint8_t i = 0; i < NUM_Z_STEPPER_DRIVERS; ++i) {
|
|
|
|
LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS) {
|
|
|
|
z_measured[i] = -(lfd.A * z_stepper_align.stepper_xy[i].x + lfd.B * z_stepper_align.stepper_xy[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]);
|
|
|
|
z_measured_min = _MIN(z_measured_min, z_measured[i]);
|
|
|
|
}
|
|
|
|
}
|
|
|
@ -250,12 +254,37 @@ void GcodeSuite::G34() {
|
|
|
|
#endif
|
|
|
|
#endif
|
|
|
|
);
|
|
|
|
);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
|
|
|
|
|
|
|
|
// Check if the applied corrections go in the correct direction.
|
|
|
|
|
|
|
|
// Calculate the sum of the absolute deviations from the mean of the probe measurements.
|
|
|
|
|
|
|
|
// Compare to the last iteration to ensure it's getting better.
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Calculate mean value as a reference
|
|
|
|
|
|
|
|
float z_measured_mean = 0.0f;
|
|
|
|
|
|
|
|
LOOP_L_N(zstepper, NUM_Z_STEPPER_DRIVERS) z_measured_mean += z_measured[zstepper];
|
|
|
|
|
|
|
|
z_measured_mean /= NUM_Z_STEPPER_DRIVERS;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Calculate the sum of the absolute deviations from the mean value
|
|
|
|
|
|
|
|
float z_align_level_indicator = 0.0f;
|
|
|
|
|
|
|
|
LOOP_L_N(zstepper, NUM_Z_STEPPER_DRIVERS)
|
|
|
|
|
|
|
|
z_align_level_indicator += ABS(z_measured[zstepper] - z_measured_mean);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// If it's getting worse, stop and throw an error
|
|
|
|
|
|
|
|
if (last_z_align_level_indicator < z_align_level_indicator * 0.7f) {
|
|
|
|
|
|
|
|
SERIAL_ECHOLNPGM("Decreasing accuracy detected.");
|
|
|
|
|
|
|
|
err_break = true;
|
|
|
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
last_z_align_level_indicator = z_align_level_indicator;
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
// The following correction actions are to be enabled for select Z-steppers only
|
|
|
|
// The following correction actions are to be enabled for select Z-steppers only
|
|
|
|
stepper.set_separate_multi_axis(true);
|
|
|
|
stepper.set_separate_multi_axis(true);
|
|
|
|
|
|
|
|
|
|
|
|
bool success_break = true;
|
|
|
|
bool success_break = true;
|
|
|
|
// Correct the individual stepper offsets
|
|
|
|
// Correct the individual stepper offsets
|
|
|
|
for (uint8_t zstepper = 0; zstepper < NUM_Z_STEPPER_DRIVERS; ++zstepper) {
|
|
|
|
LOOP_L_N(zstepper, NUM_Z_STEPPER_DRIVERS) {
|
|
|
|
// Calculate current stepper move
|
|
|
|
// Calculate current stepper move
|
|
|
|
float z_align_move = z_measured[zstepper] - z_measured_min;
|
|
|
|
float z_align_move = z_measured[zstepper] - z_measured_min;
|
|
|
|
const float z_align_abs = ABS(z_align_move);
|
|
|
|
const float z_align_abs = ABS(z_align_move);
|
|
|
@ -263,21 +292,16 @@ void GcodeSuite::G34() {
|
|
|
|
#if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
|
|
|
|
#if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
|
|
|
|
// Optimize one iteration's correction based on the first measurements
|
|
|
|
// Optimize one iteration's correction based on the first measurements
|
|
|
|
if (z_align_abs) amplification = (iteration == 1) ? _MIN(last_z_align_move[zstepper] / z_align_abs, 2.0f) : z_auto_align_amplification;
|
|
|
|
if (z_align_abs) amplification = (iteration == 1) ? _MIN(last_z_align_move[zstepper] / z_align_abs, 2.0f) : z_auto_align_amplification;
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Check for less accuracy compared to last move
|
|
|
|
// Check for less accuracy compared to last move
|
|
|
|
if (last_z_align_move[zstepper] < z_align_abs * 0.7f) {
|
|
|
|
if (last_z_align_move[zstepper] < z_align_abs * 0.7f) {
|
|
|
|
SERIAL_ECHOLNPGM("Decreasing accuracy detected.");
|
|
|
|
SERIAL_ECHOLNPGM("Decreasing accuracy detected.");
|
|
|
|
#if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
|
|
|
|
|
|
|
|
adjustment_reverse = !adjustment_reverse;
|
|
|
|
adjustment_reverse = !adjustment_reverse;
|
|
|
|
#else
|
|
|
|
}
|
|
|
|
err_break = true;
|
|
|
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Remember the alignment for the next iteration
|
|
|
|
// Remember the alignment for the next iteration
|
|
|
|
last_z_align_move[zstepper] = z_align_abs;
|
|
|
|
last_z_align_move[zstepper] = z_align_abs;
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
// Stop early if all measured points achieve accuracy target
|
|
|
|
// Stop early if all measured points achieve accuracy target
|
|
|
|
if (z_align_abs > z_auto_align_accuracy) success_break = false;
|
|
|
|
if (z_align_abs > z_auto_align_accuracy) success_break = false;
|
|
|
@ -322,11 +346,9 @@ void GcodeSuite::G34() {
|
|
|
|
|
|
|
|
|
|
|
|
// Restore the active tool after homing
|
|
|
|
// Restore the active tool after homing
|
|
|
|
#if HOTENDS > 1
|
|
|
|
#if HOTENDS > 1
|
|
|
|
tool_change(old_tool_index, (
|
|
|
|
tool_change(old_tool_index, (true
|
|
|
|
#if ENABLED(PARKING_EXTRUDER)
|
|
|
|
#if ENABLED(PARKING_EXTRUDER)
|
|
|
|
false // Fetch the previous toolhead
|
|
|
|
&& false // Fetch the previous toolhead
|
|
|
|
#else
|
|
|
|
|
|
|
|
true
|
|
|
|
|
|
|
|
#endif
|
|
|
|
#endif
|
|
|
|
));
|
|
|
|
));
|
|
|
|
#endif
|
|
|
|
#endif
|
|
|
@ -367,10 +389,10 @@ void GcodeSuite::G34() {
|
|
|
|
void GcodeSuite::M422() {
|
|
|
|
void GcodeSuite::M422() {
|
|
|
|
|
|
|
|
|
|
|
|
if (!parser.seen_any()) {
|
|
|
|
if (!parser.seen_any()) {
|
|
|
|
for (uint8_t i = 0; i < NUM_Z_STEPPER_DRIVERS; ++i)
|
|
|
|
LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS)
|
|
|
|
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);
|
|
|
|
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)
|
|
|
|
#if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
|
|
|
|
for (uint8_t i = 0; i < NUM_Z_STEPPER_DRIVERS; ++i)
|
|
|
|
LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS)
|
|
|
|
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);
|
|
|
|
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
|
|
|
|
#endif
|
|
|
|
return;
|
|
|
|
return;
|
|
|
|