|
|
|
|
@ -1381,6 +1381,8 @@
|
|
|
|
|
|
|
|
|
|
#if HAS_BED_PROBE
|
|
|
|
|
|
|
|
|
|
//#define VALIDATE_MESH_TILT
|
|
|
|
|
|
|
|
|
|
#include "../../../libs/vector_3.h"
|
|
|
|
|
|
|
|
|
|
void unified_bed_leveling::tilt_mesh_based_on_probed_grid(const bool do_3_pt_leveling) {
|
|
|
|
|
@ -1392,7 +1394,9 @@
|
|
|
|
|
float measured_z;
|
|
|
|
|
bool abort_flag = false;
|
|
|
|
|
|
|
|
|
|
//float z1, z2, z3; // Needed for algorithm validation below
|
|
|
|
|
#ifdef VALIDATE_MESH_TILT
|
|
|
|
|
float z1, z2, z3; // Needed for algorithm validation below
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
struct linear_fit_data lsf_results;
|
|
|
|
|
incremental_LSF_reset(&lsf_results);
|
|
|
|
|
@ -1403,17 +1407,19 @@
|
|
|
|
|
ui.status_printf_P(0, PSTR(MSG_LCD_TILTING_MESH " 1/3"));
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
measured_z = probe_at_point(PROBE_PT_1_X, PROBE_PT_1_Y, PROBE_PT_RAISE, g29_verbose_level);
|
|
|
|
|
measured_z = probe_at_point(x_min, y_min, PROBE_PT_RAISE, g29_verbose_level);
|
|
|
|
|
if (isnan(measured_z))
|
|
|
|
|
abort_flag = true;
|
|
|
|
|
else {
|
|
|
|
|
measured_z -= get_z_correction(PROBE_PT_1_X, PROBE_PT_1_Y);
|
|
|
|
|
//z1 = measured_z;
|
|
|
|
|
measured_z -= get_z_correction(x_min, y_min);
|
|
|
|
|
#ifdef VALIDATE_MESH_TILT
|
|
|
|
|
z1 = measured_z;
|
|
|
|
|
#endif
|
|
|
|
|
if (g29_verbose_level > 3) {
|
|
|
|
|
serial_spaces(16);
|
|
|
|
|
SERIAL_ECHOLNPAIR("Corrected_Z=", measured_z);
|
|
|
|
|
}
|
|
|
|
|
incremental_LSF(&lsf_results, PROBE_PT_1_X, PROBE_PT_1_Y, measured_z);
|
|
|
|
|
incremental_LSF(&lsf_results, x_min, y_min, measured_z);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (!abort_flag) {
|
|
|
|
|
@ -1422,17 +1428,19 @@
|
|
|
|
|
ui.status_printf_P(0, PSTR(MSG_LCD_TILTING_MESH " 2/3"));
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
measured_z = probe_at_point(PROBE_PT_2_X, PROBE_PT_2_Y, PROBE_PT_RAISE, g29_verbose_level);
|
|
|
|
|
//z2 = measured_z;
|
|
|
|
|
measured_z = probe_at_point(x_max, y_min, PROBE_PT_RAISE, g29_verbose_level);
|
|
|
|
|
#ifdef VALIDATE_MESH_TILT
|
|
|
|
|
z2 = measured_z;
|
|
|
|
|
#endif
|
|
|
|
|
if (isnan(measured_z))
|
|
|
|
|
abort_flag = true;
|
|
|
|
|
else {
|
|
|
|
|
measured_z -= get_z_correction(PROBE_PT_2_X, PROBE_PT_2_Y);
|
|
|
|
|
measured_z -= get_z_correction(x_max, y_min);
|
|
|
|
|
if (g29_verbose_level > 3) {
|
|
|
|
|
serial_spaces(16);
|
|
|
|
|
SERIAL_ECHOLNPAIR("Corrected_Z=", measured_z);
|
|
|
|
|
}
|
|
|
|
|
incremental_LSF(&lsf_results, PROBE_PT_2_X, PROBE_PT_2_Y, measured_z);
|
|
|
|
|
incremental_LSF(&lsf_results, x_max, y_min, measured_z);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
@ -1442,17 +1450,20 @@
|
|
|
|
|
ui.status_printf_P(0, PSTR(MSG_LCD_TILTING_MESH " 3/3"));
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
measured_z = probe_at_point(PROBE_PT_3_X, PROBE_PT_3_Y, PROBE_PT_STOW, g29_verbose_level);
|
|
|
|
|
//z3 = measured_z;
|
|
|
|
|
const float center_probe = (x_max - x_min) / 2;
|
|
|
|
|
measured_z = probe_at_point(center_probe, y_max, PROBE_PT_STOW, g29_verbose_level);
|
|
|
|
|
#ifdef VALIDATE_MESH_TILT
|
|
|
|
|
z3 = measured_z;
|
|
|
|
|
#endif
|
|
|
|
|
if (isnan(measured_z))
|
|
|
|
|
abort_flag = true;
|
|
|
|
|
else {
|
|
|
|
|
measured_z -= get_z_correction(PROBE_PT_3_X, PROBE_PT_3_Y);
|
|
|
|
|
measured_z -= get_z_correction(center_probe, y_max);
|
|
|
|
|
if (g29_verbose_level > 3) {
|
|
|
|
|
serial_spaces(16);
|
|
|
|
|
SERIAL_ECHOLNPAIR("Corrected_Z=", measured_z);
|
|
|
|
|
}
|
|
|
|
|
incremental_LSF(&lsf_results, PROBE_PT_3_X, PROBE_PT_3_Y, measured_z);
|
|
|
|
|
incremental_LSF(&lsf_results, center_probe, y_max, measured_z);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
@ -1598,34 +1609,34 @@
|
|
|
|
|
* is calculated. The Z error between the probed point locations and the get_z_correction()
|
|
|
|
|
* numbers for those locations should be 0.
|
|
|
|
|
*/
|
|
|
|
|
#if 0
|
|
|
|
|
float t, t1, d;
|
|
|
|
|
t = normal.x * (PROBE_PT_1_X) + normal.y * (PROBE_PT_1_Y);
|
|
|
|
|
d = t + normal.z * z1;
|
|
|
|
|
DEBUG_ECHOPAIR_F("D from 1st point: ", d, 6);
|
|
|
|
|
DEBUG_ECHOLNPAIR_F(" Z error: ", normal.z*z1-get_z_correction(PROBE_PT_1_X, PROBE_PT_1_Y), 6);
|
|
|
|
|
|
|
|
|
|
t = normal.x * (PROBE_PT_2_X) + normal.y * (PROBE_PT_2_Y);
|
|
|
|
|
d = t + normal.z * z2;
|
|
|
|
|
DEBUG_EOL();
|
|
|
|
|
DEBUG_ECHOPAIR_F("D from 2nd point: ", d, 6);
|
|
|
|
|
DEBUG_ECHOLNPAIR_F(" Z error: ", normal.z*z2-get_z_correction(PROBE_PT_2_X, PROBE_PT_2_Y), 6);
|
|
|
|
|
|
|
|
|
|
t = normal.x * (PROBE_PT_3_X) + normal.y * (PROBE_PT_3_Y);
|
|
|
|
|
d = t + normal.z * z3;
|
|
|
|
|
DEBUG_ECHOPAIR_F("D from 3rd point: ", d, 6);
|
|
|
|
|
DEBUG_ECHOLNPAIR_F(" Z error: ", normal.z*z3-get_z_correction(PROBE_PT_3_X, PROBE_PT_3_Y), 6);
|
|
|
|
|
|
|
|
|
|
t = normal.x * (Z_SAFE_HOMING_X_POINT) + normal.y * (Z_SAFE_HOMING_Y_POINT);
|
|
|
|
|
d = t + normal.z * 0;
|
|
|
|
|
DEBUG_ECHOLNPAIR_F("D from home location with Z=0 : ", d, 6);
|
|
|
|
|
|
|
|
|
|
t = normal.x * (Z_SAFE_HOMING_X_POINT) + normal.y * (Z_SAFE_HOMING_Y_POINT);
|
|
|
|
|
d = t + get_z_correction(Z_SAFE_HOMING_X_POINT, Z_SAFE_HOMING_Y_POINT); // normal.z * 0;
|
|
|
|
|
DEBUG_ECHOPAIR_F("D from home location using mesh value for Z: ", d, 6);
|
|
|
|
|
|
|
|
|
|
DEBUG_ECHOPAIR(" Z error: (", Z_SAFE_HOMING_X_POINT, ",", Z_SAFE_HOMING_Y_POINT);
|
|
|
|
|
DEBUG_ECHOLNPAIR_F(") = ", get_z_correction(Z_SAFE_HOMING_X_POINT, Z_SAFE_HOMING_Y_POINT), 6);
|
|
|
|
|
#ifdef VALIDATE_MESH_TILT
|
|
|
|
|
float t, t1, d;
|
|
|
|
|
t = normal.x * x_min + normal.y * y_min;
|
|
|
|
|
d = t + normal.z * z1;
|
|
|
|
|
DEBUG_ECHOPAIR_F("D from 1st point: ", d, 6);
|
|
|
|
|
DEBUG_ECHOLNPAIR_F(" Z error: ", normal.z * z1 - get_z_correction(x_min, y_min), 6);
|
|
|
|
|
|
|
|
|
|
t = normal.x * x_max + normal.y * y_min;
|
|
|
|
|
d = t + normal.z * z2;
|
|
|
|
|
DEBUG_EOL();
|
|
|
|
|
DEBUG_ECHOPAIR_F("D from 2nd point: ", d, 6);
|
|
|
|
|
DEBUG_ECHOLNPAIR_F(" Z error: ", normal.z * z2 - get_z_correction(x_max, y_min), 6);
|
|
|
|
|
|
|
|
|
|
t = normal.x * ((x_max - x_min) / 2) + normal.y * (y_min);
|
|
|
|
|
d = t + normal.z * z3;
|
|
|
|
|
DEBUG_ECHOPAIR_F("D from 3rd point: ", d, 6);
|
|
|
|
|
DEBUG_ECHOLNPAIR_F(" Z error: ", normal.z * z3 - get_z_correction((x_max - x_min) / 2, y_max), 6);
|
|
|
|
|
|
|
|
|
|
t = normal.x * (Z_SAFE_HOMING_X_POINT) + normal.y * (Z_SAFE_HOMING_Y_POINT);
|
|
|
|
|
d = t + normal.z * 0;
|
|
|
|
|
DEBUG_ECHOLNPAIR_F("D from home location with Z=0 : ", d, 6);
|
|
|
|
|
|
|
|
|
|
t = normal.x * (Z_SAFE_HOMING_X_POINT) + normal.y * (Z_SAFE_HOMING_Y_POINT);
|
|
|
|
|
d = t + get_z_correction(Z_SAFE_HOMING_X_POINT, Z_SAFE_HOMING_Y_POINT); // normal.z * 0;
|
|
|
|
|
DEBUG_ECHOPAIR_F("D from home location using mesh value for Z: ", d, 6);
|
|
|
|
|
|
|
|
|
|
DEBUG_ECHOPAIR(" Z error: (", Z_SAFE_HOMING_X_POINT, ",", Z_SAFE_HOMING_Y_POINT);
|
|
|
|
|
DEBUG_ECHOLNPAIR_F(") = ", get_z_correction(Z_SAFE_HOMING_X_POINT, Z_SAFE_HOMING_Y_POINT), 6);
|
|
|
|
|
#endif
|
|
|
|
|
} // DEBUGGING(LEVELING)
|
|
|
|
|
|
|
|
|
|
|
InsanityAutomation
5 years ago
committed by