|
|
@@ -1,2407 +1,2407 @@
|
|
|
-#include "Marlin.h"
|
|
|
-#include "Configuration.h"
|
|
|
-#include "ConfigurationStore.h"
|
|
|
-#include "language_all.h"
|
|
|
-#include "mesh_bed_calibration.h"
|
|
|
-#include "mesh_bed_leveling.h"
|
|
|
-#include "stepper.h"
|
|
|
-#include "ultralcd.h"
|
|
|
-
|
|
|
-uint8_t world2machine_correction_mode;
|
|
|
-float world2machine_rotation_and_skew[2][2];
|
|
|
-float world2machine_rotation_and_skew_inv[2][2];
|
|
|
-float world2machine_shift[2];
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-#define WEIGHT_FIRST_ROW_X_HIGH (1.f)
|
|
|
-#define WEIGHT_FIRST_ROW_X_LOW (0.35f)
|
|
|
-#define WEIGHT_FIRST_ROW_Y_HIGH (0.3f)
|
|
|
-#define WEIGHT_FIRST_ROW_Y_LOW (0.0f)
|
|
|
-
|
|
|
-#define BED_ZERO_REF_X (- 22.f + X_PROBE_OFFSET_FROM_EXTRUDER)
|
|
|
-#define BED_ZERO_REF_Y (- 0.6f + Y_PROBE_OFFSET_FROM_EXTRUDER)
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-#define MACHINE_AXIS_SCALE_X 1.f
|
|
|
-#define MACHINE_AXIS_SCALE_Y 1.f
|
|
|
-
|
|
|
-
|
|
|
-#define BED_SKEW_ANGLE_MILD (0.12f * M_PI / 180.f)
|
|
|
-
|
|
|
-#define BED_SKEW_ANGLE_EXTREME (0.25f * M_PI / 180.f)
|
|
|
-
|
|
|
-#define BED_CALIBRATION_POINT_OFFSET_MAX_EUCLIDIAN (0.8f)
|
|
|
-#define BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_X (0.8f)
|
|
|
-#define BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_Y (1.5f)
|
|
|
-
|
|
|
-#define MIN_BED_SENSOR_POINT_RESPONSE_DMR (2.0f)
|
|
|
-
|
|
|
-
|
|
|
-#define Y_MIN_POS_FOR_BED_CALIBRATION (Y_MIN_POS)
|
|
|
-
|
|
|
-#define Y_MIN_POS_CALIBRATION_POINT_ACCURATE (Y_MIN_POS + 3.f)
|
|
|
-
|
|
|
-
|
|
|
-#define Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH (Y_MIN_POS - 0.5f)
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-const float bed_ref_points[] PROGMEM = {
|
|
|
- 13.f - BED_ZERO_REF_X, 6.4f - BED_ZERO_REF_Y,
|
|
|
- 115.f - BED_ZERO_REF_X, 6.4f - BED_ZERO_REF_Y,
|
|
|
- 216.f - BED_ZERO_REF_X, 6.4f - BED_ZERO_REF_Y,
|
|
|
-
|
|
|
- 216.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
|
|
|
- 115.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
|
|
|
- 13.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
|
|
|
-
|
|
|
- 13.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y,
|
|
|
- 115.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y,
|
|
|
- 216.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y
|
|
|
-};
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-const float bed_ref_points_4[] PROGMEM = {
|
|
|
- 115.f - BED_ZERO_REF_X, 6.4f - BED_ZERO_REF_Y,
|
|
|
- 216.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
|
|
|
- 115.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y,
|
|
|
- 13.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y
|
|
|
-};
|
|
|
-
|
|
|
-static inline float sqr(float x) { return x * x; }
|
|
|
-
|
|
|
-static inline bool point_on_1st_row(const uint8_t i, const uint8_t npts)
|
|
|
-{
|
|
|
- if (npts == 4) return (i == 0);
|
|
|
- else return (i < 3);
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-static inline float point_weight_x(const uint8_t i, const uint8_t npts, const float &y)
|
|
|
-{
|
|
|
- float w = 1.f;
|
|
|
- if (point_on_1st_row(i, npts)) {
|
|
|
- if (y >= Y_MIN_POS_CALIBRATION_POINT_ACCURATE) {
|
|
|
- w = WEIGHT_FIRST_ROW_X_HIGH;
|
|
|
- } else if (y < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH) {
|
|
|
-
|
|
|
- w = WEIGHT_FIRST_ROW_X_LOW;
|
|
|
- } else {
|
|
|
-
|
|
|
- float t = (y - Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH) / (Y_MIN_POS_CALIBRATION_POINT_ACCURATE - Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH);
|
|
|
- w = (1.f - t) * WEIGHT_FIRST_ROW_X_LOW + t * WEIGHT_FIRST_ROW_X_HIGH;
|
|
|
- }
|
|
|
- }
|
|
|
- return w;
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-static inline float point_weight_y(const uint8_t i, const uint8_t npts, const float &y)
|
|
|
-{
|
|
|
- float w = 1.f;
|
|
|
- if (point_on_1st_row(i, npts)) {
|
|
|
- if (y >= Y_MIN_POS_CALIBRATION_POINT_ACCURATE) {
|
|
|
- w = WEIGHT_FIRST_ROW_Y_HIGH;
|
|
|
- } else if (y < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH) {
|
|
|
-
|
|
|
- w = WEIGHT_FIRST_ROW_Y_LOW;
|
|
|
- } else {
|
|
|
-
|
|
|
- float t = (y - Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH) / (Y_MIN_POS_CALIBRATION_POINT_ACCURATE - Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH);
|
|
|
- w = (1.f - t) * WEIGHT_FIRST_ROW_Y_LOW + t * WEIGHT_FIRST_ROW_Y_HIGH;
|
|
|
- }
|
|
|
- }
|
|
|
- return w;
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-BedSkewOffsetDetectionResultType calculate_machine_skew_and_offset_LS(
|
|
|
-
|
|
|
- const float *measured_pts,
|
|
|
- uint8_t npts,
|
|
|
- const float *true_pts,
|
|
|
-
|
|
|
- float *vec_x,
|
|
|
- float *vec_y,
|
|
|
- float *cntr,
|
|
|
-
|
|
|
-
|
|
|
- int8_t verbosity_level
|
|
|
- )
|
|
|
-{
|
|
|
- if (verbosity_level >= 10) {
|
|
|
- SERIAL_ECHOLNPGM("calculate machine skew and offset LS");
|
|
|
-
|
|
|
-
|
|
|
- SERIAL_ECHOPGM("X vector, initial: ");
|
|
|
- MYSERIAL.print(vec_x[0], 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(vec_x[1], 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
-
|
|
|
- SERIAL_ECHOPGM("Y vector, initial: ");
|
|
|
- MYSERIAL.print(vec_y[0], 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(vec_y[1], 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
-
|
|
|
- SERIAL_ECHOPGM("center, initial: ");
|
|
|
- MYSERIAL.print(cntr[0], 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(cntr[1], 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
-
|
|
|
- for (uint8_t i = 0; i < npts; ++i) {
|
|
|
- SERIAL_ECHOPGM("point #");
|
|
|
- MYSERIAL.print(int(i));
|
|
|
- SERIAL_ECHOPGM(" measured: (");
|
|
|
- MYSERIAL.print(measured_pts[i * 2], 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(measured_pts[i * 2 + 1], 5);
|
|
|
- SERIAL_ECHOPGM("); target: (");
|
|
|
- MYSERIAL.print(pgm_read_float(true_pts + i * 2), 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(pgm_read_float(true_pts + i * 2 + 1), 5);
|
|
|
- SERIAL_ECHOPGM("), error: ");
|
|
|
- MYSERIAL.print(sqrt(
|
|
|
- sqr(pgm_read_float(true_pts + i * 2) - measured_pts[i * 2]) +
|
|
|
- sqr(pgm_read_float(true_pts + i * 2 + 1) - measured_pts[i * 2 + 1])), 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- }
|
|
|
- delay_keep_alive(100);
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- cntr[0] = 0.f;
|
|
|
- cntr[1] = 0.f;
|
|
|
-
|
|
|
- float a1 = 0;
|
|
|
-
|
|
|
- float a2 = 0;
|
|
|
- for (int8_t iter = 0; iter < 100; ++iter) {
|
|
|
- float c1 = cos(a1) * MACHINE_AXIS_SCALE_X;
|
|
|
- float s1 = sin(a1) * MACHINE_AXIS_SCALE_X;
|
|
|
- float c2 = cos(a2) * MACHINE_AXIS_SCALE_Y;
|
|
|
- float s2 = sin(a2) * MACHINE_AXIS_SCALE_Y;
|
|
|
-
|
|
|
- float A[4][4] = { 0.f };
|
|
|
- float b[4] = { 0.f };
|
|
|
- float acc;
|
|
|
- for (uint8_t r = 0; r < 4; ++r) {
|
|
|
- for (uint8_t c = 0; c < 4; ++c) {
|
|
|
- acc = 0;
|
|
|
-
|
|
|
- for (uint8_t i = 0; i < npts; ++i) {
|
|
|
-
|
|
|
- if (r != 1 && c != 1) {
|
|
|
- float a =
|
|
|
- (r == 0) ? 1.f :
|
|
|
- ((r == 2) ? (-s1 * measured_pts[2 * i]) :
|
|
|
- (-c2 * measured_pts[2 * i + 1]));
|
|
|
- float b =
|
|
|
- (c == 0) ? 1.f :
|
|
|
- ((c == 2) ? (-s1 * measured_pts[2 * i]) :
|
|
|
- (-c2 * measured_pts[2 * i + 1]));
|
|
|
- float w = point_weight_x(i, npts, measured_pts[2 * i + 1]);
|
|
|
- acc += a * b * w;
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- if (r != 0 && c != 0) {
|
|
|
- float a =
|
|
|
- (r == 1) ? 1.f :
|
|
|
- ((r == 2) ? ( c1 * measured_pts[2 * i]) :
|
|
|
- (-s2 * measured_pts[2 * i + 1]));
|
|
|
- float b =
|
|
|
- (c == 1) ? 1.f :
|
|
|
- ((c == 2) ? ( c1 * measured_pts[2 * i]) :
|
|
|
- (-s2 * measured_pts[2 * i + 1]));
|
|
|
- float w = point_weight_y(i, npts, measured_pts[2 * i + 1]);
|
|
|
- acc += a * b * w;
|
|
|
- }
|
|
|
- }
|
|
|
- A[r][c] = acc;
|
|
|
- }
|
|
|
-
|
|
|
- acc = 0.f;
|
|
|
- for (uint8_t i = 0; i < npts; ++i) {
|
|
|
- {
|
|
|
- float j =
|
|
|
- (r == 0) ? 1.f :
|
|
|
- ((r == 1) ? 0.f :
|
|
|
- ((r == 2) ? (-s1 * measured_pts[2 * i]) :
|
|
|
- (-c2 * measured_pts[2 * i + 1])));
|
|
|
- float fx = c1 * measured_pts[2 * i] - s2 * measured_pts[2 * i + 1] + cntr[0] - pgm_read_float(true_pts + i * 2);
|
|
|
- float w = point_weight_x(i, npts, measured_pts[2 * i + 1]);
|
|
|
- acc += j * fx * w;
|
|
|
- }
|
|
|
- {
|
|
|
- float j =
|
|
|
- (r == 0) ? 0.f :
|
|
|
- ((r == 1) ? 1.f :
|
|
|
- ((r == 2) ? ( c1 * measured_pts[2 * i]) :
|
|
|
- (-s2 * measured_pts[2 * i + 1])));
|
|
|
- float fy = s1 * measured_pts[2 * i] + c2 * measured_pts[2 * i + 1] + cntr[1] - pgm_read_float(true_pts + i * 2 + 1);
|
|
|
- float w = point_weight_y(i, npts, measured_pts[2 * i + 1]);
|
|
|
- acc += j * fy * w;
|
|
|
- }
|
|
|
- }
|
|
|
- b[r] = -acc;
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
- float h[4] = { 0.f };
|
|
|
- for (uint8_t gauss_iter = 0; gauss_iter < 100; ++gauss_iter) {
|
|
|
- h[0] = (b[0] - A[0][1] * h[1] - A[0][2] * h[2] - A[0][3] * h[3]) / A[0][0];
|
|
|
- h[1] = (b[1] - A[1][0] * h[0] - A[1][2] * h[2] - A[1][3] * h[3]) / A[1][1];
|
|
|
- h[2] = (b[2] - A[2][0] * h[0] - A[2][1] * h[1] - A[2][3] * h[3]) / A[2][2];
|
|
|
- h[3] = (b[3] - A[3][0] * h[0] - A[3][1] * h[1] - A[3][2] * h[2]) / A[3][3];
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- cntr[0] += h[0];
|
|
|
- cntr[1] += h[1];
|
|
|
- a1 += h[2];
|
|
|
- a2 += h[3];
|
|
|
-
|
|
|
- if (verbosity_level >= 20) {
|
|
|
- SERIAL_ECHOPGM("iteration: ");
|
|
|
- MYSERIAL.print(int(iter));
|
|
|
- SERIAL_ECHOPGM("; correction vector: ");
|
|
|
- MYSERIAL.print(h[0], 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(h[1], 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(h[2], 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(h[3], 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- SERIAL_ECHOPGM("corrected x/y: ");
|
|
|
- MYSERIAL.print(cntr[0], 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(cntr[0], 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- SERIAL_ECHOPGM("corrected angles: ");
|
|
|
- MYSERIAL.print(180.f * a1 / M_PI, 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(180.f * a2 / M_PI, 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- vec_x[0] = cos(a1) * MACHINE_AXIS_SCALE_X;
|
|
|
- vec_x[1] = sin(a1) * MACHINE_AXIS_SCALE_X;
|
|
|
- vec_y[0] = -sin(a2) * MACHINE_AXIS_SCALE_Y;
|
|
|
- vec_y[1] = cos(a2) * MACHINE_AXIS_SCALE_Y;
|
|
|
-
|
|
|
- BedSkewOffsetDetectionResultType result = BED_SKEW_OFFSET_DETECTION_PERFECT;
|
|
|
- {
|
|
|
- float angleDiff = fabs(a2 - a1);
|
|
|
- if (angleDiff > BED_SKEW_ANGLE_MILD)
|
|
|
- result = (angleDiff > BED_SKEW_ANGLE_EXTREME) ?
|
|
|
- BED_SKEW_OFFSET_DETECTION_SKEW_EXTREME :
|
|
|
- BED_SKEW_OFFSET_DETECTION_SKEW_MILD;
|
|
|
- if (fabs(a1) > BED_SKEW_ANGLE_EXTREME ||
|
|
|
- fabs(a2) > BED_SKEW_ANGLE_EXTREME)
|
|
|
- result = BED_SKEW_OFFSET_DETECTION_SKEW_EXTREME;
|
|
|
- }
|
|
|
-
|
|
|
- if (verbosity_level >= 1) {
|
|
|
- SERIAL_ECHOPGM("correction angles: ");
|
|
|
- MYSERIAL.print(180.f * a1 / M_PI, 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(180.f * a2 / M_PI, 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- }
|
|
|
-
|
|
|
- if (verbosity_level >= 10) {
|
|
|
-
|
|
|
- SERIAL_ECHOPGM("X vector new, inverted: ");
|
|
|
- MYSERIAL.print(vec_x[0], 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(vec_x[1], 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
-
|
|
|
- SERIAL_ECHOPGM("Y vector new, inverted: ");
|
|
|
- MYSERIAL.print(vec_y[0], 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(vec_y[1], 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
-
|
|
|
- SERIAL_ECHOPGM("center new, inverted: ");
|
|
|
- MYSERIAL.print(cntr[0], 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(cntr[1], 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- delay_keep_alive(100);
|
|
|
-
|
|
|
- SERIAL_ECHOLNPGM("Error after correction: ");
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
- for (uint8_t i = 0; i < npts; ++i) {
|
|
|
- float x = vec_x[0] * measured_pts[i * 2] + vec_y[0] * measured_pts[i * 2 + 1] + cntr[0];
|
|
|
- float y = vec_x[1] * measured_pts[i * 2] + vec_y[1] * measured_pts[i * 2 + 1] + cntr[1];
|
|
|
- float errX = sqr(pgm_read_float(true_pts + i * 2) - x);
|
|
|
- float errY = sqr(pgm_read_float(true_pts + i * 2 + 1) - y);
|
|
|
- float err = sqrt(errX + errY);
|
|
|
- if (verbosity_level >= 10) {
|
|
|
- SERIAL_ECHOPGM("point #");
|
|
|
- MYSERIAL.print(int(i));
|
|
|
- SERIAL_ECHOLNPGM(":");
|
|
|
- }
|
|
|
-
|
|
|
- if (point_on_1st_row(i, npts)) {
|
|
|
- if(verbosity_level >= 20) SERIAL_ECHOPGM("Point on first row");
|
|
|
- float w = point_weight_y(i, npts, measured_pts[2 * i + 1]);
|
|
|
- if (sqrt(errX) > BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_X ||
|
|
|
- (w != 0.f && sqrt(errY) > BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_Y)) {
|
|
|
- result = BED_SKEW_OFFSET_DETECTION_FITTING_FAILED;
|
|
|
- if (verbosity_level >= 20) {
|
|
|
- SERIAL_ECHOPGM(", weigth Y: ");
|
|
|
- MYSERIAL.print(w);
|
|
|
- if (sqrt(errX) > BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_X) SERIAL_ECHOPGM(", error X > max. error X");
|
|
|
- if (w != 0.f && sqrt(errY) > BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_Y) SERIAL_ECHOPGM(", error Y > max. error Y");
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
- else {
|
|
|
- if(verbosity_level >=20 ) SERIAL_ECHOPGM("Point not on first row");
|
|
|
- if (err > BED_CALIBRATION_POINT_OFFSET_MAX_EUCLIDIAN) {
|
|
|
- result = BED_SKEW_OFFSET_DETECTION_FITTING_FAILED;
|
|
|
- if(verbosity_level >= 20) SERIAL_ECHOPGM(", error > max. error euclidian");
|
|
|
- }
|
|
|
- }
|
|
|
- if (verbosity_level >= 10) {
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- SERIAL_ECHOPGM("measured: (");
|
|
|
- MYSERIAL.print(measured_pts[i * 2], 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(measured_pts[i * 2 + 1], 5);
|
|
|
- SERIAL_ECHOPGM("); corrected: (");
|
|
|
- MYSERIAL.print(x, 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(y, 5);
|
|
|
- SERIAL_ECHOPGM("); target: (");
|
|
|
- MYSERIAL.print(pgm_read_float(true_pts + i * 2), 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(pgm_read_float(true_pts + i * 2 + 1), 5);
|
|
|
- SERIAL_ECHOLNPGM(")");
|
|
|
- SERIAL_ECHOPGM("error: ");
|
|
|
- MYSERIAL.print(err);
|
|
|
- SERIAL_ECHOPGM(", error X: ");
|
|
|
- MYSERIAL.print(sqrt(errX));
|
|
|
- SERIAL_ECHOPGM(", error Y: ");
|
|
|
- MYSERIAL.print(sqrt(errY));
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- }
|
|
|
- }
|
|
|
- if (verbosity_level >= 20) {
|
|
|
- SERIAL_ECHOLNPGM("Max. errors:");
|
|
|
- SERIAL_ECHOPGM("Max. error X:");
|
|
|
- MYSERIAL.println(BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_X);
|
|
|
- SERIAL_ECHOPGM("Max. error Y:");
|
|
|
- MYSERIAL.println(BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_Y);
|
|
|
- SERIAL_ECHOPGM("Max. error euclidian:");
|
|
|
- MYSERIAL.println(BED_CALIBRATION_POINT_OFFSET_MAX_EUCLIDIAN);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- }
|
|
|
-
|
|
|
- #if 0
|
|
|
- if (result == BED_SKEW_OFFSET_DETECTION_PERFECT && fabs(a1) < BED_SKEW_ANGLE_MILD && fabs(a2) < BED_SKEW_ANGLE_MILD) {
|
|
|
- if (verbosity_level > 0)
|
|
|
- SERIAL_ECHOLNPGM("Very little skew detected. Disabling skew correction.");
|
|
|
-
|
|
|
- vec_x[0] = MACHINE_AXIS_SCALE_X;
|
|
|
- vec_x[1] = 0.f;
|
|
|
- vec_y[0] = 0.f;
|
|
|
- vec_y[1] = MACHINE_AXIS_SCALE_Y;
|
|
|
- }
|
|
|
- #else
|
|
|
- if (result == BED_SKEW_OFFSET_DETECTION_PERFECT) {
|
|
|
- if (verbosity_level > 0)
|
|
|
- SERIAL_ECHOLNPGM("Very little skew detected. Orthogonalizing the axes.");
|
|
|
-
|
|
|
- a1 = 0.5f * (a1 + a2);
|
|
|
- vec_x[0] = cos(a1) * MACHINE_AXIS_SCALE_X;
|
|
|
- vec_x[1] = sin(a1) * MACHINE_AXIS_SCALE_X;
|
|
|
- vec_y[0] = -sin(a1) * MACHINE_AXIS_SCALE_Y;
|
|
|
- vec_y[1] = cos(a1) * MACHINE_AXIS_SCALE_Y;
|
|
|
-
|
|
|
- cntr[0] = 0.f;
|
|
|
- cntr[1] = 0.f;
|
|
|
- float wx = 0.f;
|
|
|
- float wy = 0.f;
|
|
|
- for (int8_t i = 0; i < npts; ++ i) {
|
|
|
- float x = vec_x[0] * measured_pts[i * 2] + vec_y[0] * measured_pts[i * 2 + 1];
|
|
|
- float y = vec_x[1] * measured_pts[i * 2] + vec_y[1] * measured_pts[i * 2 + 1];
|
|
|
- float w = point_weight_x(i, npts, y);
|
|
|
- cntr[0] += w * (pgm_read_float(true_pts + i * 2) - x);
|
|
|
- wx += w;
|
|
|
- if (verbosity_level >= 20) {
|
|
|
- MYSERIAL.print(i);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- SERIAL_ECHOLNPGM("Weight_x:");
|
|
|
- MYSERIAL.print(w);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- SERIAL_ECHOLNPGM("cntr[0]:");
|
|
|
- MYSERIAL.print(cntr[0]);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- SERIAL_ECHOLNPGM("wx:");
|
|
|
- MYSERIAL.print(wx);
|
|
|
- }
|
|
|
- w = point_weight_y(i, npts, y);
|
|
|
- cntr[1] += w * (pgm_read_float(true_pts + i * 2 + 1) - y);
|
|
|
- wy += w;
|
|
|
-
|
|
|
- if (verbosity_level >= 20) {
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- SERIAL_ECHOLNPGM("Weight_y:");
|
|
|
- MYSERIAL.print(w);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- SERIAL_ECHOLNPGM("cntr[1]:");
|
|
|
- MYSERIAL.print(cntr[1]);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- SERIAL_ECHOLNPGM("wy:");
|
|
|
- MYSERIAL.print(wy);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- }
|
|
|
- }
|
|
|
- cntr[0] /= wx;
|
|
|
- cntr[1] /= wy;
|
|
|
- if (verbosity_level >= 20) {
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- SERIAL_ECHOLNPGM("Final cntr values:");
|
|
|
- SERIAL_ECHOLNPGM("cntr[0]:");
|
|
|
- MYSERIAL.print(cntr[0]);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- SERIAL_ECHOLNPGM("cntr[1]:");
|
|
|
- MYSERIAL.print(cntr[1]);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- }
|
|
|
-
|
|
|
- }
|
|
|
- #endif
|
|
|
-
|
|
|
-
|
|
|
- {
|
|
|
- float d = vec_x[0] * vec_y[1] - vec_x[1] * vec_y[0];
|
|
|
- float Ainv[2][2] = {
|
|
|
- { vec_y[1] / d, -vec_y[0] / d },
|
|
|
- { -vec_x[1] / d, vec_x[0] / d }
|
|
|
- };
|
|
|
- float cntrInv[2] = {
|
|
|
- -Ainv[0][0] * cntr[0] - Ainv[0][1] * cntr[1],
|
|
|
- -Ainv[1][0] * cntr[0] - Ainv[1][1] * cntr[1]
|
|
|
- };
|
|
|
- vec_x[0] = Ainv[0][0];
|
|
|
- vec_x[1] = Ainv[1][0];
|
|
|
- vec_y[0] = Ainv[0][1];
|
|
|
- vec_y[1] = Ainv[1][1];
|
|
|
- cntr[0] = cntrInv[0];
|
|
|
- cntr[1] = cntrInv[1];
|
|
|
- }
|
|
|
-
|
|
|
- if (verbosity_level >= 1) {
|
|
|
-
|
|
|
- SERIAL_ECHOPGM("X vector, adjusted: ");
|
|
|
- MYSERIAL.print(vec_x[0], 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(vec_x[1], 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
-
|
|
|
- SERIAL_ECHOPGM("Y vector, adjusted: ");
|
|
|
- MYSERIAL.print(vec_y[0], 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(vec_y[1], 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
-
|
|
|
- SERIAL_ECHOPGM("center, adjusted: ");
|
|
|
- MYSERIAL.print(cntr[0], 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(cntr[1], 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- delay_keep_alive(100);
|
|
|
- }
|
|
|
-
|
|
|
- if (verbosity_level >= 2) {
|
|
|
- SERIAL_ECHOLNPGM("Difference after correction: ");
|
|
|
- for (uint8_t i = 0; i < npts; ++i) {
|
|
|
- float x = vec_x[0] * pgm_read_float(true_pts + i * 2) + vec_y[0] * pgm_read_float(true_pts + i * 2 + 1) + cntr[0];
|
|
|
- float y = vec_x[1] * pgm_read_float(true_pts + i * 2) + vec_y[1] * pgm_read_float(true_pts + i * 2 + 1) + cntr[1];
|
|
|
- SERIAL_ECHOPGM("point #");
|
|
|
- MYSERIAL.print(int(i));
|
|
|
- SERIAL_ECHOPGM("measured: (");
|
|
|
- MYSERIAL.print(measured_pts[i * 2], 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(measured_pts[i * 2 + 1], 5);
|
|
|
- SERIAL_ECHOPGM("); measured-corrected: (");
|
|
|
- MYSERIAL.print(x, 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(y, 5);
|
|
|
- SERIAL_ECHOPGM("); target: (");
|
|
|
- MYSERIAL.print(pgm_read_float(true_pts + i * 2), 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(pgm_read_float(true_pts + i * 2 + 1), 5);
|
|
|
- SERIAL_ECHOPGM("), error: ");
|
|
|
- MYSERIAL.print(sqrt(sqr(measured_pts[i * 2] - x) + sqr(measured_pts[i * 2 + 1] - y)));
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- }
|
|
|
- if (verbosity_level >= 20) {
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- SERIAL_ECHOLNPGM("Calculate offset and skew returning result:");
|
|
|
- MYSERIAL.print(int(result));
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- }
|
|
|
- delay_keep_alive(100);
|
|
|
- }
|
|
|
-
|
|
|
- return result;
|
|
|
-}
|
|
|
-
|
|
|
-void reset_bed_offset_and_skew()
|
|
|
-{
|
|
|
- eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_CENTER+0), 0x0FFFFFFFF);
|
|
|
- eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_CENTER+4), 0x0FFFFFFFF);
|
|
|
- eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_X +0), 0x0FFFFFFFF);
|
|
|
- eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_X +4), 0x0FFFFFFFF);
|
|
|
- eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_Y +0), 0x0FFFFFFFF);
|
|
|
- eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_Y +4), 0x0FFFFFFFF);
|
|
|
-
|
|
|
-
|
|
|
- for (int8_t i = 0; i < 4; ++ i)
|
|
|
- eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_Z_JITTER+i*4), 0x0FFFFFFFF);
|
|
|
-}
|
|
|
-
|
|
|
-bool is_bed_z_jitter_data_valid()
|
|
|
-
|
|
|
-{
|
|
|
- for (int8_t i = 0; i < 8; ++ i)
|
|
|
- if (eeprom_read_word((uint16_t*)(EEPROM_BED_CALIBRATION_Z_JITTER+i*2)) == 0x0FFFF)
|
|
|
- return false;
|
|
|
- return true;
|
|
|
-}
|
|
|
-
|
|
|
-static void world2machine_update(const float vec_x[2], const float vec_y[2], const float cntr[2])
|
|
|
-{
|
|
|
- world2machine_rotation_and_skew[0][0] = vec_x[0];
|
|
|
- world2machine_rotation_and_skew[1][0] = vec_x[1];
|
|
|
- world2machine_rotation_and_skew[0][1] = vec_y[0];
|
|
|
- world2machine_rotation_and_skew[1][1] = vec_y[1];
|
|
|
- world2machine_shift[0] = cntr[0];
|
|
|
- world2machine_shift[1] = cntr[1];
|
|
|
-
|
|
|
- world2machine_correction_mode = WORLD2MACHINE_CORRECTION_NONE;
|
|
|
- if (world2machine_shift[0] != 0.f || world2machine_shift[1] != 0.f)
|
|
|
-
|
|
|
- world2machine_correction_mode |= WORLD2MACHINE_CORRECTION_SHIFT;
|
|
|
- if (world2machine_rotation_and_skew[0][0] != 1.f || world2machine_rotation_and_skew[0][1] != 0.f ||
|
|
|
- world2machine_rotation_and_skew[1][0] != 0.f || world2machine_rotation_and_skew[1][1] != 1.f) {
|
|
|
-
|
|
|
- world2machine_correction_mode |= WORLD2MACHINE_CORRECTION_SKEW;
|
|
|
-
|
|
|
- float d = world2machine_rotation_and_skew[0][0] * world2machine_rotation_and_skew[1][1] - world2machine_rotation_and_skew[1][0] * world2machine_rotation_and_skew[0][1];
|
|
|
- world2machine_rotation_and_skew_inv[0][0] = world2machine_rotation_and_skew[1][1] / d;
|
|
|
- world2machine_rotation_and_skew_inv[0][1] = -world2machine_rotation_and_skew[0][1] / d;
|
|
|
- world2machine_rotation_and_skew_inv[1][0] = -world2machine_rotation_and_skew[1][0] / d;
|
|
|
- world2machine_rotation_and_skew_inv[1][1] = world2machine_rotation_and_skew[0][0] / d;
|
|
|
- } else {
|
|
|
- world2machine_rotation_and_skew_inv[0][0] = 1.f;
|
|
|
- world2machine_rotation_and_skew_inv[0][1] = 0.f;
|
|
|
- world2machine_rotation_and_skew_inv[1][0] = 0.f;
|
|
|
- world2machine_rotation_and_skew_inv[1][1] = 1.f;
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void world2machine_reset()
|
|
|
-{
|
|
|
- const float vx[] = { 1.f, 0.f };
|
|
|
- const float vy[] = { 0.f, 1.f };
|
|
|
- const float cntr[] = { 0.f, 0.f };
|
|
|
- world2machine_update(vx, vy, cntr);
|
|
|
-}
|
|
|
-
|
|
|
-void world2machine_revert_to_uncorrected()
|
|
|
-{
|
|
|
- if (world2machine_correction_mode != WORLD2MACHINE_CORRECTION_NONE) {
|
|
|
-
|
|
|
- const float vx[] = { 1.f, 0.f };
|
|
|
- const float vy[] = { 0.f, 1.f };
|
|
|
- const float cntr[] = { 0.f, 0.f };
|
|
|
- world2machine_update(vx, vy, cntr);
|
|
|
-
|
|
|
- st_synchronize();
|
|
|
- current_position[X_AXIS] = st_get_position_mm(X_AXIS);
|
|
|
- current_position[Y_AXIS] = st_get_position_mm(Y_AXIS);
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-static inline bool vec_undef(const float v[2])
|
|
|
-{
|
|
|
- const uint32_t *vx = (const uint32_t*)v;
|
|
|
- return vx[0] == 0x0FFFFFFFF || vx[1] == 0x0FFFFFFFF;
|
|
|
-}
|
|
|
-
|
|
|
-void world2machine_initialize()
|
|
|
-{
|
|
|
- SERIAL_ECHOLNPGM("world2machine_initialize");
|
|
|
- float cntr[2] = {
|
|
|
- eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER+0)),
|
|
|
- eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER+4))
|
|
|
- };
|
|
|
- float vec_x[2] = {
|
|
|
- eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +0)),
|
|
|
- eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +4))
|
|
|
- };
|
|
|
- float vec_y[2] = {
|
|
|
- eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +0)),
|
|
|
- eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +4))
|
|
|
- };
|
|
|
-
|
|
|
- bool reset = false;
|
|
|
- if (vec_undef(cntr) || vec_undef(vec_x) || vec_undef(vec_y)) {
|
|
|
- SERIAL_ECHOLNPGM("Undefined bed correction matrix.");
|
|
|
- reset = true;
|
|
|
- }
|
|
|
- else {
|
|
|
-
|
|
|
- float l = sqrt(vec_x[0] * vec_x[0] + vec_x[1] * vec_x[1]);
|
|
|
- if (l < 0.9 || l > 1.1) {
|
|
|
- SERIAL_ECHOLNPGM("X vector length:");
|
|
|
- MYSERIAL.println(l);
|
|
|
- SERIAL_ECHOLNPGM("Invalid bed correction matrix. Length of the X vector out of range.");
|
|
|
- reset = true;
|
|
|
- }
|
|
|
-
|
|
|
- l = sqrt(vec_y[0] * vec_y[0] + vec_y[1] * vec_y[1]);
|
|
|
- if (l < 0.9 || l > 1.1) {
|
|
|
- SERIAL_ECHOLNPGM("Y vector length:");
|
|
|
- MYSERIAL.println(l);
|
|
|
- SERIAL_ECHOLNPGM("Invalid bed correction matrix. Length of the Y vector out of range.");
|
|
|
- reset = true;
|
|
|
- }
|
|
|
-
|
|
|
- l = sqrt(cntr[0] * cntr[0] + cntr[1] * cntr[1]);
|
|
|
- if (l > 15.f) {
|
|
|
- SERIAL_ECHOLNPGM("Zero point correction:");
|
|
|
- MYSERIAL.println(l);
|
|
|
- SERIAL_ECHOLNPGM("Invalid bed correction matrix. Shift out of range.");
|
|
|
- reset = true;
|
|
|
- }
|
|
|
-
|
|
|
- l = vec_x[0] * vec_y[0] + vec_x[1] * vec_y[1];
|
|
|
- if (fabs(l) > 0.1f) {
|
|
|
- SERIAL_ECHOLNPGM("Invalid bed correction matrix. X/Y axes are far from being perpendicular.");
|
|
|
- reset = true;
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- if (reset) {
|
|
|
- SERIAL_ECHOLNPGM("Invalid bed correction matrix. Resetting to identity.");
|
|
|
- reset_bed_offset_and_skew();
|
|
|
- world2machine_reset();
|
|
|
- } else {
|
|
|
- world2machine_update(vec_x, vec_y, cntr);
|
|
|
-
|
|
|
- SERIAL_ECHOPGM("world2machine_initialize() loaded: ");
|
|
|
- MYSERIAL.print(world2machine_rotation_and_skew[0][0], 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(world2machine_rotation_and_skew[0][1], 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(world2machine_rotation_and_skew[1][0], 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(world2machine_rotation_and_skew[1][1], 5);
|
|
|
- SERIAL_ECHOPGM(", offset ");
|
|
|
- MYSERIAL.print(world2machine_shift[0], 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(world2machine_shift[1], 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- */
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-void world2machine_update_current()
|
|
|
-{
|
|
|
- float x = current_position[X_AXIS] - world2machine_shift[0];
|
|
|
- float y = current_position[Y_AXIS] - world2machine_shift[1];
|
|
|
- current_position[X_AXIS] = world2machine_rotation_and_skew_inv[0][0] * x + world2machine_rotation_and_skew_inv[0][1] * y;
|
|
|
- current_position[Y_AXIS] = world2machine_rotation_and_skew_inv[1][0] * x + world2machine_rotation_and_skew_inv[1][1] * y;
|
|
|
-}
|
|
|
-
|
|
|
-static inline void go_xyz(float x, float y, float z, float fr)
|
|
|
-{
|
|
|
- plan_buffer_line(x, y, z, current_position[E_AXIS], fr, active_extruder);
|
|
|
- st_synchronize();
|
|
|
-}
|
|
|
-
|
|
|
-static inline void go_xy(float x, float y, float fr)
|
|
|
-{
|
|
|
- plan_buffer_line(x, y, current_position[Z_AXIS], current_position[E_AXIS], fr, active_extruder);
|
|
|
- st_synchronize();
|
|
|
-}
|
|
|
-
|
|
|
-static inline void go_to_current(float fr)
|
|
|
-{
|
|
|
- plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], fr, active_extruder);
|
|
|
- st_synchronize();
|
|
|
-}
|
|
|
-
|
|
|
-static inline void update_current_position_xyz()
|
|
|
-{
|
|
|
- current_position[X_AXIS] = st_get_position_mm(X_AXIS);
|
|
|
- current_position[Y_AXIS] = st_get_position_mm(Y_AXIS);
|
|
|
- current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
|
|
|
- plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
|
|
|
-}
|
|
|
-
|
|
|
-static inline void update_current_position_z()
|
|
|
-{
|
|
|
- current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
|
|
|
- plan_set_z_position(current_position[Z_AXIS]);
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-inline bool find_bed_induction_sensor_point_z(float minimum_z, uint8_t n_iter)
|
|
|
-{
|
|
|
- SERIAL_ECHOLNPGM("find bed induction sensor point z");
|
|
|
- bool endstops_enabled = enable_endstops(true);
|
|
|
- bool endstop_z_enabled = enable_z_endstop(false);
|
|
|
- float z = 0.f;
|
|
|
- endstop_z_hit_on_purpose();
|
|
|
-
|
|
|
-
|
|
|
- current_position[Z_AXIS] = minimum_z;
|
|
|
- go_to_current(homing_feedrate[Z_AXIS]/60);
|
|
|
-
|
|
|
- update_current_position_z();
|
|
|
- if (! endstop_z_hit_on_purpose())
|
|
|
- goto error;
|
|
|
-
|
|
|
- for (uint8_t i = 0; i < n_iter; ++ i) {
|
|
|
-
|
|
|
- current_position[Z_AXIS] += .5f;
|
|
|
- go_to_current(homing_feedrate[Z_AXIS]/60);
|
|
|
-
|
|
|
- current_position[Z_AXIS] = minimum_z;
|
|
|
- go_to_current(homing_feedrate[Z_AXIS]/(4*60));
|
|
|
-
|
|
|
- update_current_position_z();
|
|
|
- if (! endstop_z_hit_on_purpose())
|
|
|
- goto error;
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- z += current_position[Z_AXIS];
|
|
|
- }
|
|
|
- current_position[Z_AXIS] = z;
|
|
|
- if (n_iter > 1)
|
|
|
- current_position[Z_AXIS] /= float(n_iter);
|
|
|
-
|
|
|
- enable_endstops(endstops_enabled);
|
|
|
- enable_z_endstop(endstop_z_enabled);
|
|
|
-
|
|
|
- return true;
|
|
|
-
|
|
|
-error:
|
|
|
-
|
|
|
- enable_endstops(endstops_enabled);
|
|
|
- enable_z_endstop(endstop_z_enabled);
|
|
|
- return false;
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-#define FIND_BED_INDUCTION_SENSOR_POINT_X_RADIUS (8.f)
|
|
|
-#define FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS (6.f)
|
|
|
-#define FIND_BED_INDUCTION_SENSOR_POINT_XY_STEP (1.f)
|
|
|
-#define FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP (0.2f)
|
|
|
-inline bool find_bed_induction_sensor_point_xy()
|
|
|
-{
|
|
|
- MYSERIAL.println("find bed induction sensor point xy");
|
|
|
- float feedrate = homing_feedrate[X_AXIS] / 60.f;
|
|
|
- bool found = false;
|
|
|
-
|
|
|
- {
|
|
|
- float x0 = current_position[X_AXIS] - FIND_BED_INDUCTION_SENSOR_POINT_X_RADIUS;
|
|
|
- float x1 = current_position[X_AXIS] + FIND_BED_INDUCTION_SENSOR_POINT_X_RADIUS;
|
|
|
- float y0 = current_position[Y_AXIS] - FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS;
|
|
|
- float y1 = current_position[Y_AXIS] + FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS;
|
|
|
- uint8_t nsteps_y;
|
|
|
- uint8_t i;
|
|
|
- if (x0 < X_MIN_POS)
|
|
|
- x0 = X_MIN_POS;
|
|
|
- if (x1 > X_MAX_POS)
|
|
|
- x1 = X_MAX_POS;
|
|
|
- if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION)
|
|
|
- y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
|
|
|
- if (y1 > Y_MAX_POS)
|
|
|
- y1 = Y_MAX_POS;
|
|
|
- nsteps_y = int(ceil((y1 - y0) / FIND_BED_INDUCTION_SENSOR_POINT_XY_STEP));
|
|
|
-
|
|
|
- enable_endstops(false);
|
|
|
- bool dir_positive = true;
|
|
|
-
|
|
|
-
|
|
|
- go_xyz(x0, y0, current_position[Z_AXIS], feedrate);
|
|
|
-
|
|
|
- endstops_hit_on_purpose();
|
|
|
- enable_z_endstop(true);
|
|
|
- while (current_position[Z_AXIS] > -10.f) {
|
|
|
-
|
|
|
- current_position[Y_AXIS] = y0;
|
|
|
- for (i = 0; i < nsteps_y; current_position[Y_AXIS] += (y1 - y0) / float(nsteps_y - 1), ++ i) {
|
|
|
-
|
|
|
- current_position[Z_AXIS] -= FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP / float(nsteps_y);
|
|
|
- go_xyz(dir_positive ? x1 : x0, current_position[Y_AXIS], current_position[Z_AXIS], feedrate);
|
|
|
- dir_positive = ! dir_positive;
|
|
|
- if (endstop_z_hit_on_purpose())
|
|
|
- goto endloop;
|
|
|
- }
|
|
|
- for (i = 0; i < nsteps_y; current_position[Y_AXIS] -= (y1 - y0) / float(nsteps_y - 1), ++ i) {
|
|
|
-
|
|
|
- current_position[Z_AXIS] -= FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP / float(nsteps_y);
|
|
|
- go_xyz(dir_positive ? x1 : x0, current_position[Y_AXIS], current_position[Z_AXIS], feedrate);
|
|
|
- dir_positive = ! dir_positive;
|
|
|
- if (endstop_z_hit_on_purpose())
|
|
|
- goto endloop;
|
|
|
- }
|
|
|
- }
|
|
|
- endloop:
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- update_current_position_xyz();
|
|
|
-
|
|
|
-
|
|
|
- for (int8_t iter = 0; iter < 3; ++ iter) {
|
|
|
- if (iter > 0) {
|
|
|
-
|
|
|
- current_position[Z_AXIS] -= 0.02f;
|
|
|
- go_xyz(current_position[X_AXIS], current_position[Y_AXIS], MESH_HOME_Z_SEARCH, homing_feedrate[Z_AXIS]/60);
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
- float a, b;
|
|
|
- enable_endstops(false);
|
|
|
- enable_z_endstop(false);
|
|
|
- current_position[Y_AXIS] = y0;
|
|
|
- go_xy(x0, current_position[Y_AXIS], feedrate);
|
|
|
- enable_z_endstop(true);
|
|
|
- found = false;
|
|
|
- for (i = 0, dir_positive = true; i < nsteps_y; current_position[Y_AXIS] += (y1 - y0) / float(nsteps_y - 1), ++ i, dir_positive = ! dir_positive) {
|
|
|
- go_xy(dir_positive ? x1 : x0, current_position[Y_AXIS], feedrate);
|
|
|
- if (endstop_z_hit_on_purpose()) {
|
|
|
- found = true;
|
|
|
- break;
|
|
|
- }
|
|
|
- }
|
|
|
- update_current_position_xyz();
|
|
|
- if (! found) {
|
|
|
-
|
|
|
- continue;
|
|
|
- }
|
|
|
-
|
|
|
- a = current_position[Y_AXIS];
|
|
|
-
|
|
|
- enable_z_endstop(false);
|
|
|
- current_position[Y_AXIS] = y1;
|
|
|
- go_xy(x0, current_position[Y_AXIS], feedrate);
|
|
|
- enable_z_endstop(true);
|
|
|
- found = false;
|
|
|
- for (i = 0, dir_positive = true; i < nsteps_y; current_position[Y_AXIS] -= (y1 - y0) / float(nsteps_y - 1), ++ i, dir_positive = ! dir_positive) {
|
|
|
- go_xy(dir_positive ? x1 : x0, current_position[Y_AXIS], feedrate);
|
|
|
- if (endstop_z_hit_on_purpose()) {
|
|
|
- found = true;
|
|
|
- break;
|
|
|
- }
|
|
|
- }
|
|
|
- update_current_position_xyz();
|
|
|
- if (! found) {
|
|
|
-
|
|
|
- continue;
|
|
|
- }
|
|
|
-
|
|
|
- b = current_position[Y_AXIS];
|
|
|
- current_position[Y_AXIS] = 0.5f * (a + b);
|
|
|
-
|
|
|
-
|
|
|
- found = false;
|
|
|
- enable_z_endstop(false);
|
|
|
- go_xy(x0, current_position[Y_AXIS], feedrate);
|
|
|
- enable_z_endstop(true);
|
|
|
- go_xy(x1, current_position[Y_AXIS], feedrate);
|
|
|
- update_current_position_xyz();
|
|
|
- if (! endstop_z_hit_on_purpose()) {
|
|
|
-
|
|
|
- continue;
|
|
|
- }
|
|
|
-
|
|
|
- a = current_position[X_AXIS];
|
|
|
- enable_z_endstop(false);
|
|
|
- go_xy(x1, current_position[Y_AXIS], feedrate);
|
|
|
- enable_z_endstop(true);
|
|
|
- go_xy(x0, current_position[Y_AXIS], feedrate);
|
|
|
- update_current_position_xyz();
|
|
|
- if (! endstop_z_hit_on_purpose()) {
|
|
|
-
|
|
|
- continue;
|
|
|
- }
|
|
|
-
|
|
|
- b = current_position[X_AXIS];
|
|
|
-
|
|
|
- enable_z_endstop(false);
|
|
|
- current_position[X_AXIS] = 0.5f * (a + b);
|
|
|
- go_xy(current_position[X_AXIS], current_position[Y_AXIS], feedrate);
|
|
|
- found = true;
|
|
|
-
|
|
|
-#if 1
|
|
|
-
|
|
|
- found = false;
|
|
|
- enable_z_endstop(false);
|
|
|
- go_xy(current_position[X_AXIS], y0, feedrate);
|
|
|
- enable_z_endstop(true);
|
|
|
- go_xy(current_position[X_AXIS], y1, feedrate);
|
|
|
- update_current_position_xyz();
|
|
|
- if (! endstop_z_hit_on_purpose()) {
|
|
|
-
|
|
|
- continue;
|
|
|
- }
|
|
|
-
|
|
|
- a = current_position[Y_AXIS];
|
|
|
- enable_z_endstop(false);
|
|
|
- go_xy(current_position[X_AXIS], y1, feedrate);
|
|
|
- enable_z_endstop(true);
|
|
|
- go_xy(current_position[X_AXIS], y0, feedrate);
|
|
|
- update_current_position_xyz();
|
|
|
- if (! endstop_z_hit_on_purpose()) {
|
|
|
-
|
|
|
- continue;
|
|
|
- }
|
|
|
-
|
|
|
- b = current_position[Y_AXIS];
|
|
|
-
|
|
|
- enable_z_endstop(false);
|
|
|
- current_position[Y_AXIS] = 0.5f * (a + b);
|
|
|
- go_xy(current_position[X_AXIS], current_position[Y_AXIS], feedrate);
|
|
|
- found = true;
|
|
|
-#endif
|
|
|
- break;
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- enable_z_endstop(false);
|
|
|
- return found;
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-inline bool improve_bed_induction_sensor_point()
|
|
|
-{
|
|
|
- static const float search_radius = 8.f;
|
|
|
-
|
|
|
- bool endstops_enabled = enable_endstops(false);
|
|
|
- bool endstop_z_enabled = enable_z_endstop(false);
|
|
|
- bool found = false;
|
|
|
- float feedrate = homing_feedrate[X_AXIS] / 60.f;
|
|
|
- float center_old_x = current_position[X_AXIS];
|
|
|
- float center_old_y = current_position[Y_AXIS];
|
|
|
- float center_x = 0.f;
|
|
|
- float center_y = 0.f;
|
|
|
-
|
|
|
- for (uint8_t iter = 0; iter < 4; ++ iter) {
|
|
|
- switch (iter) {
|
|
|
- case 0:
|
|
|
- destination[X_AXIS] = center_old_x - search_radius * 0.707;
|
|
|
- destination[Y_AXIS] = center_old_y - search_radius * 0.707;
|
|
|
- break;
|
|
|
- case 1:
|
|
|
- destination[X_AXIS] = center_old_x + search_radius * 0.707;
|
|
|
- destination[Y_AXIS] = center_old_y + search_radius * 0.707;
|
|
|
- break;
|
|
|
- case 2:
|
|
|
- destination[X_AXIS] = center_old_x + search_radius * 0.707;
|
|
|
- destination[Y_AXIS] = center_old_y - search_radius * 0.707;
|
|
|
- break;
|
|
|
- case 3:
|
|
|
- default:
|
|
|
- destination[X_AXIS] = center_old_x - search_radius * 0.707;
|
|
|
- destination[Y_AXIS] = center_old_y + search_radius * 0.707;
|
|
|
- break;
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
- float vx = destination[X_AXIS] - center_old_x;
|
|
|
- float vy = destination[Y_AXIS] - center_old_y;
|
|
|
- float l = sqrt(vx*vx+vy*vy);
|
|
|
- float t;
|
|
|
- if (destination[X_AXIS] < X_MIN_POS) {
|
|
|
-
|
|
|
- t = (center_x - X_MIN_POS) / l;
|
|
|
- destination[X_AXIS] = X_MIN_POS;
|
|
|
- destination[Y_AXIS] = center_old_y + t * vy;
|
|
|
- } else if (destination[X_AXIS] > X_MAX_POS) {
|
|
|
-
|
|
|
- t = (X_MAX_POS - center_x) / l;
|
|
|
- destination[X_AXIS] = X_MAX_POS;
|
|
|
- destination[Y_AXIS] = center_old_y + t * vy;
|
|
|
- }
|
|
|
- if (destination[Y_AXIS] < Y_MIN_POS_FOR_BED_CALIBRATION) {
|
|
|
-
|
|
|
- t = (center_y - Y_MIN_POS_FOR_BED_CALIBRATION) / l;
|
|
|
- destination[X_AXIS] = center_old_x + t * vx;
|
|
|
- destination[Y_AXIS] = Y_MIN_POS_FOR_BED_CALIBRATION;
|
|
|
- } else if (destination[Y_AXIS] > Y_MAX_POS) {
|
|
|
-
|
|
|
- t = (Y_MAX_POS - center_y) / l;
|
|
|
- destination[X_AXIS] = center_old_x + t * vx;
|
|
|
- destination[Y_AXIS] = Y_MAX_POS;
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
- enable_endstops(false);
|
|
|
- go_xy(destination[X_AXIS], destination[Y_AXIS], feedrate);
|
|
|
-
|
|
|
- enable_endstops(true);
|
|
|
- go_xy(center_old_x, center_old_y, feedrate);
|
|
|
- update_current_position_xyz();
|
|
|
-
|
|
|
- center_x += current_position[X_AXIS];
|
|
|
- center_y += current_position[Y_AXIS];
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
- center_x /= 4.f;
|
|
|
- center_y /= 4.f;
|
|
|
- current_position[X_AXIS] = center_x;
|
|
|
- current_position[Y_AXIS] = center_y;
|
|
|
- enable_endstops(false);
|
|
|
- go_xy(current_position[X_AXIS], current_position[Y_AXIS], feedrate);
|
|
|
-
|
|
|
- enable_endstops(endstops_enabled);
|
|
|
- enable_z_endstop(endstop_z_enabled);
|
|
|
- return found;
|
|
|
-}
|
|
|
-
|
|
|
-static inline void debug_output_point(const char *type, const float &x, const float &y, const float &z)
|
|
|
-{
|
|
|
- SERIAL_ECHOPGM("Measured ");
|
|
|
- SERIAL_ECHORPGM(type);
|
|
|
- SERIAL_ECHOPGM(" ");
|
|
|
- MYSERIAL.print(x, 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(y, 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(z, 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-#define IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS (8.f)
|
|
|
-inline bool improve_bed_induction_sensor_point2(bool lift_z_on_min_y, int8_t verbosity_level)
|
|
|
-{
|
|
|
- float center_old_x = current_position[X_AXIS];
|
|
|
- float center_old_y = current_position[Y_AXIS];
|
|
|
- float a, b;
|
|
|
- bool point_small = false;
|
|
|
-
|
|
|
- enable_endstops(false);
|
|
|
-
|
|
|
- {
|
|
|
- float x0 = center_old_x - IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS;
|
|
|
- float x1 = center_old_x + IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS;
|
|
|
- if (x0 < X_MIN_POS)
|
|
|
- x0 = X_MIN_POS;
|
|
|
- if (x1 > X_MAX_POS)
|
|
|
- x1 = X_MAX_POS;
|
|
|
-
|
|
|
-
|
|
|
- enable_z_endstop(false);
|
|
|
- go_xy(x0, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
|
|
|
- enable_z_endstop(true);
|
|
|
- go_xy(x1, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
|
|
|
- update_current_position_xyz();
|
|
|
- if (! endstop_z_hit_on_purpose()) {
|
|
|
- current_position[X_AXIS] = center_old_x;
|
|
|
- goto canceled;
|
|
|
- }
|
|
|
- a = current_position[X_AXIS];
|
|
|
- enable_z_endstop(false);
|
|
|
- go_xy(x1, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
|
|
|
- enable_z_endstop(true);
|
|
|
- go_xy(x0, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
|
|
|
- update_current_position_xyz();
|
|
|
- if (! endstop_z_hit_on_purpose()) {
|
|
|
- current_position[X_AXIS] = center_old_x;
|
|
|
- goto canceled;
|
|
|
- }
|
|
|
- b = current_position[X_AXIS];
|
|
|
- if (b - a < MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
|
|
|
- if (verbosity_level >= 5) {
|
|
|
- SERIAL_ECHOPGM("Point width too small: ");
|
|
|
- SERIAL_ECHO(b - a);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- }
|
|
|
-
|
|
|
- if (b - a < 0.5f * MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
|
|
|
-
|
|
|
- current_position[X_AXIS] = center_old_x;
|
|
|
- goto canceled;
|
|
|
- } else {
|
|
|
-
|
|
|
- point_small = true;
|
|
|
- }
|
|
|
- }
|
|
|
- if (verbosity_level >= 5) {
|
|
|
- debug_output_point(PSTR("left" ), a, current_position[Y_AXIS], current_position[Z_AXIS]);
|
|
|
- debug_output_point(PSTR("right"), b, current_position[Y_AXIS], current_position[Z_AXIS]);
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
- enable_z_endstop(false);
|
|
|
- current_position[X_AXIS] = 0.5f * (a + b);
|
|
|
- go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
|
|
|
- }
|
|
|
-
|
|
|
- {
|
|
|
- float y0 = center_old_y - IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS;
|
|
|
- float y1 = center_old_y + IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS;
|
|
|
- if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION)
|
|
|
- y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
|
|
|
- if (y1 > Y_MAX_POS)
|
|
|
- y1 = Y_MAX_POS;
|
|
|
-
|
|
|
-
|
|
|
- enable_z_endstop(false);
|
|
|
- go_xy(current_position[X_AXIS], y0, homing_feedrate[X_AXIS] / 60.f);
|
|
|
- if (lift_z_on_min_y) {
|
|
|
-
|
|
|
-
|
|
|
- go_xyz(current_position[X_AXIS], y0, current_position[Z_AXIS]+1.5f, homing_feedrate[Z_AXIS] / 60.f);
|
|
|
-
|
|
|
- go_xyz(current_position[X_AXIS], y0, current_position[Z_AXIS], homing_feedrate[Z_AXIS] / 60.f);
|
|
|
- }
|
|
|
- if (lift_z_on_min_y && (READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING) == 1) {
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- a = current_position[Y_AXIS];
|
|
|
- } else {
|
|
|
- enable_z_endstop(true);
|
|
|
- go_xy(current_position[X_AXIS], y1, homing_feedrate[X_AXIS] / 60.f);
|
|
|
- update_current_position_xyz();
|
|
|
- if (! endstop_z_hit_on_purpose()) {
|
|
|
- current_position[Y_AXIS] = center_old_y;
|
|
|
- goto canceled;
|
|
|
- }
|
|
|
- a = current_position[Y_AXIS];
|
|
|
- }
|
|
|
- enable_z_endstop(false);
|
|
|
- go_xy(current_position[X_AXIS], y1, homing_feedrate[X_AXIS] / 60.f);
|
|
|
- enable_z_endstop(true);
|
|
|
- go_xy(current_position[X_AXIS], y0, homing_feedrate[X_AXIS] / 60.f);
|
|
|
- update_current_position_xyz();
|
|
|
- if (! endstop_z_hit_on_purpose()) {
|
|
|
- current_position[Y_AXIS] = center_old_y;
|
|
|
- goto canceled;
|
|
|
- }
|
|
|
- b = current_position[Y_AXIS];
|
|
|
- if (b - a < MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
|
|
|
-
|
|
|
- if (verbosity_level >= 5) {
|
|
|
- SERIAL_ECHOPGM("Point height too small: ");
|
|
|
- SERIAL_ECHO(b - a);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- }
|
|
|
- if (b - a < 0.5f * MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
|
|
|
-
|
|
|
- current_position[Y_AXIS] = center_old_y;
|
|
|
- goto canceled;
|
|
|
- } else {
|
|
|
-
|
|
|
- point_small = true;
|
|
|
- }
|
|
|
- }
|
|
|
- if (verbosity_level >= 5) {
|
|
|
- debug_output_point(PSTR("top" ), current_position[X_AXIS], a, current_position[Z_AXIS]);
|
|
|
- debug_output_point(PSTR("bottom"), current_position[X_AXIS], b, current_position[Z_AXIS]);
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
- enable_z_endstop(false);
|
|
|
- current_position[Y_AXIS] = 0.5f * (a + b);
|
|
|
- go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- return ! point_small;
|
|
|
-
|
|
|
-canceled:
|
|
|
-
|
|
|
- enable_z_endstop(false);
|
|
|
- go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
|
|
|
- return false;
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-#define IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS (4.f)
|
|
|
-#define IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y (0.1f)
|
|
|
-inline bool improve_bed_induction_sensor_point3(int verbosity_level)
|
|
|
-{
|
|
|
- float center_old_x = current_position[X_AXIS];
|
|
|
- float center_old_y = current_position[Y_AXIS];
|
|
|
- float a, b;
|
|
|
- bool result = true;
|
|
|
-
|
|
|
- if (verbosity_level >= 20) MYSERIAL.println("Improve bed induction sensor point3");
|
|
|
-
|
|
|
-
|
|
|
- {
|
|
|
- float x0 = center_old_x - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
|
|
|
- float x1 = center_old_x + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
|
|
|
- float y0 = center_old_y - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
|
|
|
- float y1 = center_old_y + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
|
|
|
- float y = y0;
|
|
|
-
|
|
|
- if (x0 < X_MIN_POS)
|
|
|
- x0 = X_MIN_POS;
|
|
|
- if (x1 > X_MAX_POS)
|
|
|
- x1 = X_MAX_POS;
|
|
|
- if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION)
|
|
|
- y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
|
|
|
- if (y1 > Y_MAX_POS)
|
|
|
- y1 = Y_MAX_POS;
|
|
|
-
|
|
|
- if (verbosity_level >= 20) {
|
|
|
- SERIAL_ECHOPGM("Initial position: ");
|
|
|
- SERIAL_ECHO(center_old_x);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- SERIAL_ECHO(center_old_y);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- float dmax = 0.f;
|
|
|
- float xmax1 = 0.f;
|
|
|
- float xmax2 = 0.f;
|
|
|
- for (y = y0; y < y1; y += IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
|
|
|
- enable_z_endstop(false);
|
|
|
- go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
- enable_z_endstop(true);
|
|
|
- go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
- update_current_position_xyz();
|
|
|
- if (! endstop_z_hit_on_purpose()) {
|
|
|
- continue;
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- }
|
|
|
- a = current_position[X_AXIS];
|
|
|
- enable_z_endstop(false);
|
|
|
- go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
- enable_z_endstop(true);
|
|
|
- go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
- update_current_position_xyz();
|
|
|
- if (! endstop_z_hit_on_purpose()) {
|
|
|
- continue;
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- }
|
|
|
- b = current_position[X_AXIS];
|
|
|
- if (verbosity_level >= 5) {
|
|
|
- debug_output_point(PSTR("left" ), a, current_position[Y_AXIS], current_position[Z_AXIS]);
|
|
|
- debug_output_point(PSTR("right"), b, current_position[Y_AXIS], current_position[Z_AXIS]);
|
|
|
- }
|
|
|
- float d = b - a;
|
|
|
- if (d > dmax) {
|
|
|
- xmax1 = 0.5f * (a + b);
|
|
|
- dmax = d;
|
|
|
- } else if (dmax > 0.) {
|
|
|
- y0 = y - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y;
|
|
|
- break;
|
|
|
- }
|
|
|
- }
|
|
|
- if (dmax == 0.) {
|
|
|
- if (verbosity_level > 0)
|
|
|
- SERIAL_PROTOCOLPGM("failed - not found\n");
|
|
|
- current_position[X_AXIS] = center_old_x;
|
|
|
- current_position[Y_AXIS] = center_old_y;
|
|
|
- goto canceled;
|
|
|
- }
|
|
|
-
|
|
|
- {
|
|
|
-
|
|
|
- enable_z_endstop(false);
|
|
|
- go_xy(xmax1, y0 + IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS, homing_feedrate[X_AXIS] / 60.f);
|
|
|
- enable_z_endstop(true);
|
|
|
- go_xy(xmax1, max(y0 - IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS, Y_MIN_POS_FOR_BED_CALIBRATION), homing_feedrate[X_AXIS] / 60.f);
|
|
|
- update_current_position_xyz();
|
|
|
- if (! endstop_z_hit_on_purpose()) {
|
|
|
- current_position[Y_AXIS] = center_old_y;
|
|
|
- goto canceled;
|
|
|
- }
|
|
|
- if (verbosity_level >= 5)
|
|
|
- debug_output_point(PSTR("top" ), current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
|
|
|
- y1 = current_position[Y_AXIS];
|
|
|
- }
|
|
|
-
|
|
|
- if (y1 <= y0) {
|
|
|
-
|
|
|
- current_position[Y_AXIS] = center_old_y;
|
|
|
- goto canceled;
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
- dmax = 0.f;
|
|
|
-
|
|
|
-
|
|
|
- for (y = y1; y >= y0; y -= IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
|
|
|
- enable_z_endstop(false);
|
|
|
- go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
- enable_z_endstop(true);
|
|
|
- go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
- update_current_position_xyz();
|
|
|
- if (! endstop_z_hit_on_purpose()) {
|
|
|
- continue;
|
|
|
-
|
|
|
- current_position[X_AXIS] = center_old_x;
|
|
|
- SERIAL_PROTOCOLPGM("Failed 3\n");
|
|
|
- goto canceled;
|
|
|
- */
|
|
|
- }
|
|
|
- a = current_position[X_AXIS];
|
|
|
- enable_z_endstop(false);
|
|
|
- go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
- enable_z_endstop(true);
|
|
|
- go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
- update_current_position_xyz();
|
|
|
- if (! endstop_z_hit_on_purpose()) {
|
|
|
- continue;
|
|
|
-
|
|
|
- current_position[X_AXIS] = center_old_x;
|
|
|
- SERIAL_PROTOCOLPGM("Failed 4\n");
|
|
|
- goto canceled;
|
|
|
- */
|
|
|
- }
|
|
|
- b = current_position[X_AXIS];
|
|
|
- if (verbosity_level >= 5) {
|
|
|
- debug_output_point(PSTR("left" ), a, current_position[Y_AXIS], current_position[Z_AXIS]);
|
|
|
- debug_output_point(PSTR("right"), b, current_position[Y_AXIS], current_position[Z_AXIS]);
|
|
|
- }
|
|
|
- float d = b - a;
|
|
|
- if (d > dmax) {
|
|
|
- xmax2 = 0.5f * (a + b);
|
|
|
- dmax = d;
|
|
|
- } else if (dmax > 0.) {
|
|
|
- y1 = y + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y;
|
|
|
- break;
|
|
|
- }
|
|
|
- }
|
|
|
- float xmax, ymax;
|
|
|
- if (dmax == 0.f) {
|
|
|
-
|
|
|
- xmax = xmax1;
|
|
|
- ymax = y0;
|
|
|
- } else {
|
|
|
-
|
|
|
- xmax = xmax2;
|
|
|
- ymax = 0.5f * (y0 + y1);
|
|
|
- for (; y >= y0; y -= IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
|
|
|
- enable_z_endstop(false);
|
|
|
- go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
- enable_z_endstop(true);
|
|
|
- go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
- update_current_position_xyz();
|
|
|
- if (! endstop_z_hit_on_purpose()) {
|
|
|
- continue;
|
|
|
-
|
|
|
- current_position[X_AXIS] = center_old_x;
|
|
|
- SERIAL_PROTOCOLPGM("Failed 3\n");
|
|
|
- goto canceled;
|
|
|
- */
|
|
|
- }
|
|
|
- a = current_position[X_AXIS];
|
|
|
- enable_z_endstop(false);
|
|
|
- go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
- enable_z_endstop(true);
|
|
|
- go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
- update_current_position_xyz();
|
|
|
- if (! endstop_z_hit_on_purpose()) {
|
|
|
- continue;
|
|
|
-
|
|
|
- current_position[X_AXIS] = center_old_x;
|
|
|
- SERIAL_PROTOCOLPGM("Failed 4\n");
|
|
|
- goto canceled;
|
|
|
- */
|
|
|
- }
|
|
|
- b = current_position[X_AXIS];
|
|
|
- if (verbosity_level >= 5) {
|
|
|
- debug_output_point(PSTR("left" ), a, current_position[Y_AXIS], current_position[Z_AXIS]);
|
|
|
- debug_output_point(PSTR("right"), b, current_position[Y_AXIS], current_position[Z_AXIS]);
|
|
|
- }
|
|
|
- float d = b - a;
|
|
|
- if (d > dmax) {
|
|
|
- xmax = 0.5f * (a + b);
|
|
|
- ymax = y;
|
|
|
- dmax = d;
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- {
|
|
|
-
|
|
|
-
|
|
|
- enable_z_endstop(false);
|
|
|
- go_xy(xmax, ymax + IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS, homing_feedrate[X_AXIS] / 60.f);
|
|
|
- enable_z_endstop(true);
|
|
|
- go_xy(xmax, max(ymax - IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS, Y_MIN_POS_FOR_BED_CALIBRATION), homing_feedrate[X_AXIS] / 60.f);
|
|
|
- update_current_position_xyz();
|
|
|
- if (! endstop_z_hit_on_purpose()) {
|
|
|
- current_position[Y_AXIS] = center_old_y;
|
|
|
- goto canceled;
|
|
|
- }
|
|
|
- if (verbosity_level >= 5)
|
|
|
- debug_output_point(PSTR("top" ), current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
|
|
|
- if (current_position[Y_AXIS] - Y_MIN_POS_FOR_BED_CALIBRATION < 0.5f * dmax) {
|
|
|
-
|
|
|
-
|
|
|
- if (current_position[Y_AXIS] < ymax || dmax < 0.5f * MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
|
|
|
- if (verbosity_level >= 5) {
|
|
|
- SERIAL_ECHOPGM("Partial point diameter too small: ");
|
|
|
- SERIAL_ECHO(dmax);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- }
|
|
|
- result = false;
|
|
|
- } else {
|
|
|
-
|
|
|
- float h = current_position[Y_AXIS] - ymax;
|
|
|
- float r = dmax * dmax / (8.f * h) + 0.5f * h;
|
|
|
- if (r < 0.8f * MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
|
|
|
- if (verbosity_level >= 5) {
|
|
|
- SERIAL_ECHOPGM("Partial point estimated radius too small: ");
|
|
|
- SERIAL_ECHO(r);
|
|
|
- SERIAL_ECHOPGM(", dmax:");
|
|
|
- SERIAL_ECHO(dmax);
|
|
|
- SERIAL_ECHOPGM(", h:");
|
|
|
- SERIAL_ECHO(h);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- }
|
|
|
- result = false;
|
|
|
- } else {
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- r = 0.5f * (0.5f * dmax + r);
|
|
|
- ymax = current_position[Y_AXIS] - r;
|
|
|
- }
|
|
|
- }
|
|
|
- } else {
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- result = xmax >= MIN_BED_SENSOR_POINT_RESPONSE_DMR;
|
|
|
- if (y0 > Y_MIN_POS_FOR_BED_CALIBRATION + 0.2f)
|
|
|
-
|
|
|
-
|
|
|
- ymax = 0.5f * ymax + 0.25f * (y0 + y1);
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
- enable_z_endstop(false);
|
|
|
- current_position[X_AXIS] = xmax;
|
|
|
- current_position[Y_AXIS] = ymax;
|
|
|
- if (verbosity_level >= 20) {
|
|
|
- SERIAL_ECHOPGM("Adjusted position: ");
|
|
|
- SERIAL_ECHO(current_position[X_AXIS]);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- SERIAL_ECHO(current_position[Y_AXIS]);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- go_xy(current_position[X_AXIS], max(Y_MIN_POS, current_position[Y_AXIS]), homing_feedrate[X_AXIS] / 60.f);
|
|
|
-
|
|
|
- }
|
|
|
-
|
|
|
- if (result)
|
|
|
- return true;
|
|
|
-
|
|
|
-
|
|
|
-canceled:
|
|
|
-
|
|
|
- enable_z_endstop(false);
|
|
|
- if (current_position[Y_AXIS] < Y_MIN_POS)
|
|
|
- current_position[Y_AXIS] = Y_MIN_POS;
|
|
|
- go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
|
|
|
- return false;
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-inline void scan_bed_induction_sensor_point()
|
|
|
-{
|
|
|
- float center_old_x = current_position[X_AXIS];
|
|
|
- float center_old_y = current_position[Y_AXIS];
|
|
|
- float x0 = center_old_x - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
|
|
|
- float x1 = center_old_x + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
|
|
|
- float y0 = center_old_y - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
|
|
|
- float y1 = center_old_y + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
|
|
|
- float y = y0;
|
|
|
-
|
|
|
- if (x0 < X_MIN_POS)
|
|
|
- x0 = X_MIN_POS;
|
|
|
- if (x1 > X_MAX_POS)
|
|
|
- x1 = X_MAX_POS;
|
|
|
- if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION)
|
|
|
- y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
|
|
|
- if (y1 > Y_MAX_POS)
|
|
|
- y1 = Y_MAX_POS;
|
|
|
-
|
|
|
- for (float y = y0; y < y1; y += IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
|
|
|
- enable_z_endstop(false);
|
|
|
- go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
- enable_z_endstop(true);
|
|
|
- go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
- update_current_position_xyz();
|
|
|
- if (endstop_z_hit_on_purpose())
|
|
|
- debug_output_point(PSTR("left" ), current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
|
|
|
- enable_z_endstop(false);
|
|
|
- go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
- enable_z_endstop(true);
|
|
|
- go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
- update_current_position_xyz();
|
|
|
- if (endstop_z_hit_on_purpose())
|
|
|
- debug_output_point(PSTR("right"), current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
|
|
|
- }
|
|
|
-
|
|
|
- enable_z_endstop(false);
|
|
|
- current_position[X_AXIS] = center_old_x;
|
|
|
- current_position[Y_AXIS] = center_old_y;
|
|
|
- go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
|
|
|
-}
|
|
|
-
|
|
|
-#define MESH_BED_CALIBRATION_SHOW_LCD
|
|
|
-
|
|
|
-BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level, uint8_t &too_far_mask)
|
|
|
-{
|
|
|
-
|
|
|
- refresh_cmd_timeout();
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- float *pts = &mbl.z_values[0][0];
|
|
|
- float *vec_x = pts + 2 * 4;
|
|
|
- float *vec_y = vec_x + 2;
|
|
|
- float *cntr = vec_y + 2;
|
|
|
- memset(pts, 0, sizeof(float) * 7 * 7);
|
|
|
- uint8_t iteration = 0;
|
|
|
- BedSkewOffsetDetectionResultType result;
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- while (iteration < 3) {
|
|
|
-
|
|
|
- SERIAL_ECHOPGM("Iteration: ");
|
|
|
- MYSERIAL.println(int(iteration + 1));
|
|
|
- if (verbosity_level >= 20) {
|
|
|
- SERIAL_ECHOLNPGM("Vectors: ");
|
|
|
-
|
|
|
- SERIAL_ECHOPGM("vec_x[0]:");
|
|
|
- MYSERIAL.print(vec_x[0], 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- SERIAL_ECHOPGM("vec_x[1]:");
|
|
|
- MYSERIAL.print(vec_x[1], 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- SERIAL_ECHOPGM("vec_y[0]:");
|
|
|
- MYSERIAL.print(vec_y[0], 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- SERIAL_ECHOPGM("vec_y[1]:");
|
|
|
- MYSERIAL.print(vec_y[1], 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- SERIAL_ECHOPGM("cntr[0]:");
|
|
|
- MYSERIAL.print(cntr[0], 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- SERIAL_ECHOPGM("cntr[1]:");
|
|
|
- MYSERIAL.print(cntr[1], 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- }
|
|
|
-#ifdef MESH_BED_CALIBRATION_SHOW_LCD
|
|
|
- uint8_t next_line;
|
|
|
- lcd_display_message_fullscreen_P(MSG_FIND_BED_OFFSET_AND_SKEW_LINE1, next_line);
|
|
|
- if (next_line > 3)
|
|
|
- next_line = 3;
|
|
|
-#endif
|
|
|
-
|
|
|
-
|
|
|
- current_position[Z_AXIS] = MESH_HOME_Z_SEARCH + FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP * iteration * 0.3;
|
|
|
- for (int k = 0; k < 4; ++k) {
|
|
|
-
|
|
|
- refresh_cmd_timeout();
|
|
|
-#ifdef MESH_BED_CALIBRATION_SHOW_LCD
|
|
|
- lcd_implementation_print_at(0, next_line, k + 1);
|
|
|
- lcd_printPGM(MSG_FIND_BED_OFFSET_AND_SKEW_LINE2);
|
|
|
-
|
|
|
- if (iteration > 0) {
|
|
|
- lcd_print_at_PGM(0, next_line + 1, MSG_FIND_BED_OFFSET_AND_SKEW_ITERATION);
|
|
|
- lcd_implementation_print(int(iteration + 1));
|
|
|
- }
|
|
|
-#endif
|
|
|
- float *pt = pts + k * 2;
|
|
|
-
|
|
|
-
|
|
|
- go_to_current(homing_feedrate[Z_AXIS] / 60.f);
|
|
|
- if (verbosity_level >= 20) {
|
|
|
-
|
|
|
- current_position[Y_AXIS] = 0.f;
|
|
|
- go_to_current(homing_feedrate[X_AXIS] / 60.f);
|
|
|
- SERIAL_ECHOLNPGM("At Y0");
|
|
|
- delay_keep_alive(5000);
|
|
|
- current_position[Y_AXIS] = Y_MIN_POS;
|
|
|
- go_to_current(homing_feedrate[X_AXIS] / 60.f);
|
|
|
- SERIAL_ECHOLNPGM("At Y-4");
|
|
|
- delay_keep_alive(5000);
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
- current_position[X_AXIS] = pgm_read_float(bed_ref_points_4 + k * 2);
|
|
|
- current_position[Y_AXIS] = pgm_read_float(bed_ref_points_4 + k * 2 + 1);
|
|
|
-
|
|
|
- else {
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- current_position[X_AXIS] = vec_x[0] * pgm_read_float(bed_ref_points_4 + k * 2) + vec_y[0] * pgm_read_float(bed_ref_points_4 + k * 2 + 1) + cntr[0];
|
|
|
- current_position[Y_AXIS] = vec_x[1] * pgm_read_float(bed_ref_points_4 + k * 2) + vec_y[1] * pgm_read_float(bed_ref_points_4 + k * 2 + 1) + cntr[1];
|
|
|
-
|
|
|
-
|
|
|
- if (current_position[Y_AXIS] < Y_MIN_POS_FOR_BED_CALIBRATION)
|
|
|
- current_position[Y_AXIS] = Y_MIN_POS_FOR_BED_CALIBRATION;
|
|
|
-
|
|
|
- }*/
|
|
|
- if (verbosity_level >= 20) {
|
|
|
- SERIAL_ECHOPGM("current_position[X_AXIS]:");
|
|
|
- MYSERIAL.print(current_position[X_AXIS], 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- SERIAL_ECHOPGM("current_position[Y_AXIS]:");
|
|
|
- MYSERIAL.print(current_position[Y_AXIS], 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- SERIAL_ECHOPGM("current_position[Z_AXIS]:");
|
|
|
- MYSERIAL.print(current_position[Z_AXIS], 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
- go_to_current(homing_feedrate[X_AXIS] / 60.f);
|
|
|
- if (verbosity_level >= 10)
|
|
|
- delay_keep_alive(3000);
|
|
|
- if (!find_bed_induction_sensor_point_xy())
|
|
|
- return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
|
|
|
-#if 1
|
|
|
-
|
|
|
- if (k == 0) {
|
|
|
-
|
|
|
- find_bed_induction_sensor_point_z();
|
|
|
- int8_t i = 4;
|
|
|
- for (;;) {
|
|
|
- if (improve_bed_induction_sensor_point3(verbosity_level))
|
|
|
- break;
|
|
|
- if (--i == 0)
|
|
|
- return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
|
|
|
-
|
|
|
- current_position[Z_AXIS] -= 0.025f;
|
|
|
- enable_endstops(false);
|
|
|
- enable_z_endstop(false);
|
|
|
- go_to_current(homing_feedrate[Z_AXIS]);
|
|
|
- }
|
|
|
- if (i == 0)
|
|
|
-
|
|
|
- return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
|
|
|
- }
|
|
|
-#endif
|
|
|
- if (verbosity_level >= 10)
|
|
|
- delay_keep_alive(3000);
|
|
|
-
|
|
|
-
|
|
|
- if (verbosity_level >= 20) {
|
|
|
- SERIAL_ECHOLNPGM("Measured:");
|
|
|
- MYSERIAL.println(current_position[X_AXIS]);
|
|
|
- MYSERIAL.println(current_position[Y_AXIS]);
|
|
|
- }
|
|
|
- pt[0] = (pt[0] * iteration) / (iteration + 1);
|
|
|
- pt[0] += (current_position[X_AXIS]/(iteration + 1));
|
|
|
- pt[1] = (pt[1] * iteration) / (iteration + 1);
|
|
|
- pt[1] += (current_position[Y_AXIS] / (iteration + 1));
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- if (verbosity_level >= 20) {
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- SERIAL_ECHOPGM("pt[0]:");
|
|
|
- MYSERIAL.println(pt[0]);
|
|
|
- SERIAL_ECHOPGM("pt[1]:");
|
|
|
- MYSERIAL.println(pt[1]);
|
|
|
- }
|
|
|
-
|
|
|
- if (current_position[Y_AXIS] < Y_MIN_POS)
|
|
|
- current_position[Y_AXIS] = Y_MIN_POS;
|
|
|
-
|
|
|
- current_position[Z_AXIS] += 3.f + FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP * iteration * 0.3;
|
|
|
-
|
|
|
-
|
|
|
- if (verbosity_level >= 10 && k == 0) {
|
|
|
-
|
|
|
- current_position[Y_AXIS] = MANUAL_Y_HOME_POS;
|
|
|
- go_to_current(homing_feedrate[X_AXIS] / 60.f);
|
|
|
- delay_keep_alive(3000);
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- if (verbosity_level >= 20) {
|
|
|
-
|
|
|
- delay_keep_alive(3000);
|
|
|
- for (int8_t mesh_point = 0; mesh_point < 4; ++mesh_point) {
|
|
|
-
|
|
|
- refresh_cmd_timeout();
|
|
|
-
|
|
|
-
|
|
|
- current_position[X_AXIS] = pts[mesh_point * 2];
|
|
|
- current_position[Y_AXIS] = pts[mesh_point * 2 + 1];
|
|
|
- go_to_current(homing_feedrate[X_AXIS] / 60);
|
|
|
- delay_keep_alive(3000);
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- if (pts[1] < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH) {
|
|
|
- too_far_mask |= 1 << 1;
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- SERIAL_ECHOPGM("WARNING: Front point not reachable. Y coordinate:");
|
|
|
- MYSERIAL.print(pts[1]);
|
|
|
- SERIAL_ECHOPGM(" < ");
|
|
|
- MYSERIAL.println(Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH);
|
|
|
- }
|
|
|
-
|
|
|
- result = calculate_machine_skew_and_offset_LS(pts, 4, bed_ref_points_4, vec_x, vec_y, cntr, verbosity_level);
|
|
|
- if (result >= 0) {
|
|
|
- world2machine_update(vec_x, vec_y, cntr);
|
|
|
-#if 1
|
|
|
-
|
|
|
- eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER + 0), cntr[0]);
|
|
|
- eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER + 4), cntr[1]);
|
|
|
- eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X + 0), vec_x[0]);
|
|
|
- eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X + 4), vec_x[1]);
|
|
|
- eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y + 0), vec_y[0]);
|
|
|
- eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y + 4), vec_y[1]);
|
|
|
-#endif
|
|
|
- if (verbosity_level >= 10) {
|
|
|
-
|
|
|
- float l = sqrt(vec_x[0] * vec_x[0] + vec_x[1] * vec_x[1]);
|
|
|
- SERIAL_ECHOLNPGM("X vector length:");
|
|
|
- MYSERIAL.println(l);
|
|
|
-
|
|
|
-
|
|
|
- l = sqrt(vec_y[0] * vec_y[0] + vec_y[1] * vec_y[1]);
|
|
|
- SERIAL_ECHOLNPGM("Y vector length:");
|
|
|
- MYSERIAL.println(l);
|
|
|
-
|
|
|
- l = sqrt(cntr[0] * cntr[0] + cntr[1] * cntr[1]);
|
|
|
- SERIAL_ECHOLNPGM("Zero point correction:");
|
|
|
- MYSERIAL.println(l);
|
|
|
-
|
|
|
-
|
|
|
- l = vec_x[0] * vec_y[0] + vec_x[1] * vec_y[1];
|
|
|
- SERIAL_ECHOLNPGM("Perpendicularity");
|
|
|
- MYSERIAL.println(fabs(l));
|
|
|
- SERIAL_ECHOLNPGM("Saving bed calibration vectors to EEPROM");
|
|
|
- }
|
|
|
-
|
|
|
- world2machine_update_current();
|
|
|
-
|
|
|
-
|
|
|
- if (verbosity_level >= 20) {
|
|
|
-
|
|
|
- delay_keep_alive(3000);
|
|
|
- for (int8_t mesh_point = 0; mesh_point < 9; ++mesh_point) {
|
|
|
-
|
|
|
- refresh_cmd_timeout();
|
|
|
-
|
|
|
-
|
|
|
- current_position[X_AXIS] = pgm_read_float(bed_ref_points + mesh_point * 2);
|
|
|
- current_position[Y_AXIS] = pgm_read_float(bed_ref_points + mesh_point * 2 + 1);
|
|
|
- go_to_current(homing_feedrate[X_AXIS] / 60);
|
|
|
- delay_keep_alive(3000);
|
|
|
- }
|
|
|
- }
|
|
|
- return result;
|
|
|
- }
|
|
|
- if (result == BED_SKEW_OFFSET_DETECTION_FITTING_FAILED && too_far_mask == 2) return result;
|
|
|
- iteration++;
|
|
|
- }
|
|
|
- return result;
|
|
|
-}
|
|
|
-
|
|
|
-BedSkewOffsetDetectionResultType improve_bed_offset_and_skew(int8_t method, int8_t verbosity_level, uint8_t &too_far_mask)
|
|
|
-{
|
|
|
-
|
|
|
- refresh_cmd_timeout();
|
|
|
-
|
|
|
-
|
|
|
- too_far_mask = 0;
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- float *pts = &mbl.z_values[0][0];
|
|
|
- float *vec_x = pts + 2 * 9;
|
|
|
- float *vec_y = vec_x + 2;
|
|
|
- float *cntr = vec_y + 2;
|
|
|
- memset(pts, 0, sizeof(float) * 7 * 7);
|
|
|
-
|
|
|
-
|
|
|
- world2machine_initialize();
|
|
|
- vec_x[0] = world2machine_rotation_and_skew[0][0];
|
|
|
- vec_x[1] = world2machine_rotation_and_skew[1][0];
|
|
|
- vec_y[0] = world2machine_rotation_and_skew[0][1];
|
|
|
- vec_y[1] = world2machine_rotation_and_skew[1][1];
|
|
|
- cntr[0] = world2machine_shift[0];
|
|
|
- cntr[1] = world2machine_shift[1];
|
|
|
-
|
|
|
- world2machine_reset();
|
|
|
-
|
|
|
- bool endstops_enabled = enable_endstops(false);
|
|
|
- bool endstop_z_enabled = enable_z_endstop(false);
|
|
|
-
|
|
|
-#ifdef MESH_BED_CALIBRATION_SHOW_LCD
|
|
|
- uint8_t next_line;
|
|
|
- lcd_display_message_fullscreen_P(MSG_IMPROVE_BED_OFFSET_AND_SKEW_LINE1, next_line);
|
|
|
- if (next_line > 3)
|
|
|
- next_line = 3;
|
|
|
-#endif
|
|
|
-
|
|
|
-
|
|
|
- BedSkewOffsetDetectionResultType result = BED_SKEW_OFFSET_DETECTION_PERFECT;
|
|
|
- for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
|
|
|
-
|
|
|
- refresh_cmd_timeout();
|
|
|
-
|
|
|
-#ifdef MESH_BED_CALIBRATION_SHOW_LCD
|
|
|
- lcd_implementation_print_at(0, next_line, mesh_point+1);
|
|
|
- lcd_printPGM(MSG_IMPROVE_BED_OFFSET_AND_SKEW_LINE2);
|
|
|
-#endif
|
|
|
-
|
|
|
-
|
|
|
- current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
|
|
|
- enable_endstops(false);
|
|
|
- enable_z_endstop(false);
|
|
|
- go_to_current(homing_feedrate[Z_AXIS]/60);
|
|
|
- if (verbosity_level >= 20) {
|
|
|
-
|
|
|
- current_position[Y_AXIS] = 0.f;
|
|
|
- go_to_current(homing_feedrate[X_AXIS] / 60.f);
|
|
|
- SERIAL_ECHOLNPGM("At Y0");
|
|
|
- delay_keep_alive(5000);
|
|
|
- current_position[Y_AXIS] = Y_MIN_POS;
|
|
|
- go_to_current(homing_feedrate[X_AXIS] / 60.f);
|
|
|
- SERIAL_ECHOLNPGM("At Y-4");
|
|
|
- delay_keep_alive(5000);
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
- current_position[X_AXIS] = vec_x[0] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[0] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[0];
|
|
|
- current_position[Y_AXIS] = vec_x[1] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[1] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[1];
|
|
|
-
|
|
|
- if (current_position[Y_AXIS] < Y_MIN_POS_FOR_BED_CALIBRATION)
|
|
|
- current_position[Y_AXIS] = Y_MIN_POS_FOR_BED_CALIBRATION;
|
|
|
- go_to_current(homing_feedrate[X_AXIS]/60);
|
|
|
-
|
|
|
- if (verbosity_level >= 10)
|
|
|
- delay_keep_alive(3000);
|
|
|
- find_bed_induction_sensor_point_z();
|
|
|
- if (verbosity_level >= 10)
|
|
|
- delay_keep_alive(3000);
|
|
|
-
|
|
|
- current_position[Z_AXIS] -= 0.025f;
|
|
|
-
|
|
|
- int8_t n_errors = 3;
|
|
|
- for (int8_t iter = 0; iter < 8; ) {
|
|
|
- if (verbosity_level > 20) {
|
|
|
- SERIAL_ECHOPGM("Improving bed point ");
|
|
|
- SERIAL_ECHO(mesh_point);
|
|
|
- SERIAL_ECHOPGM(", iteration ");
|
|
|
- SERIAL_ECHO(iter);
|
|
|
- SERIAL_ECHOPGM(", z");
|
|
|
- MYSERIAL.print(current_position[Z_AXIS], 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- }
|
|
|
- bool found = false;
|
|
|
- if (mesh_point < 3) {
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- found = improve_bed_induction_sensor_point3(verbosity_level);
|
|
|
- } else {
|
|
|
- switch (method) {
|
|
|
- case 0: found = improve_bed_induction_sensor_point(); break;
|
|
|
- case 1: found = improve_bed_induction_sensor_point2(mesh_point < 3, verbosity_level); break;
|
|
|
- default: break;
|
|
|
- }
|
|
|
- }
|
|
|
- if (found) {
|
|
|
- if (iter > 3) {
|
|
|
-
|
|
|
- pts[mesh_point*2 ] += current_position[X_AXIS];
|
|
|
- pts[mesh_point*2+1] += current_position[Y_AXIS];
|
|
|
- }
|
|
|
- if (current_position[Y_AXIS] < Y_MIN_POS)
|
|
|
- current_position[Y_AXIS] = Y_MIN_POS;
|
|
|
- ++ iter;
|
|
|
- } else if (n_errors -- == 0) {
|
|
|
-
|
|
|
- result = BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
|
|
|
- goto canceled;
|
|
|
- } else {
|
|
|
-
|
|
|
- current_position[Z_AXIS] -= 0.05f;
|
|
|
- enable_endstops(false);
|
|
|
- enable_z_endstop(false);
|
|
|
- go_to_current(homing_feedrate[Z_AXIS]);
|
|
|
- if (verbosity_level >= 5) {
|
|
|
- SERIAL_ECHOPGM("Improving bed point ");
|
|
|
- SERIAL_ECHO(mesh_point);
|
|
|
- SERIAL_ECHOPGM(", iteration ");
|
|
|
- SERIAL_ECHO(iter);
|
|
|
- SERIAL_ECHOPGM(" failed. Lowering z to ");
|
|
|
- MYSERIAL.print(current_position[Z_AXIS], 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
- if (verbosity_level >= 10)
|
|
|
- delay_keep_alive(3000);
|
|
|
- }
|
|
|
-
|
|
|
- refresh_cmd_timeout();
|
|
|
-
|
|
|
-
|
|
|
- for (int8_t i = 0; i < 18; ++ i)
|
|
|
- pts[i] *= (1.f/4.f);
|
|
|
-
|
|
|
- enable_endstops(false);
|
|
|
- enable_z_endstop(false);
|
|
|
-
|
|
|
- if (verbosity_level >= 5) {
|
|
|
-
|
|
|
- current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
|
|
|
- for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
|
|
|
-
|
|
|
- refresh_cmd_timeout();
|
|
|
-
|
|
|
-
|
|
|
- current_position[X_AXIS] = pts[mesh_point*2];
|
|
|
- current_position[Y_AXIS] = pts[mesh_point*2+1];
|
|
|
- if (verbosity_level >= 10) {
|
|
|
- go_to_current(homing_feedrate[X_AXIS]/60);
|
|
|
- delay_keep_alive(3000);
|
|
|
- }
|
|
|
- SERIAL_ECHOPGM("Final measured bed point ");
|
|
|
- SERIAL_ECHO(mesh_point);
|
|
|
- SERIAL_ECHOPGM(": ");
|
|
|
- MYSERIAL.print(current_position[X_AXIS], 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(current_position[Y_AXIS], 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- {
|
|
|
-
|
|
|
- for (uint8_t mesh_point = 0; mesh_point < 3; ++ mesh_point)
|
|
|
- if (pts[mesh_point * 2 + 1] < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH)
|
|
|
- too_far_mask |= 1 << mesh_point;
|
|
|
- result = calculate_machine_skew_and_offset_LS(pts, 9, bed_ref_points, vec_x, vec_y, cntr, verbosity_level);
|
|
|
- if (result < 0) {
|
|
|
- SERIAL_ECHOLNPGM("Calculation of the machine skew and offset failed.");
|
|
|
- goto canceled;
|
|
|
- }
|
|
|
-
|
|
|
- for (uint8_t mesh_point = 0; mesh_point < 3; ++ mesh_point) {
|
|
|
- float y = vec_x[1] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[1] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[1];
|
|
|
- if (y < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH)
|
|
|
- too_far_mask |= 1 << mesh_point;
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- world2machine_update(vec_x, vec_y, cntr);
|
|
|
-#if 1
|
|
|
-
|
|
|
- eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER+0), cntr [0]);
|
|
|
- eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER+4), cntr [1]);
|
|
|
- eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +0), vec_x[0]);
|
|
|
- eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +4), vec_x[1]);
|
|
|
- eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +0), vec_y[0]);
|
|
|
- eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +4), vec_y[1]);
|
|
|
-#endif
|
|
|
-
|
|
|
-
|
|
|
- world2machine_update_current();
|
|
|
-
|
|
|
- enable_endstops(false);
|
|
|
- enable_z_endstop(false);
|
|
|
-
|
|
|
- if (verbosity_level >= 5) {
|
|
|
-
|
|
|
- delay_keep_alive(3000);
|
|
|
- current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
|
|
|
- for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
|
|
|
-
|
|
|
- refresh_cmd_timeout();
|
|
|
-
|
|
|
-
|
|
|
- current_position[X_AXIS] = pgm_read_float(bed_ref_points+mesh_point*2);
|
|
|
- current_position[Y_AXIS] = pgm_read_float(bed_ref_points+mesh_point*2+1);
|
|
|
- if (verbosity_level >= 10) {
|
|
|
- go_to_current(homing_feedrate[X_AXIS]/60);
|
|
|
- delay_keep_alive(3000);
|
|
|
- }
|
|
|
- {
|
|
|
- float x, y;
|
|
|
- world2machine(current_position[X_AXIS], current_position[Y_AXIS], x, y);
|
|
|
- SERIAL_ECHOPGM("Final calculated bed point ");
|
|
|
- SERIAL_ECHO(mesh_point);
|
|
|
- SERIAL_ECHOPGM(": ");
|
|
|
- MYSERIAL.print(x, 5);
|
|
|
- SERIAL_ECHOPGM(", ");
|
|
|
- MYSERIAL.print(y, 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- if (! sample_mesh_and_store_reference())
|
|
|
- goto canceled;
|
|
|
-
|
|
|
- enable_endstops(endstops_enabled);
|
|
|
- enable_z_endstop(endstop_z_enabled);
|
|
|
-
|
|
|
- refresh_cmd_timeout();
|
|
|
- return result;
|
|
|
-
|
|
|
-canceled:
|
|
|
-
|
|
|
- refresh_cmd_timeout();
|
|
|
-
|
|
|
- current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
|
|
|
- go_to_current(homing_feedrate[Z_AXIS]/60);
|
|
|
-
|
|
|
- reset_bed_offset_and_skew();
|
|
|
- enable_endstops(endstops_enabled);
|
|
|
- enable_z_endstop(endstop_z_enabled);
|
|
|
- return result;
|
|
|
-}
|
|
|
-
|
|
|
-void go_home_with_z_lift()
|
|
|
-{
|
|
|
-
|
|
|
- refresh_cmd_timeout();
|
|
|
-
|
|
|
-
|
|
|
- current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
|
|
|
- go_to_current(homing_feedrate[Z_AXIS]/60);
|
|
|
-
|
|
|
- current_position[X_AXIS] = X_MIN_POS+0.2;
|
|
|
- current_position[Y_AXIS] = Y_MIN_POS+0.2;
|
|
|
-
|
|
|
- world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
|
|
|
- go_to_current(homing_feedrate[X_AXIS]/60);
|
|
|
-
|
|
|
- current_position[Z_AXIS] = Z_MIN_POS;
|
|
|
- go_to_current(homing_feedrate[Z_AXIS]/60);
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-bool sample_mesh_and_store_reference()
|
|
|
-{
|
|
|
- bool endstops_enabled = enable_endstops(false);
|
|
|
- bool endstop_z_enabled = enable_z_endstop(false);
|
|
|
-
|
|
|
-
|
|
|
- refresh_cmd_timeout();
|
|
|
-
|
|
|
-#ifdef MESH_BED_CALIBRATION_SHOW_LCD
|
|
|
- uint8_t next_line;
|
|
|
- lcd_display_message_fullscreen_P(MSG_MEASURE_BED_REFERENCE_HEIGHT_LINE1, next_line);
|
|
|
- if (next_line > 3)
|
|
|
- next_line = 3;
|
|
|
-
|
|
|
- lcd_implementation_print_at(0, next_line, 1);
|
|
|
- lcd_printPGM(MSG_MEASURE_BED_REFERENCE_HEIGHT_LINE2);
|
|
|
-#endif
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- {
|
|
|
-
|
|
|
- current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
|
|
|
- go_to_current(homing_feedrate[Z_AXIS]/60);
|
|
|
- current_position[X_AXIS] = pgm_read_float(bed_ref_points);
|
|
|
- current_position[Y_AXIS] = pgm_read_float(bed_ref_points+1);
|
|
|
- world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
|
|
|
- go_to_current(homing_feedrate[X_AXIS]/60);
|
|
|
- memcpy(destination, current_position, sizeof(destination));
|
|
|
- enable_endstops(true);
|
|
|
- homeaxis(Z_AXIS);
|
|
|
- enable_endstops(false);
|
|
|
- find_bed_induction_sensor_point_z();
|
|
|
- mbl.set_z(0, 0, current_position[Z_AXIS]);
|
|
|
- }
|
|
|
- for (int8_t mesh_point = 1; mesh_point != MESH_MEAS_NUM_X_POINTS * MESH_MEAS_NUM_Y_POINTS; ++ mesh_point) {
|
|
|
-
|
|
|
- refresh_cmd_timeout();
|
|
|
-
|
|
|
- current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
|
|
|
- go_to_current(homing_feedrate[Z_AXIS]/60);
|
|
|
- current_position[X_AXIS] = pgm_read_float(bed_ref_points+2*mesh_point);
|
|
|
- current_position[Y_AXIS] = pgm_read_float(bed_ref_points+2*mesh_point+1);
|
|
|
- world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
|
|
|
- go_to_current(homing_feedrate[X_AXIS]/60);
|
|
|
-#ifdef MESH_BED_CALIBRATION_SHOW_LCD
|
|
|
-
|
|
|
- lcd_implementation_print_at(0, next_line, mesh_point+1);
|
|
|
- lcd_printPGM(MSG_MEASURE_BED_REFERENCE_HEIGHT_LINE2);
|
|
|
-#endif
|
|
|
- find_bed_induction_sensor_point_z();
|
|
|
-
|
|
|
- int8_t ix = mesh_point % MESH_MEAS_NUM_X_POINTS;
|
|
|
- int8_t iy = mesh_point / MESH_MEAS_NUM_X_POINTS;
|
|
|
- if (iy & 1) ix = (MESH_MEAS_NUM_X_POINTS - 1) - ix;
|
|
|
- mbl.set_z(ix, iy, current_position[Z_AXIS]);
|
|
|
- }
|
|
|
- {
|
|
|
-
|
|
|
- float zmin = mbl.z_values[0][0];
|
|
|
- float zmax = zmax;
|
|
|
- for (int8_t j = 0; j < 3; ++ j)
|
|
|
- for (int8_t i = 0; i < 3; ++ i) {
|
|
|
- zmin = min(zmin, mbl.z_values[j][i]);
|
|
|
- zmax = min(zmax, mbl.z_values[j][i]);
|
|
|
- }
|
|
|
- if (zmax - zmin > 3.f) {
|
|
|
-
|
|
|
-
|
|
|
- SERIAL_PROTOCOLLNPGM("Exreme span of the Z values!");
|
|
|
- return false;
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- {
|
|
|
- uint16_t addr = EEPROM_BED_CALIBRATION_Z_JITTER;
|
|
|
- for (int8_t j = 0; j < 3; ++ j)
|
|
|
- for (int8_t i = 0; i < 3; ++ i) {
|
|
|
- if (i == 0 && j == 0)
|
|
|
- continue;
|
|
|
- float dif = mbl.z_values[j][i] - mbl.z_values[0][0];
|
|
|
- int16_t dif_quantized = int16_t(floor(dif * 100.f + 0.5f));
|
|
|
- eeprom_update_word((uint16_t*)addr, *reinterpret_cast<uint16_t*>(&dif_quantized));
|
|
|
- #if 0
|
|
|
- {
|
|
|
- uint16_t z_offset_u = eeprom_read_word((uint16_t*)addr);
|
|
|
- float dif2 = *reinterpret_cast<int16_t*>(&z_offset_u) * 0.01;
|
|
|
-
|
|
|
- SERIAL_ECHOPGM("Bed point ");
|
|
|
- SERIAL_ECHO(i);
|
|
|
- SERIAL_ECHOPGM(",");
|
|
|
- SERIAL_ECHO(j);
|
|
|
- SERIAL_ECHOPGM(", differences: written ");
|
|
|
- MYSERIAL.print(dif, 5);
|
|
|
- SERIAL_ECHOPGM(", read: ");
|
|
|
- MYSERIAL.print(dif2, 5);
|
|
|
- SERIAL_ECHOLNPGM("");
|
|
|
- }
|
|
|
- #endif
|
|
|
- addr += 2;
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- mbl.upsample_3x3();
|
|
|
- mbl.active = true;
|
|
|
-
|
|
|
- go_home_with_z_lift();
|
|
|
-
|
|
|
- enable_endstops(endstops_enabled);
|
|
|
- enable_z_endstop(endstop_z_enabled);
|
|
|
- return true;
|
|
|
-}
|
|
|
-
|
|
|
-bool scan_bed_induction_points(int8_t verbosity_level)
|
|
|
-{
|
|
|
-
|
|
|
- refresh_cmd_timeout();
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- float *pts = &mbl.z_values[0][0];
|
|
|
- float *vec_x = pts + 2 * 9;
|
|
|
- float *vec_y = vec_x + 2;
|
|
|
- float *cntr = vec_y + 2;
|
|
|
- memset(pts, 0, sizeof(float) * 7 * 7);
|
|
|
-
|
|
|
-
|
|
|
- world2machine_initialize();
|
|
|
- vec_x[0] = world2machine_rotation_and_skew[0][0];
|
|
|
- vec_x[1] = world2machine_rotation_and_skew[1][0];
|
|
|
- vec_y[0] = world2machine_rotation_and_skew[0][1];
|
|
|
- vec_y[1] = world2machine_rotation_and_skew[1][1];
|
|
|
- cntr[0] = world2machine_shift[0];
|
|
|
- cntr[1] = world2machine_shift[1];
|
|
|
-
|
|
|
- world2machine_reset();
|
|
|
-
|
|
|
- bool endstops_enabled = enable_endstops(false);
|
|
|
- bool endstop_z_enabled = enable_z_endstop(false);
|
|
|
-
|
|
|
-
|
|
|
- for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
|
|
|
-
|
|
|
- refresh_cmd_timeout();
|
|
|
-
|
|
|
-
|
|
|
- current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
|
|
|
- enable_endstops(false);
|
|
|
- enable_z_endstop(false);
|
|
|
- go_to_current(homing_feedrate[Z_AXIS]/60);
|
|
|
-
|
|
|
-
|
|
|
- current_position[X_AXIS] = vec_x[0] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[0] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[0];
|
|
|
- current_position[Y_AXIS] = vec_x[1] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[1] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[1];
|
|
|
-
|
|
|
- if (current_position[Y_AXIS] < Y_MIN_POS_FOR_BED_CALIBRATION)
|
|
|
- current_position[Y_AXIS] = Y_MIN_POS_FOR_BED_CALIBRATION;
|
|
|
- go_to_current(homing_feedrate[X_AXIS]/60);
|
|
|
- find_bed_induction_sensor_point_z();
|
|
|
- scan_bed_induction_sensor_point();
|
|
|
- }
|
|
|
-
|
|
|
- refresh_cmd_timeout();
|
|
|
-
|
|
|
- enable_endstops(false);
|
|
|
- enable_z_endstop(false);
|
|
|
-
|
|
|
-
|
|
|
- refresh_cmd_timeout();
|
|
|
-
|
|
|
- enable_endstops(endstops_enabled);
|
|
|
- enable_z_endstop(endstop_z_enabled);
|
|
|
- return true;
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-static void shift_z(float delta)
|
|
|
-{
|
|
|
- plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] - delta, current_position[E_AXIS], homing_feedrate[Z_AXIS]/40, active_extruder);
|
|
|
- st_synchronize();
|
|
|
- plan_set_z_position(current_position[Z_AXIS]);
|
|
|
-}
|
|
|
-
|
|
|
-#define BABYSTEP_LOADZ_BY_PLANNER
|
|
|
-
|
|
|
-
|
|
|
-static int babystepLoadZ = 0;
|
|
|
-
|
|
|
-void babystep_apply()
|
|
|
-{
|
|
|
-
|
|
|
- if(calibration_status() < CALIBRATION_STATUS_LIVE_ADJUST)
|
|
|
- {
|
|
|
- check_babystep();
|
|
|
-
|
|
|
-
|
|
|
- EEPROM_read_B(EEPROM_BABYSTEP_Z,&babystepLoadZ);
|
|
|
-
|
|
|
- #if 0
|
|
|
- SERIAL_ECHO("Z baby step: ");
|
|
|
- SERIAL_ECHO(babystepLoadZ);
|
|
|
- SERIAL_ECHO(", current Z: ");
|
|
|
- SERIAL_ECHO(current_position[Z_AXIS]);
|
|
|
- SERIAL_ECHO("correction: ");
|
|
|
- SERIAL_ECHO(float(babystepLoadZ) / float(axis_steps_per_unit[Z_AXIS]));
|
|
|
- SERIAL_ECHOLN("");
|
|
|
- #endif
|
|
|
- #ifdef BABYSTEP_LOADZ_BY_PLANNER
|
|
|
- shift_z(- float(babystepLoadZ) / float(axis_steps_per_unit[Z_AXIS]));
|
|
|
- #else
|
|
|
- babystepsTodoZadd(babystepLoadZ);
|
|
|
- #endif
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void babystep_undo()
|
|
|
-{
|
|
|
-#ifdef BABYSTEP_LOADZ_BY_PLANNER
|
|
|
- shift_z(float(babystepLoadZ) / float(axis_steps_per_unit[Z_AXIS]));
|
|
|
-#else
|
|
|
- babystepsTodoZsubtract(babystepLoadZ);
|
|
|
-#endif
|
|
|
- babystepLoadZ = 0;
|
|
|
-}
|
|
|
-
|
|
|
-void babystep_reset()
|
|
|
-{
|
|
|
- babystepLoadZ = 0;
|
|
|
+#include "Marlin.h"
|
|
|
+#include "Configuration.h"
|
|
|
+#include "ConfigurationStore.h"
|
|
|
+#include "language_all.h"
|
|
|
+#include "mesh_bed_calibration.h"
|
|
|
+#include "mesh_bed_leveling.h"
|
|
|
+#include "stepper.h"
|
|
|
+#include "ultralcd.h"
|
|
|
+
|
|
|
+uint8_t world2machine_correction_mode;
|
|
|
+float world2machine_rotation_and_skew[2][2];
|
|
|
+float world2machine_rotation_and_skew_inv[2][2];
|
|
|
+float world2machine_shift[2];
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+#define WEIGHT_FIRST_ROW_X_HIGH (1.f)
|
|
|
+#define WEIGHT_FIRST_ROW_X_LOW (0.35f)
|
|
|
+#define WEIGHT_FIRST_ROW_Y_HIGH (0.3f)
|
|
|
+#define WEIGHT_FIRST_ROW_Y_LOW (0.0f)
|
|
|
+
|
|
|
+#define BED_ZERO_REF_X (- 22.f + X_PROBE_OFFSET_FROM_EXTRUDER)
|
|
|
+#define BED_ZERO_REF_Y (- 0.6f + Y_PROBE_OFFSET_FROM_EXTRUDER)
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+#define MACHINE_AXIS_SCALE_X 1.f
|
|
|
+#define MACHINE_AXIS_SCALE_Y 1.f
|
|
|
+
|
|
|
+
|
|
|
+#define BED_SKEW_ANGLE_MILD (0.12f * M_PI / 180.f)
|
|
|
+
|
|
|
+#define BED_SKEW_ANGLE_EXTREME (0.25f * M_PI / 180.f)
|
|
|
+
|
|
|
+#define BED_CALIBRATION_POINT_OFFSET_MAX_EUCLIDIAN (0.8f)
|
|
|
+#define BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_X (0.8f)
|
|
|
+#define BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_Y (1.5f)
|
|
|
+
|
|
|
+#define MIN_BED_SENSOR_POINT_RESPONSE_DMR (2.0f)
|
|
|
+
|
|
|
+
|
|
|
+#define Y_MIN_POS_FOR_BED_CALIBRATION (Y_MIN_POS)
|
|
|
+
|
|
|
+#define Y_MIN_POS_CALIBRATION_POINT_ACCURATE (Y_MIN_POS + 3.f)
|
|
|
+
|
|
|
+
|
|
|
+#define Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH (Y_MIN_POS - 0.5f)
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+const float bed_ref_points[] PROGMEM = {
|
|
|
+ 13.f - BED_ZERO_REF_X, 6.4f - BED_ZERO_REF_Y,
|
|
|
+ 115.f - BED_ZERO_REF_X, 6.4f - BED_ZERO_REF_Y,
|
|
|
+ 216.f - BED_ZERO_REF_X, 6.4f - BED_ZERO_REF_Y,
|
|
|
+
|
|
|
+ 216.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
|
|
|
+ 115.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
|
|
|
+ 13.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
|
|
|
+
|
|
|
+ 13.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y,
|
|
|
+ 115.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y,
|
|
|
+ 216.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y
|
|
|
+};
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+const float bed_ref_points_4[] PROGMEM = {
|
|
|
+ 115.f - BED_ZERO_REF_X, 6.4f - BED_ZERO_REF_Y,
|
|
|
+ 216.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
|
|
|
+ 115.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y,
|
|
|
+ 13.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y
|
|
|
+};
|
|
|
+
|
|
|
+static inline float sqr(float x) { return x * x; }
|
|
|
+
|
|
|
+static inline bool point_on_1st_row(const uint8_t i, const uint8_t npts)
|
|
|
+{
|
|
|
+ if (npts == 4) return (i == 0);
|
|
|
+ else return (i < 3);
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+static inline float point_weight_x(const uint8_t i, const uint8_t npts, const float &y)
|
|
|
+{
|
|
|
+ float w = 1.f;
|
|
|
+ if (point_on_1st_row(i, npts)) {
|
|
|
+ if (y >= Y_MIN_POS_CALIBRATION_POINT_ACCURATE) {
|
|
|
+ w = WEIGHT_FIRST_ROW_X_HIGH;
|
|
|
+ } else if (y < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH) {
|
|
|
+
|
|
|
+ w = WEIGHT_FIRST_ROW_X_LOW;
|
|
|
+ } else {
|
|
|
+
|
|
|
+ float t = (y - Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH) / (Y_MIN_POS_CALIBRATION_POINT_ACCURATE - Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH);
|
|
|
+ w = (1.f - t) * WEIGHT_FIRST_ROW_X_LOW + t * WEIGHT_FIRST_ROW_X_HIGH;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return w;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+static inline float point_weight_y(const uint8_t i, const uint8_t npts, const float &y)
|
|
|
+{
|
|
|
+ float w = 1.f;
|
|
|
+ if (point_on_1st_row(i, npts)) {
|
|
|
+ if (y >= Y_MIN_POS_CALIBRATION_POINT_ACCURATE) {
|
|
|
+ w = WEIGHT_FIRST_ROW_Y_HIGH;
|
|
|
+ } else if (y < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH) {
|
|
|
+
|
|
|
+ w = WEIGHT_FIRST_ROW_Y_LOW;
|
|
|
+ } else {
|
|
|
+
|
|
|
+ float t = (y - Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH) / (Y_MIN_POS_CALIBRATION_POINT_ACCURATE - Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH);
|
|
|
+ w = (1.f - t) * WEIGHT_FIRST_ROW_Y_LOW + t * WEIGHT_FIRST_ROW_Y_HIGH;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return w;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+BedSkewOffsetDetectionResultType calculate_machine_skew_and_offset_LS(
|
|
|
+
|
|
|
+ const float *measured_pts,
|
|
|
+ uint8_t npts,
|
|
|
+ const float *true_pts,
|
|
|
+
|
|
|
+ float *vec_x,
|
|
|
+ float *vec_y,
|
|
|
+ float *cntr,
|
|
|
+
|
|
|
+
|
|
|
+ int8_t verbosity_level
|
|
|
+ )
|
|
|
+{
|
|
|
+ if (verbosity_level >= 10) {
|
|
|
+ SERIAL_ECHOLNPGM("calculate machine skew and offset LS");
|
|
|
+
|
|
|
+
|
|
|
+ SERIAL_ECHOPGM("X vector, initial: ");
|
|
|
+ MYSERIAL.print(vec_x[0], 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(vec_x[1], 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+
|
|
|
+ SERIAL_ECHOPGM("Y vector, initial: ");
|
|
|
+ MYSERIAL.print(vec_y[0], 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(vec_y[1], 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+
|
|
|
+ SERIAL_ECHOPGM("center, initial: ");
|
|
|
+ MYSERIAL.print(cntr[0], 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(cntr[1], 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+
|
|
|
+ for (uint8_t i = 0; i < npts; ++i) {
|
|
|
+ SERIAL_ECHOPGM("point #");
|
|
|
+ MYSERIAL.print(int(i));
|
|
|
+ SERIAL_ECHOPGM(" measured: (");
|
|
|
+ MYSERIAL.print(measured_pts[i * 2], 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(measured_pts[i * 2 + 1], 5);
|
|
|
+ SERIAL_ECHOPGM("); target: (");
|
|
|
+ MYSERIAL.print(pgm_read_float(true_pts + i * 2), 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(pgm_read_float(true_pts + i * 2 + 1), 5);
|
|
|
+ SERIAL_ECHOPGM("), error: ");
|
|
|
+ MYSERIAL.print(sqrt(
|
|
|
+ sqr(pgm_read_float(true_pts + i * 2) - measured_pts[i * 2]) +
|
|
|
+ sqr(pgm_read_float(true_pts + i * 2 + 1) - measured_pts[i * 2 + 1])), 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ }
|
|
|
+ delay_keep_alive(100);
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ cntr[0] = 0.f;
|
|
|
+ cntr[1] = 0.f;
|
|
|
+
|
|
|
+ float a1 = 0;
|
|
|
+
|
|
|
+ float a2 = 0;
|
|
|
+ for (int8_t iter = 0; iter < 100; ++iter) {
|
|
|
+ float c1 = cos(a1) * MACHINE_AXIS_SCALE_X;
|
|
|
+ float s1 = sin(a1) * MACHINE_AXIS_SCALE_X;
|
|
|
+ float c2 = cos(a2) * MACHINE_AXIS_SCALE_Y;
|
|
|
+ float s2 = sin(a2) * MACHINE_AXIS_SCALE_Y;
|
|
|
+
|
|
|
+ float A[4][4] = { 0.f };
|
|
|
+ float b[4] = { 0.f };
|
|
|
+ float acc;
|
|
|
+ for (uint8_t r = 0; r < 4; ++r) {
|
|
|
+ for (uint8_t c = 0; c < 4; ++c) {
|
|
|
+ acc = 0;
|
|
|
+
|
|
|
+ for (uint8_t i = 0; i < npts; ++i) {
|
|
|
+
|
|
|
+ if (r != 1 && c != 1) {
|
|
|
+ float a =
|
|
|
+ (r == 0) ? 1.f :
|
|
|
+ ((r == 2) ? (-s1 * measured_pts[2 * i]) :
|
|
|
+ (-c2 * measured_pts[2 * i + 1]));
|
|
|
+ float b =
|
|
|
+ (c == 0) ? 1.f :
|
|
|
+ ((c == 2) ? (-s1 * measured_pts[2 * i]) :
|
|
|
+ (-c2 * measured_pts[2 * i + 1]));
|
|
|
+ float w = point_weight_x(i, npts, measured_pts[2 * i + 1]);
|
|
|
+ acc += a * b * w;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if (r != 0 && c != 0) {
|
|
|
+ float a =
|
|
|
+ (r == 1) ? 1.f :
|
|
|
+ ((r == 2) ? ( c1 * measured_pts[2 * i]) :
|
|
|
+ (-s2 * measured_pts[2 * i + 1]));
|
|
|
+ float b =
|
|
|
+ (c == 1) ? 1.f :
|
|
|
+ ((c == 2) ? ( c1 * measured_pts[2 * i]) :
|
|
|
+ (-s2 * measured_pts[2 * i + 1]));
|
|
|
+ float w = point_weight_y(i, npts, measured_pts[2 * i + 1]);
|
|
|
+ acc += a * b * w;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ A[r][c] = acc;
|
|
|
+ }
|
|
|
+
|
|
|
+ acc = 0.f;
|
|
|
+ for (uint8_t i = 0; i < npts; ++i) {
|
|
|
+ {
|
|
|
+ float j =
|
|
|
+ (r == 0) ? 1.f :
|
|
|
+ ((r == 1) ? 0.f :
|
|
|
+ ((r == 2) ? (-s1 * measured_pts[2 * i]) :
|
|
|
+ (-c2 * measured_pts[2 * i + 1])));
|
|
|
+ float fx = c1 * measured_pts[2 * i] - s2 * measured_pts[2 * i + 1] + cntr[0] - pgm_read_float(true_pts + i * 2);
|
|
|
+ float w = point_weight_x(i, npts, measured_pts[2 * i + 1]);
|
|
|
+ acc += j * fx * w;
|
|
|
+ }
|
|
|
+ {
|
|
|
+ float j =
|
|
|
+ (r == 0) ? 0.f :
|
|
|
+ ((r == 1) ? 1.f :
|
|
|
+ ((r == 2) ? ( c1 * measured_pts[2 * i]) :
|
|
|
+ (-s2 * measured_pts[2 * i + 1])));
|
|
|
+ float fy = s1 * measured_pts[2 * i] + c2 * measured_pts[2 * i + 1] + cntr[1] - pgm_read_float(true_pts + i * 2 + 1);
|
|
|
+ float w = point_weight_y(i, npts, measured_pts[2 * i + 1]);
|
|
|
+ acc += j * fy * w;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ b[r] = -acc;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ float h[4] = { 0.f };
|
|
|
+ for (uint8_t gauss_iter = 0; gauss_iter < 100; ++gauss_iter) {
|
|
|
+ h[0] = (b[0] - A[0][1] * h[1] - A[0][2] * h[2] - A[0][3] * h[3]) / A[0][0];
|
|
|
+ h[1] = (b[1] - A[1][0] * h[0] - A[1][2] * h[2] - A[1][3] * h[3]) / A[1][1];
|
|
|
+ h[2] = (b[2] - A[2][0] * h[0] - A[2][1] * h[1] - A[2][3] * h[3]) / A[2][2];
|
|
|
+ h[3] = (b[3] - A[3][0] * h[0] - A[3][1] * h[1] - A[3][2] * h[2]) / A[3][3];
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ cntr[0] += h[0];
|
|
|
+ cntr[1] += h[1];
|
|
|
+ a1 += h[2];
|
|
|
+ a2 += h[3];
|
|
|
+
|
|
|
+ if (verbosity_level >= 20) {
|
|
|
+ SERIAL_ECHOPGM("iteration: ");
|
|
|
+ MYSERIAL.print(int(iter));
|
|
|
+ SERIAL_ECHOPGM("; correction vector: ");
|
|
|
+ MYSERIAL.print(h[0], 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(h[1], 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(h[2], 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(h[3], 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ SERIAL_ECHOPGM("corrected x/y: ");
|
|
|
+ MYSERIAL.print(cntr[0], 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(cntr[0], 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ SERIAL_ECHOPGM("corrected angles: ");
|
|
|
+ MYSERIAL.print(180.f * a1 / M_PI, 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(180.f * a2 / M_PI, 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ vec_x[0] = cos(a1) * MACHINE_AXIS_SCALE_X;
|
|
|
+ vec_x[1] = sin(a1) * MACHINE_AXIS_SCALE_X;
|
|
|
+ vec_y[0] = -sin(a2) * MACHINE_AXIS_SCALE_Y;
|
|
|
+ vec_y[1] = cos(a2) * MACHINE_AXIS_SCALE_Y;
|
|
|
+
|
|
|
+ BedSkewOffsetDetectionResultType result = BED_SKEW_OFFSET_DETECTION_PERFECT;
|
|
|
+ {
|
|
|
+ float angleDiff = fabs(a2 - a1);
|
|
|
+ if (angleDiff > BED_SKEW_ANGLE_MILD)
|
|
|
+ result = (angleDiff > BED_SKEW_ANGLE_EXTREME) ?
|
|
|
+ BED_SKEW_OFFSET_DETECTION_SKEW_EXTREME :
|
|
|
+ BED_SKEW_OFFSET_DETECTION_SKEW_MILD;
|
|
|
+ if (fabs(a1) > BED_SKEW_ANGLE_EXTREME ||
|
|
|
+ fabs(a2) > BED_SKEW_ANGLE_EXTREME)
|
|
|
+ result = BED_SKEW_OFFSET_DETECTION_SKEW_EXTREME;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (verbosity_level >= 1) {
|
|
|
+ SERIAL_ECHOPGM("correction angles: ");
|
|
|
+ MYSERIAL.print(180.f * a1 / M_PI, 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(180.f * a2 / M_PI, 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ }
|
|
|
+
|
|
|
+ if (verbosity_level >= 10) {
|
|
|
+
|
|
|
+ SERIAL_ECHOPGM("X vector new, inverted: ");
|
|
|
+ MYSERIAL.print(vec_x[0], 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(vec_x[1], 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+
|
|
|
+ SERIAL_ECHOPGM("Y vector new, inverted: ");
|
|
|
+ MYSERIAL.print(vec_y[0], 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(vec_y[1], 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+
|
|
|
+ SERIAL_ECHOPGM("center new, inverted: ");
|
|
|
+ MYSERIAL.print(cntr[0], 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(cntr[1], 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ delay_keep_alive(100);
|
|
|
+
|
|
|
+ SERIAL_ECHOLNPGM("Error after correction: ");
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ for (uint8_t i = 0; i < npts; ++i) {
|
|
|
+ float x = vec_x[0] * measured_pts[i * 2] + vec_y[0] * measured_pts[i * 2 + 1] + cntr[0];
|
|
|
+ float y = vec_x[1] * measured_pts[i * 2] + vec_y[1] * measured_pts[i * 2 + 1] + cntr[1];
|
|
|
+ float errX = sqr(pgm_read_float(true_pts + i * 2) - x);
|
|
|
+ float errY = sqr(pgm_read_float(true_pts + i * 2 + 1) - y);
|
|
|
+ float err = sqrt(errX + errY);
|
|
|
+ if (verbosity_level >= 10) {
|
|
|
+ SERIAL_ECHOPGM("point #");
|
|
|
+ MYSERIAL.print(int(i));
|
|
|
+ SERIAL_ECHOLNPGM(":");
|
|
|
+ }
|
|
|
+
|
|
|
+ if (point_on_1st_row(i, npts)) {
|
|
|
+ if(verbosity_level >= 20) SERIAL_ECHOPGM("Point on first row");
|
|
|
+ float w = point_weight_y(i, npts, measured_pts[2 * i + 1]);
|
|
|
+ if (sqrt(errX) > BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_X ||
|
|
|
+ (w != 0.f && sqrt(errY) > BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_Y)) {
|
|
|
+ result = BED_SKEW_OFFSET_DETECTION_FITTING_FAILED;
|
|
|
+ if (verbosity_level >= 20) {
|
|
|
+ SERIAL_ECHOPGM(", weigth Y: ");
|
|
|
+ MYSERIAL.print(w);
|
|
|
+ if (sqrt(errX) > BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_X) SERIAL_ECHOPGM(", error X > max. error X");
|
|
|
+ if (w != 0.f && sqrt(errY) > BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_Y) SERIAL_ECHOPGM(", error Y > max. error Y");
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ if(verbosity_level >=20 ) SERIAL_ECHOPGM("Point not on first row");
|
|
|
+ if (err > BED_CALIBRATION_POINT_OFFSET_MAX_EUCLIDIAN) {
|
|
|
+ result = BED_SKEW_OFFSET_DETECTION_FITTING_FAILED;
|
|
|
+ if(verbosity_level >= 20) SERIAL_ECHOPGM(", error > max. error euclidian");
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if (verbosity_level >= 10) {
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ SERIAL_ECHOPGM("measured: (");
|
|
|
+ MYSERIAL.print(measured_pts[i * 2], 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(measured_pts[i * 2 + 1], 5);
|
|
|
+ SERIAL_ECHOPGM("); corrected: (");
|
|
|
+ MYSERIAL.print(x, 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(y, 5);
|
|
|
+ SERIAL_ECHOPGM("); target: (");
|
|
|
+ MYSERIAL.print(pgm_read_float(true_pts + i * 2), 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(pgm_read_float(true_pts + i * 2 + 1), 5);
|
|
|
+ SERIAL_ECHOLNPGM(")");
|
|
|
+ SERIAL_ECHOPGM("error: ");
|
|
|
+ MYSERIAL.print(err);
|
|
|
+ SERIAL_ECHOPGM(", error X: ");
|
|
|
+ MYSERIAL.print(sqrt(errX));
|
|
|
+ SERIAL_ECHOPGM(", error Y: ");
|
|
|
+ MYSERIAL.print(sqrt(errY));
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if (verbosity_level >= 20) {
|
|
|
+ SERIAL_ECHOLNPGM("Max. errors:");
|
|
|
+ SERIAL_ECHOPGM("Max. error X:");
|
|
|
+ MYSERIAL.println(BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_X);
|
|
|
+ SERIAL_ECHOPGM("Max. error Y:");
|
|
|
+ MYSERIAL.println(BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_Y);
|
|
|
+ SERIAL_ECHOPGM("Max. error euclidian:");
|
|
|
+ MYSERIAL.println(BED_CALIBRATION_POINT_OFFSET_MAX_EUCLIDIAN);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ }
|
|
|
+
|
|
|
+ #if 0
|
|
|
+ if (result == BED_SKEW_OFFSET_DETECTION_PERFECT && fabs(a1) < BED_SKEW_ANGLE_MILD && fabs(a2) < BED_SKEW_ANGLE_MILD) {
|
|
|
+ if (verbosity_level > 0)
|
|
|
+ SERIAL_ECHOLNPGM("Very little skew detected. Disabling skew correction.");
|
|
|
+
|
|
|
+ vec_x[0] = MACHINE_AXIS_SCALE_X;
|
|
|
+ vec_x[1] = 0.f;
|
|
|
+ vec_y[0] = 0.f;
|
|
|
+ vec_y[1] = MACHINE_AXIS_SCALE_Y;
|
|
|
+ }
|
|
|
+ #else
|
|
|
+ if (result == BED_SKEW_OFFSET_DETECTION_PERFECT) {
|
|
|
+ if (verbosity_level > 0)
|
|
|
+ SERIAL_ECHOLNPGM("Very little skew detected. Orthogonalizing the axes.");
|
|
|
+
|
|
|
+ a1 = 0.5f * (a1 + a2);
|
|
|
+ vec_x[0] = cos(a1) * MACHINE_AXIS_SCALE_X;
|
|
|
+ vec_x[1] = sin(a1) * MACHINE_AXIS_SCALE_X;
|
|
|
+ vec_y[0] = -sin(a1) * MACHINE_AXIS_SCALE_Y;
|
|
|
+ vec_y[1] = cos(a1) * MACHINE_AXIS_SCALE_Y;
|
|
|
+
|
|
|
+ cntr[0] = 0.f;
|
|
|
+ cntr[1] = 0.f;
|
|
|
+ float wx = 0.f;
|
|
|
+ float wy = 0.f;
|
|
|
+ for (int8_t i = 0; i < npts; ++ i) {
|
|
|
+ float x = vec_x[0] * measured_pts[i * 2] + vec_y[0] * measured_pts[i * 2 + 1];
|
|
|
+ float y = vec_x[1] * measured_pts[i * 2] + vec_y[1] * measured_pts[i * 2 + 1];
|
|
|
+ float w = point_weight_x(i, npts, y);
|
|
|
+ cntr[0] += w * (pgm_read_float(true_pts + i * 2) - x);
|
|
|
+ wx += w;
|
|
|
+ if (verbosity_level >= 20) {
|
|
|
+ MYSERIAL.print(i);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ SERIAL_ECHOLNPGM("Weight_x:");
|
|
|
+ MYSERIAL.print(w);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ SERIAL_ECHOLNPGM("cntr[0]:");
|
|
|
+ MYSERIAL.print(cntr[0]);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ SERIAL_ECHOLNPGM("wx:");
|
|
|
+ MYSERIAL.print(wx);
|
|
|
+ }
|
|
|
+ w = point_weight_y(i, npts, y);
|
|
|
+ cntr[1] += w * (pgm_read_float(true_pts + i * 2 + 1) - y);
|
|
|
+ wy += w;
|
|
|
+
|
|
|
+ if (verbosity_level >= 20) {
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ SERIAL_ECHOLNPGM("Weight_y:");
|
|
|
+ MYSERIAL.print(w);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ SERIAL_ECHOLNPGM("cntr[1]:");
|
|
|
+ MYSERIAL.print(cntr[1]);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ SERIAL_ECHOLNPGM("wy:");
|
|
|
+ MYSERIAL.print(wy);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ }
|
|
|
+ }
|
|
|
+ cntr[0] /= wx;
|
|
|
+ cntr[1] /= wy;
|
|
|
+ if (verbosity_level >= 20) {
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ SERIAL_ECHOLNPGM("Final cntr values:");
|
|
|
+ SERIAL_ECHOLNPGM("cntr[0]:");
|
|
|
+ MYSERIAL.print(cntr[0]);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ SERIAL_ECHOLNPGM("cntr[1]:");
|
|
|
+ MYSERIAL.print(cntr[1]);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ #endif
|
|
|
+
|
|
|
+
|
|
|
+ {
|
|
|
+ float d = vec_x[0] * vec_y[1] - vec_x[1] * vec_y[0];
|
|
|
+ float Ainv[2][2] = {
|
|
|
+ { vec_y[1] / d, -vec_y[0] / d },
|
|
|
+ { -vec_x[1] / d, vec_x[0] / d }
|
|
|
+ };
|
|
|
+ float cntrInv[2] = {
|
|
|
+ -Ainv[0][0] * cntr[0] - Ainv[0][1] * cntr[1],
|
|
|
+ -Ainv[1][0] * cntr[0] - Ainv[1][1] * cntr[1]
|
|
|
+ };
|
|
|
+ vec_x[0] = Ainv[0][0];
|
|
|
+ vec_x[1] = Ainv[1][0];
|
|
|
+ vec_y[0] = Ainv[0][1];
|
|
|
+ vec_y[1] = Ainv[1][1];
|
|
|
+ cntr[0] = cntrInv[0];
|
|
|
+ cntr[1] = cntrInv[1];
|
|
|
+ }
|
|
|
+
|
|
|
+ if (verbosity_level >= 1) {
|
|
|
+
|
|
|
+ SERIAL_ECHOPGM("X vector, adjusted: ");
|
|
|
+ MYSERIAL.print(vec_x[0], 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(vec_x[1], 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+
|
|
|
+ SERIAL_ECHOPGM("Y vector, adjusted: ");
|
|
|
+ MYSERIAL.print(vec_y[0], 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(vec_y[1], 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+
|
|
|
+ SERIAL_ECHOPGM("center, adjusted: ");
|
|
|
+ MYSERIAL.print(cntr[0], 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(cntr[1], 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ delay_keep_alive(100);
|
|
|
+ }
|
|
|
+
|
|
|
+ if (verbosity_level >= 2) {
|
|
|
+ SERIAL_ECHOLNPGM("Difference after correction: ");
|
|
|
+ for (uint8_t i = 0; i < npts; ++i) {
|
|
|
+ float x = vec_x[0] * pgm_read_float(true_pts + i * 2) + vec_y[0] * pgm_read_float(true_pts + i * 2 + 1) + cntr[0];
|
|
|
+ float y = vec_x[1] * pgm_read_float(true_pts + i * 2) + vec_y[1] * pgm_read_float(true_pts + i * 2 + 1) + cntr[1];
|
|
|
+ SERIAL_ECHOPGM("point #");
|
|
|
+ MYSERIAL.print(int(i));
|
|
|
+ SERIAL_ECHOPGM("measured: (");
|
|
|
+ MYSERIAL.print(measured_pts[i * 2], 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(measured_pts[i * 2 + 1], 5);
|
|
|
+ SERIAL_ECHOPGM("); measured-corrected: (");
|
|
|
+ MYSERIAL.print(x, 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(y, 5);
|
|
|
+ SERIAL_ECHOPGM("); target: (");
|
|
|
+ MYSERIAL.print(pgm_read_float(true_pts + i * 2), 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(pgm_read_float(true_pts + i * 2 + 1), 5);
|
|
|
+ SERIAL_ECHOPGM("), error: ");
|
|
|
+ MYSERIAL.print(sqrt(sqr(measured_pts[i * 2] - x) + sqr(measured_pts[i * 2 + 1] - y)));
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ }
|
|
|
+ if (verbosity_level >= 20) {
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ SERIAL_ECHOLNPGM("Calculate offset and skew returning result:");
|
|
|
+ MYSERIAL.print(int(result));
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ }
|
|
|
+ delay_keep_alive(100);
|
|
|
+ }
|
|
|
+
|
|
|
+ return result;
|
|
|
+}
|
|
|
+
|
|
|
+void reset_bed_offset_and_skew()
|
|
|
+{
|
|
|
+ eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_CENTER+0), 0x0FFFFFFFF);
|
|
|
+ eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_CENTER+4), 0x0FFFFFFFF);
|
|
|
+ eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_X +0), 0x0FFFFFFFF);
|
|
|
+ eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_X +4), 0x0FFFFFFFF);
|
|
|
+ eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_Y +0), 0x0FFFFFFFF);
|
|
|
+ eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_Y +4), 0x0FFFFFFFF);
|
|
|
+
|
|
|
+
|
|
|
+ for (int8_t i = 0; i < 4; ++ i)
|
|
|
+ eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_Z_JITTER+i*4), 0x0FFFFFFFF);
|
|
|
+}
|
|
|
+
|
|
|
+bool is_bed_z_jitter_data_valid()
|
|
|
+
|
|
|
+{
|
|
|
+ for (int8_t i = 0; i < 8; ++ i)
|
|
|
+ if (eeprom_read_word((uint16_t*)(EEPROM_BED_CALIBRATION_Z_JITTER+i*2)) == 0x0FFFF)
|
|
|
+ return false;
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
+static void world2machine_update(const float vec_x[2], const float vec_y[2], const float cntr[2])
|
|
|
+{
|
|
|
+ world2machine_rotation_and_skew[0][0] = vec_x[0];
|
|
|
+ world2machine_rotation_and_skew[1][0] = vec_x[1];
|
|
|
+ world2machine_rotation_and_skew[0][1] = vec_y[0];
|
|
|
+ world2machine_rotation_and_skew[1][1] = vec_y[1];
|
|
|
+ world2machine_shift[0] = cntr[0];
|
|
|
+ world2machine_shift[1] = cntr[1];
|
|
|
+
|
|
|
+ world2machine_correction_mode = WORLD2MACHINE_CORRECTION_NONE;
|
|
|
+ if (world2machine_shift[0] != 0.f || world2machine_shift[1] != 0.f)
|
|
|
+
|
|
|
+ world2machine_correction_mode |= WORLD2MACHINE_CORRECTION_SHIFT;
|
|
|
+ if (world2machine_rotation_and_skew[0][0] != 1.f || world2machine_rotation_and_skew[0][1] != 0.f ||
|
|
|
+ world2machine_rotation_and_skew[1][0] != 0.f || world2machine_rotation_and_skew[1][1] != 1.f) {
|
|
|
+
|
|
|
+ world2machine_correction_mode |= WORLD2MACHINE_CORRECTION_SKEW;
|
|
|
+
|
|
|
+ float d = world2machine_rotation_and_skew[0][0] * world2machine_rotation_and_skew[1][1] - world2machine_rotation_and_skew[1][0] * world2machine_rotation_and_skew[0][1];
|
|
|
+ world2machine_rotation_and_skew_inv[0][0] = world2machine_rotation_and_skew[1][1] / d;
|
|
|
+ world2machine_rotation_and_skew_inv[0][1] = -world2machine_rotation_and_skew[0][1] / d;
|
|
|
+ world2machine_rotation_and_skew_inv[1][0] = -world2machine_rotation_and_skew[1][0] / d;
|
|
|
+ world2machine_rotation_and_skew_inv[1][1] = world2machine_rotation_and_skew[0][0] / d;
|
|
|
+ } else {
|
|
|
+ world2machine_rotation_and_skew_inv[0][0] = 1.f;
|
|
|
+ world2machine_rotation_and_skew_inv[0][1] = 0.f;
|
|
|
+ world2machine_rotation_and_skew_inv[1][0] = 0.f;
|
|
|
+ world2machine_rotation_and_skew_inv[1][1] = 1.f;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void world2machine_reset()
|
|
|
+{
|
|
|
+ const float vx[] = { 1.f, 0.f };
|
|
|
+ const float vy[] = { 0.f, 1.f };
|
|
|
+ const float cntr[] = { 0.f, 0.f };
|
|
|
+ world2machine_update(vx, vy, cntr);
|
|
|
+}
|
|
|
+
|
|
|
+void world2machine_revert_to_uncorrected()
|
|
|
+{
|
|
|
+ if (world2machine_correction_mode != WORLD2MACHINE_CORRECTION_NONE) {
|
|
|
+
|
|
|
+ const float vx[] = { 1.f, 0.f };
|
|
|
+ const float vy[] = { 0.f, 1.f };
|
|
|
+ const float cntr[] = { 0.f, 0.f };
|
|
|
+ world2machine_update(vx, vy, cntr);
|
|
|
+
|
|
|
+ st_synchronize();
|
|
|
+ current_position[X_AXIS] = st_get_position_mm(X_AXIS);
|
|
|
+ current_position[Y_AXIS] = st_get_position_mm(Y_AXIS);
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+static inline bool vec_undef(const float v[2])
|
|
|
+{
|
|
|
+ const uint32_t *vx = (const uint32_t*)v;
|
|
|
+ return vx[0] == 0x0FFFFFFFF || vx[1] == 0x0FFFFFFFF;
|
|
|
+}
|
|
|
+
|
|
|
+void world2machine_initialize()
|
|
|
+{
|
|
|
+ SERIAL_ECHOLNPGM("world2machine_initialize");
|
|
|
+ float cntr[2] = {
|
|
|
+ eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER+0)),
|
|
|
+ eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER+4))
|
|
|
+ };
|
|
|
+ float vec_x[2] = {
|
|
|
+ eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +0)),
|
|
|
+ eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +4))
|
|
|
+ };
|
|
|
+ float vec_y[2] = {
|
|
|
+ eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +0)),
|
|
|
+ eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +4))
|
|
|
+ };
|
|
|
+
|
|
|
+ bool reset = false;
|
|
|
+ if (vec_undef(cntr) || vec_undef(vec_x) || vec_undef(vec_y)) {
|
|
|
+ SERIAL_ECHOLNPGM("Undefined bed correction matrix.");
|
|
|
+ reset = true;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+
|
|
|
+ float l = sqrt(vec_x[0] * vec_x[0] + vec_x[1] * vec_x[1]);
|
|
|
+ if (l < 0.9 || l > 1.1) {
|
|
|
+ SERIAL_ECHOLNPGM("X vector length:");
|
|
|
+ MYSERIAL.println(l);
|
|
|
+ SERIAL_ECHOLNPGM("Invalid bed correction matrix. Length of the X vector out of range.");
|
|
|
+ reset = true;
|
|
|
+ }
|
|
|
+
|
|
|
+ l = sqrt(vec_y[0] * vec_y[0] + vec_y[1] * vec_y[1]);
|
|
|
+ if (l < 0.9 || l > 1.1) {
|
|
|
+ SERIAL_ECHOLNPGM("Y vector length:");
|
|
|
+ MYSERIAL.println(l);
|
|
|
+ SERIAL_ECHOLNPGM("Invalid bed correction matrix. Length of the Y vector out of range.");
|
|
|
+ reset = true;
|
|
|
+ }
|
|
|
+
|
|
|
+ l = sqrt(cntr[0] * cntr[0] + cntr[1] * cntr[1]);
|
|
|
+ if (l > 15.f) {
|
|
|
+ SERIAL_ECHOLNPGM("Zero point correction:");
|
|
|
+ MYSERIAL.println(l);
|
|
|
+ SERIAL_ECHOLNPGM("Invalid bed correction matrix. Shift out of range.");
|
|
|
+ reset = true;
|
|
|
+ }
|
|
|
+
|
|
|
+ l = vec_x[0] * vec_y[0] + vec_x[1] * vec_y[1];
|
|
|
+ if (fabs(l) > 0.1f) {
|
|
|
+ SERIAL_ECHOLNPGM("Invalid bed correction matrix. X/Y axes are far from being perpendicular.");
|
|
|
+ reset = true;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if (reset) {
|
|
|
+ SERIAL_ECHOLNPGM("Invalid bed correction matrix. Resetting to identity.");
|
|
|
+ reset_bed_offset_and_skew();
|
|
|
+ world2machine_reset();
|
|
|
+ } else {
|
|
|
+ world2machine_update(vec_x, vec_y, cntr);
|
|
|
+
|
|
|
+ SERIAL_ECHOPGM("world2machine_initialize() loaded: ");
|
|
|
+ MYSERIAL.print(world2machine_rotation_and_skew[0][0], 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(world2machine_rotation_and_skew[0][1], 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(world2machine_rotation_and_skew[1][0], 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(world2machine_rotation_and_skew[1][1], 5);
|
|
|
+ SERIAL_ECHOPGM(", offset ");
|
|
|
+ MYSERIAL.print(world2machine_shift[0], 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(world2machine_shift[1], 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ */
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+void world2machine_update_current()
|
|
|
+{
|
|
|
+ float x = current_position[X_AXIS] - world2machine_shift[0];
|
|
|
+ float y = current_position[Y_AXIS] - world2machine_shift[1];
|
|
|
+ current_position[X_AXIS] = world2machine_rotation_and_skew_inv[0][0] * x + world2machine_rotation_and_skew_inv[0][1] * y;
|
|
|
+ current_position[Y_AXIS] = world2machine_rotation_and_skew_inv[1][0] * x + world2machine_rotation_and_skew_inv[1][1] * y;
|
|
|
+}
|
|
|
+
|
|
|
+static inline void go_xyz(float x, float y, float z, float fr)
|
|
|
+{
|
|
|
+ plan_buffer_line(x, y, z, current_position[E_AXIS], fr, active_extruder);
|
|
|
+ st_synchronize();
|
|
|
+}
|
|
|
+
|
|
|
+static inline void go_xy(float x, float y, float fr)
|
|
|
+{
|
|
|
+ plan_buffer_line(x, y, current_position[Z_AXIS], current_position[E_AXIS], fr, active_extruder);
|
|
|
+ st_synchronize();
|
|
|
+}
|
|
|
+
|
|
|
+static inline void go_to_current(float fr)
|
|
|
+{
|
|
|
+ plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], fr, active_extruder);
|
|
|
+ st_synchronize();
|
|
|
+}
|
|
|
+
|
|
|
+static inline void update_current_position_xyz()
|
|
|
+{
|
|
|
+ current_position[X_AXIS] = st_get_position_mm(X_AXIS);
|
|
|
+ current_position[Y_AXIS] = st_get_position_mm(Y_AXIS);
|
|
|
+ current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
|
|
|
+ plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
|
|
|
+}
|
|
|
+
|
|
|
+static inline void update_current_position_z()
|
|
|
+{
|
|
|
+ current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
|
|
|
+ plan_set_z_position(current_position[Z_AXIS]);
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+inline bool find_bed_induction_sensor_point_z(float minimum_z, uint8_t n_iter)
|
|
|
+{
|
|
|
+ SERIAL_ECHOLNPGM("find bed induction sensor point z");
|
|
|
+ bool endstops_enabled = enable_endstops(true);
|
|
|
+ bool endstop_z_enabled = enable_z_endstop(false);
|
|
|
+ float z = 0.f;
|
|
|
+ endstop_z_hit_on_purpose();
|
|
|
+
|
|
|
+
|
|
|
+ current_position[Z_AXIS] = minimum_z;
|
|
|
+ go_to_current(homing_feedrate[Z_AXIS]/60);
|
|
|
+
|
|
|
+ update_current_position_z();
|
|
|
+ if (! endstop_z_hit_on_purpose())
|
|
|
+ goto error;
|
|
|
+
|
|
|
+ for (uint8_t i = 0; i < n_iter; ++ i) {
|
|
|
+
|
|
|
+ current_position[Z_AXIS] += .5f;
|
|
|
+ go_to_current(homing_feedrate[Z_AXIS]/60);
|
|
|
+
|
|
|
+ current_position[Z_AXIS] = minimum_z;
|
|
|
+ go_to_current(homing_feedrate[Z_AXIS]/(4*60));
|
|
|
+
|
|
|
+ update_current_position_z();
|
|
|
+ if (! endstop_z_hit_on_purpose())
|
|
|
+ goto error;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ z += current_position[Z_AXIS];
|
|
|
+ }
|
|
|
+ current_position[Z_AXIS] = z;
|
|
|
+ if (n_iter > 1)
|
|
|
+ current_position[Z_AXIS] /= float(n_iter);
|
|
|
+
|
|
|
+ enable_endstops(endstops_enabled);
|
|
|
+ enable_z_endstop(endstop_z_enabled);
|
|
|
+
|
|
|
+ return true;
|
|
|
+
|
|
|
+error:
|
|
|
+
|
|
|
+ enable_endstops(endstops_enabled);
|
|
|
+ enable_z_endstop(endstop_z_enabled);
|
|
|
+ return false;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+#define FIND_BED_INDUCTION_SENSOR_POINT_X_RADIUS (8.f)
|
|
|
+#define FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS (6.f)
|
|
|
+#define FIND_BED_INDUCTION_SENSOR_POINT_XY_STEP (1.f)
|
|
|
+#define FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP (0.2f)
|
|
|
+inline bool find_bed_induction_sensor_point_xy()
|
|
|
+{
|
|
|
+ MYSERIAL.println("find bed induction sensor point xy");
|
|
|
+ float feedrate = homing_feedrate[X_AXIS] / 60.f;
|
|
|
+ bool found = false;
|
|
|
+
|
|
|
+ {
|
|
|
+ float x0 = current_position[X_AXIS] - FIND_BED_INDUCTION_SENSOR_POINT_X_RADIUS;
|
|
|
+ float x1 = current_position[X_AXIS] + FIND_BED_INDUCTION_SENSOR_POINT_X_RADIUS;
|
|
|
+ float y0 = current_position[Y_AXIS] - FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS;
|
|
|
+ float y1 = current_position[Y_AXIS] + FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS;
|
|
|
+ uint8_t nsteps_y;
|
|
|
+ uint8_t i;
|
|
|
+ if (x0 < X_MIN_POS)
|
|
|
+ x0 = X_MIN_POS;
|
|
|
+ if (x1 > X_MAX_POS)
|
|
|
+ x1 = X_MAX_POS;
|
|
|
+ if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION)
|
|
|
+ y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
|
|
|
+ if (y1 > Y_MAX_POS)
|
|
|
+ y1 = Y_MAX_POS;
|
|
|
+ nsteps_y = int(ceil((y1 - y0) / FIND_BED_INDUCTION_SENSOR_POINT_XY_STEP));
|
|
|
+
|
|
|
+ enable_endstops(false);
|
|
|
+ bool dir_positive = true;
|
|
|
+
|
|
|
+
|
|
|
+ go_xyz(x0, y0, current_position[Z_AXIS], feedrate);
|
|
|
+
|
|
|
+ endstops_hit_on_purpose();
|
|
|
+ enable_z_endstop(true);
|
|
|
+ while (current_position[Z_AXIS] > -10.f) {
|
|
|
+
|
|
|
+ current_position[Y_AXIS] = y0;
|
|
|
+ for (i = 0; i < nsteps_y; current_position[Y_AXIS] += (y1 - y0) / float(nsteps_y - 1), ++ i) {
|
|
|
+
|
|
|
+ current_position[Z_AXIS] -= FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP / float(nsteps_y);
|
|
|
+ go_xyz(dir_positive ? x1 : x0, current_position[Y_AXIS], current_position[Z_AXIS], feedrate);
|
|
|
+ dir_positive = ! dir_positive;
|
|
|
+ if (endstop_z_hit_on_purpose())
|
|
|
+ goto endloop;
|
|
|
+ }
|
|
|
+ for (i = 0; i < nsteps_y; current_position[Y_AXIS] -= (y1 - y0) / float(nsteps_y - 1), ++ i) {
|
|
|
+
|
|
|
+ current_position[Z_AXIS] -= FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP / float(nsteps_y);
|
|
|
+ go_xyz(dir_positive ? x1 : x0, current_position[Y_AXIS], current_position[Z_AXIS], feedrate);
|
|
|
+ dir_positive = ! dir_positive;
|
|
|
+ if (endstop_z_hit_on_purpose())
|
|
|
+ goto endloop;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ endloop:
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ update_current_position_xyz();
|
|
|
+
|
|
|
+
|
|
|
+ for (int8_t iter = 0; iter < 3; ++ iter) {
|
|
|
+ if (iter > 0) {
|
|
|
+
|
|
|
+ current_position[Z_AXIS] -= 0.02f;
|
|
|
+ go_xyz(current_position[X_AXIS], current_position[Y_AXIS], MESH_HOME_Z_SEARCH, homing_feedrate[Z_AXIS]/60);
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ float a, b;
|
|
|
+ enable_endstops(false);
|
|
|
+ enable_z_endstop(false);
|
|
|
+ current_position[Y_AXIS] = y0;
|
|
|
+ go_xy(x0, current_position[Y_AXIS], feedrate);
|
|
|
+ enable_z_endstop(true);
|
|
|
+ found = false;
|
|
|
+ for (i = 0, dir_positive = true; i < nsteps_y; current_position[Y_AXIS] += (y1 - y0) / float(nsteps_y - 1), ++ i, dir_positive = ! dir_positive) {
|
|
|
+ go_xy(dir_positive ? x1 : x0, current_position[Y_AXIS], feedrate);
|
|
|
+ if (endstop_z_hit_on_purpose()) {
|
|
|
+ found = true;
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ update_current_position_xyz();
|
|
|
+ if (! found) {
|
|
|
+
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+
|
|
|
+ a = current_position[Y_AXIS];
|
|
|
+
|
|
|
+ enable_z_endstop(false);
|
|
|
+ current_position[Y_AXIS] = y1;
|
|
|
+ go_xy(x0, current_position[Y_AXIS], feedrate);
|
|
|
+ enable_z_endstop(true);
|
|
|
+ found = false;
|
|
|
+ for (i = 0, dir_positive = true; i < nsteps_y; current_position[Y_AXIS] -= (y1 - y0) / float(nsteps_y - 1), ++ i, dir_positive = ! dir_positive) {
|
|
|
+ go_xy(dir_positive ? x1 : x0, current_position[Y_AXIS], feedrate);
|
|
|
+ if (endstop_z_hit_on_purpose()) {
|
|
|
+ found = true;
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ update_current_position_xyz();
|
|
|
+ if (! found) {
|
|
|
+
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+
|
|
|
+ b = current_position[Y_AXIS];
|
|
|
+ current_position[Y_AXIS] = 0.5f * (a + b);
|
|
|
+
|
|
|
+
|
|
|
+ found = false;
|
|
|
+ enable_z_endstop(false);
|
|
|
+ go_xy(x0, current_position[Y_AXIS], feedrate);
|
|
|
+ enable_z_endstop(true);
|
|
|
+ go_xy(x1, current_position[Y_AXIS], feedrate);
|
|
|
+ update_current_position_xyz();
|
|
|
+ if (! endstop_z_hit_on_purpose()) {
|
|
|
+
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+
|
|
|
+ a = current_position[X_AXIS];
|
|
|
+ enable_z_endstop(false);
|
|
|
+ go_xy(x1, current_position[Y_AXIS], feedrate);
|
|
|
+ enable_z_endstop(true);
|
|
|
+ go_xy(x0, current_position[Y_AXIS], feedrate);
|
|
|
+ update_current_position_xyz();
|
|
|
+ if (! endstop_z_hit_on_purpose()) {
|
|
|
+
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+
|
|
|
+ b = current_position[X_AXIS];
|
|
|
+
|
|
|
+ enable_z_endstop(false);
|
|
|
+ current_position[X_AXIS] = 0.5f * (a + b);
|
|
|
+ go_xy(current_position[X_AXIS], current_position[Y_AXIS], feedrate);
|
|
|
+ found = true;
|
|
|
+
|
|
|
+#if 1
|
|
|
+
|
|
|
+ found = false;
|
|
|
+ enable_z_endstop(false);
|
|
|
+ go_xy(current_position[X_AXIS], y0, feedrate);
|
|
|
+ enable_z_endstop(true);
|
|
|
+ go_xy(current_position[X_AXIS], y1, feedrate);
|
|
|
+ update_current_position_xyz();
|
|
|
+ if (! endstop_z_hit_on_purpose()) {
|
|
|
+
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+
|
|
|
+ a = current_position[Y_AXIS];
|
|
|
+ enable_z_endstop(false);
|
|
|
+ go_xy(current_position[X_AXIS], y1, feedrate);
|
|
|
+ enable_z_endstop(true);
|
|
|
+ go_xy(current_position[X_AXIS], y0, feedrate);
|
|
|
+ update_current_position_xyz();
|
|
|
+ if (! endstop_z_hit_on_purpose()) {
|
|
|
+
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+
|
|
|
+ b = current_position[Y_AXIS];
|
|
|
+
|
|
|
+ enable_z_endstop(false);
|
|
|
+ current_position[Y_AXIS] = 0.5f * (a + b);
|
|
|
+ go_xy(current_position[X_AXIS], current_position[Y_AXIS], feedrate);
|
|
|
+ found = true;
|
|
|
+#endif
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ enable_z_endstop(false);
|
|
|
+ return found;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+inline bool improve_bed_induction_sensor_point()
|
|
|
+{
|
|
|
+ static const float search_radius = 8.f;
|
|
|
+
|
|
|
+ bool endstops_enabled = enable_endstops(false);
|
|
|
+ bool endstop_z_enabled = enable_z_endstop(false);
|
|
|
+ bool found = false;
|
|
|
+ float feedrate = homing_feedrate[X_AXIS] / 60.f;
|
|
|
+ float center_old_x = current_position[X_AXIS];
|
|
|
+ float center_old_y = current_position[Y_AXIS];
|
|
|
+ float center_x = 0.f;
|
|
|
+ float center_y = 0.f;
|
|
|
+
|
|
|
+ for (uint8_t iter = 0; iter < 4; ++ iter) {
|
|
|
+ switch (iter) {
|
|
|
+ case 0:
|
|
|
+ destination[X_AXIS] = center_old_x - search_radius * 0.707;
|
|
|
+ destination[Y_AXIS] = center_old_y - search_radius * 0.707;
|
|
|
+ break;
|
|
|
+ case 1:
|
|
|
+ destination[X_AXIS] = center_old_x + search_radius * 0.707;
|
|
|
+ destination[Y_AXIS] = center_old_y + search_radius * 0.707;
|
|
|
+ break;
|
|
|
+ case 2:
|
|
|
+ destination[X_AXIS] = center_old_x + search_radius * 0.707;
|
|
|
+ destination[Y_AXIS] = center_old_y - search_radius * 0.707;
|
|
|
+ break;
|
|
|
+ case 3:
|
|
|
+ default:
|
|
|
+ destination[X_AXIS] = center_old_x - search_radius * 0.707;
|
|
|
+ destination[Y_AXIS] = center_old_y + search_radius * 0.707;
|
|
|
+ break;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ float vx = destination[X_AXIS] - center_old_x;
|
|
|
+ float vy = destination[Y_AXIS] - center_old_y;
|
|
|
+ float l = sqrt(vx*vx+vy*vy);
|
|
|
+ float t;
|
|
|
+ if (destination[X_AXIS] < X_MIN_POS) {
|
|
|
+
|
|
|
+ t = (center_x - X_MIN_POS) / l;
|
|
|
+ destination[X_AXIS] = X_MIN_POS;
|
|
|
+ destination[Y_AXIS] = center_old_y + t * vy;
|
|
|
+ } else if (destination[X_AXIS] > X_MAX_POS) {
|
|
|
+
|
|
|
+ t = (X_MAX_POS - center_x) / l;
|
|
|
+ destination[X_AXIS] = X_MAX_POS;
|
|
|
+ destination[Y_AXIS] = center_old_y + t * vy;
|
|
|
+ }
|
|
|
+ if (destination[Y_AXIS] < Y_MIN_POS_FOR_BED_CALIBRATION) {
|
|
|
+
|
|
|
+ t = (center_y - Y_MIN_POS_FOR_BED_CALIBRATION) / l;
|
|
|
+ destination[X_AXIS] = center_old_x + t * vx;
|
|
|
+ destination[Y_AXIS] = Y_MIN_POS_FOR_BED_CALIBRATION;
|
|
|
+ } else if (destination[Y_AXIS] > Y_MAX_POS) {
|
|
|
+
|
|
|
+ t = (Y_MAX_POS - center_y) / l;
|
|
|
+ destination[X_AXIS] = center_old_x + t * vx;
|
|
|
+ destination[Y_AXIS] = Y_MAX_POS;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ enable_endstops(false);
|
|
|
+ go_xy(destination[X_AXIS], destination[Y_AXIS], feedrate);
|
|
|
+
|
|
|
+ enable_endstops(true);
|
|
|
+ go_xy(center_old_x, center_old_y, feedrate);
|
|
|
+ update_current_position_xyz();
|
|
|
+
|
|
|
+ center_x += current_position[X_AXIS];
|
|
|
+ center_y += current_position[Y_AXIS];
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ center_x /= 4.f;
|
|
|
+ center_y /= 4.f;
|
|
|
+ current_position[X_AXIS] = center_x;
|
|
|
+ current_position[Y_AXIS] = center_y;
|
|
|
+ enable_endstops(false);
|
|
|
+ go_xy(current_position[X_AXIS], current_position[Y_AXIS], feedrate);
|
|
|
+
|
|
|
+ enable_endstops(endstops_enabled);
|
|
|
+ enable_z_endstop(endstop_z_enabled);
|
|
|
+ return found;
|
|
|
+}
|
|
|
+
|
|
|
+static inline void debug_output_point(const char *type, const float &x, const float &y, const float &z)
|
|
|
+{
|
|
|
+ SERIAL_ECHOPGM("Measured ");
|
|
|
+ SERIAL_ECHORPGM(type);
|
|
|
+ SERIAL_ECHOPGM(" ");
|
|
|
+ MYSERIAL.print(x, 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(y, 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(z, 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+#define IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS (8.f)
|
|
|
+inline bool improve_bed_induction_sensor_point2(bool lift_z_on_min_y, int8_t verbosity_level)
|
|
|
+{
|
|
|
+ float center_old_x = current_position[X_AXIS];
|
|
|
+ float center_old_y = current_position[Y_AXIS];
|
|
|
+ float a, b;
|
|
|
+ bool point_small = false;
|
|
|
+
|
|
|
+ enable_endstops(false);
|
|
|
+
|
|
|
+ {
|
|
|
+ float x0 = center_old_x - IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS;
|
|
|
+ float x1 = center_old_x + IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS;
|
|
|
+ if (x0 < X_MIN_POS)
|
|
|
+ x0 = X_MIN_POS;
|
|
|
+ if (x1 > X_MAX_POS)
|
|
|
+ x1 = X_MAX_POS;
|
|
|
+
|
|
|
+
|
|
|
+ enable_z_endstop(false);
|
|
|
+ go_xy(x0, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ enable_z_endstop(true);
|
|
|
+ go_xy(x1, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ update_current_position_xyz();
|
|
|
+ if (! endstop_z_hit_on_purpose()) {
|
|
|
+ current_position[X_AXIS] = center_old_x;
|
|
|
+ goto canceled;
|
|
|
+ }
|
|
|
+ a = current_position[X_AXIS];
|
|
|
+ enable_z_endstop(false);
|
|
|
+ go_xy(x1, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ enable_z_endstop(true);
|
|
|
+ go_xy(x0, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ update_current_position_xyz();
|
|
|
+ if (! endstop_z_hit_on_purpose()) {
|
|
|
+ current_position[X_AXIS] = center_old_x;
|
|
|
+ goto canceled;
|
|
|
+ }
|
|
|
+ b = current_position[X_AXIS];
|
|
|
+ if (b - a < MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
|
|
|
+ if (verbosity_level >= 5) {
|
|
|
+ SERIAL_ECHOPGM("Point width too small: ");
|
|
|
+ SERIAL_ECHO(b - a);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ }
|
|
|
+
|
|
|
+ if (b - a < 0.5f * MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
|
|
|
+
|
|
|
+ current_position[X_AXIS] = center_old_x;
|
|
|
+ goto canceled;
|
|
|
+ } else {
|
|
|
+
|
|
|
+ point_small = true;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if (verbosity_level >= 5) {
|
|
|
+ debug_output_point(PSTR("left" ), a, current_position[Y_AXIS], current_position[Z_AXIS]);
|
|
|
+ debug_output_point(PSTR("right"), b, current_position[Y_AXIS], current_position[Z_AXIS]);
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ enable_z_endstop(false);
|
|
|
+ current_position[X_AXIS] = 0.5f * (a + b);
|
|
|
+ go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ }
|
|
|
+
|
|
|
+ {
|
|
|
+ float y0 = center_old_y - IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS;
|
|
|
+ float y1 = center_old_y + IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS;
|
|
|
+ if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION)
|
|
|
+ y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
|
|
|
+ if (y1 > Y_MAX_POS)
|
|
|
+ y1 = Y_MAX_POS;
|
|
|
+
|
|
|
+
|
|
|
+ enable_z_endstop(false);
|
|
|
+ go_xy(current_position[X_AXIS], y0, homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ if (lift_z_on_min_y) {
|
|
|
+
|
|
|
+
|
|
|
+ go_xyz(current_position[X_AXIS], y0, current_position[Z_AXIS]+1.5f, homing_feedrate[Z_AXIS] / 60.f);
|
|
|
+
|
|
|
+ go_xyz(current_position[X_AXIS], y0, current_position[Z_AXIS], homing_feedrate[Z_AXIS] / 60.f);
|
|
|
+ }
|
|
|
+ if (lift_z_on_min_y && (READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING) == 1) {
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ a = current_position[Y_AXIS];
|
|
|
+ } else {
|
|
|
+ enable_z_endstop(true);
|
|
|
+ go_xy(current_position[X_AXIS], y1, homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ update_current_position_xyz();
|
|
|
+ if (! endstop_z_hit_on_purpose()) {
|
|
|
+ current_position[Y_AXIS] = center_old_y;
|
|
|
+ goto canceled;
|
|
|
+ }
|
|
|
+ a = current_position[Y_AXIS];
|
|
|
+ }
|
|
|
+ enable_z_endstop(false);
|
|
|
+ go_xy(current_position[X_AXIS], y1, homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ enable_z_endstop(true);
|
|
|
+ go_xy(current_position[X_AXIS], y0, homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ update_current_position_xyz();
|
|
|
+ if (! endstop_z_hit_on_purpose()) {
|
|
|
+ current_position[Y_AXIS] = center_old_y;
|
|
|
+ goto canceled;
|
|
|
+ }
|
|
|
+ b = current_position[Y_AXIS];
|
|
|
+ if (b - a < MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
|
|
|
+
|
|
|
+ if (verbosity_level >= 5) {
|
|
|
+ SERIAL_ECHOPGM("Point height too small: ");
|
|
|
+ SERIAL_ECHO(b - a);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ }
|
|
|
+ if (b - a < 0.5f * MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
|
|
|
+
|
|
|
+ current_position[Y_AXIS] = center_old_y;
|
|
|
+ goto canceled;
|
|
|
+ } else {
|
|
|
+
|
|
|
+ point_small = true;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if (verbosity_level >= 5) {
|
|
|
+ debug_output_point(PSTR("top" ), current_position[X_AXIS], a, current_position[Z_AXIS]);
|
|
|
+ debug_output_point(PSTR("bottom"), current_position[X_AXIS], b, current_position[Z_AXIS]);
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ enable_z_endstop(false);
|
|
|
+ current_position[Y_AXIS] = 0.5f * (a + b);
|
|
|
+ go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ return ! point_small;
|
|
|
+
|
|
|
+canceled:
|
|
|
+
|
|
|
+ enable_z_endstop(false);
|
|
|
+ go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ return false;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+#define IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS (4.f)
|
|
|
+#define IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y (0.1f)
|
|
|
+inline bool improve_bed_induction_sensor_point3(int verbosity_level)
|
|
|
+{
|
|
|
+ float center_old_x = current_position[X_AXIS];
|
|
|
+ float center_old_y = current_position[Y_AXIS];
|
|
|
+ float a, b;
|
|
|
+ bool result = true;
|
|
|
+
|
|
|
+ if (verbosity_level >= 20) MYSERIAL.println("Improve bed induction sensor point3");
|
|
|
+
|
|
|
+
|
|
|
+ {
|
|
|
+ float x0 = center_old_x - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
|
|
|
+ float x1 = center_old_x + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
|
|
|
+ float y0 = center_old_y - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
|
|
|
+ float y1 = center_old_y + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
|
|
|
+ float y = y0;
|
|
|
+
|
|
|
+ if (x0 < X_MIN_POS)
|
|
|
+ x0 = X_MIN_POS;
|
|
|
+ if (x1 > X_MAX_POS)
|
|
|
+ x1 = X_MAX_POS;
|
|
|
+ if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION)
|
|
|
+ y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
|
|
|
+ if (y1 > Y_MAX_POS)
|
|
|
+ y1 = Y_MAX_POS;
|
|
|
+
|
|
|
+ if (verbosity_level >= 20) {
|
|
|
+ SERIAL_ECHOPGM("Initial position: ");
|
|
|
+ SERIAL_ECHO(center_old_x);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ SERIAL_ECHO(center_old_y);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ float dmax = 0.f;
|
|
|
+ float xmax1 = 0.f;
|
|
|
+ float xmax2 = 0.f;
|
|
|
+ for (y = y0; y < y1; y += IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
|
|
|
+ enable_z_endstop(false);
|
|
|
+ go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ enable_z_endstop(true);
|
|
|
+ go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ update_current_position_xyz();
|
|
|
+ if (! endstop_z_hit_on_purpose()) {
|
|
|
+ continue;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ }
|
|
|
+ a = current_position[X_AXIS];
|
|
|
+ enable_z_endstop(false);
|
|
|
+ go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ enable_z_endstop(true);
|
|
|
+ go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ update_current_position_xyz();
|
|
|
+ if (! endstop_z_hit_on_purpose()) {
|
|
|
+ continue;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ }
|
|
|
+ b = current_position[X_AXIS];
|
|
|
+ if (verbosity_level >= 5) {
|
|
|
+ debug_output_point(PSTR("left" ), a, current_position[Y_AXIS], current_position[Z_AXIS]);
|
|
|
+ debug_output_point(PSTR("right"), b, current_position[Y_AXIS], current_position[Z_AXIS]);
|
|
|
+ }
|
|
|
+ float d = b - a;
|
|
|
+ if (d > dmax) {
|
|
|
+ xmax1 = 0.5f * (a + b);
|
|
|
+ dmax = d;
|
|
|
+ } else if (dmax > 0.) {
|
|
|
+ y0 = y - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y;
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if (dmax == 0.) {
|
|
|
+ if (verbosity_level > 0)
|
|
|
+ SERIAL_PROTOCOLPGM("failed - not found\n");
|
|
|
+ current_position[X_AXIS] = center_old_x;
|
|
|
+ current_position[Y_AXIS] = center_old_y;
|
|
|
+ goto canceled;
|
|
|
+ }
|
|
|
+
|
|
|
+ {
|
|
|
+
|
|
|
+ enable_z_endstop(false);
|
|
|
+ go_xy(xmax1, y0 + IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS, homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ enable_z_endstop(true);
|
|
|
+ go_xy(xmax1, max(y0 - IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS, Y_MIN_POS_FOR_BED_CALIBRATION), homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ update_current_position_xyz();
|
|
|
+ if (! endstop_z_hit_on_purpose()) {
|
|
|
+ current_position[Y_AXIS] = center_old_y;
|
|
|
+ goto canceled;
|
|
|
+ }
|
|
|
+ if (verbosity_level >= 5)
|
|
|
+ debug_output_point(PSTR("top" ), current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
|
|
|
+ y1 = current_position[Y_AXIS];
|
|
|
+ }
|
|
|
+
|
|
|
+ if (y1 <= y0) {
|
|
|
+
|
|
|
+ current_position[Y_AXIS] = center_old_y;
|
|
|
+ goto canceled;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ dmax = 0.f;
|
|
|
+
|
|
|
+
|
|
|
+ for (y = y1; y >= y0; y -= IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
|
|
|
+ enable_z_endstop(false);
|
|
|
+ go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ enable_z_endstop(true);
|
|
|
+ go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ update_current_position_xyz();
|
|
|
+ if (! endstop_z_hit_on_purpose()) {
|
|
|
+ continue;
|
|
|
+
|
|
|
+ current_position[X_AXIS] = center_old_x;
|
|
|
+ SERIAL_PROTOCOLPGM("Failed 3\n");
|
|
|
+ goto canceled;
|
|
|
+ */
|
|
|
+ }
|
|
|
+ a = current_position[X_AXIS];
|
|
|
+ enable_z_endstop(false);
|
|
|
+ go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ enable_z_endstop(true);
|
|
|
+ go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ update_current_position_xyz();
|
|
|
+ if (! endstop_z_hit_on_purpose()) {
|
|
|
+ continue;
|
|
|
+
|
|
|
+ current_position[X_AXIS] = center_old_x;
|
|
|
+ SERIAL_PROTOCOLPGM("Failed 4\n");
|
|
|
+ goto canceled;
|
|
|
+ */
|
|
|
+ }
|
|
|
+ b = current_position[X_AXIS];
|
|
|
+ if (verbosity_level >= 5) {
|
|
|
+ debug_output_point(PSTR("left" ), a, current_position[Y_AXIS], current_position[Z_AXIS]);
|
|
|
+ debug_output_point(PSTR("right"), b, current_position[Y_AXIS], current_position[Z_AXIS]);
|
|
|
+ }
|
|
|
+ float d = b - a;
|
|
|
+ if (d > dmax) {
|
|
|
+ xmax2 = 0.5f * (a + b);
|
|
|
+ dmax = d;
|
|
|
+ } else if (dmax > 0.) {
|
|
|
+ y1 = y + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y;
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ float xmax, ymax;
|
|
|
+ if (dmax == 0.f) {
|
|
|
+
|
|
|
+ xmax = xmax1;
|
|
|
+ ymax = y0;
|
|
|
+ } else {
|
|
|
+
|
|
|
+ xmax = xmax2;
|
|
|
+ ymax = 0.5f * (y0 + y1);
|
|
|
+ for (; y >= y0; y -= IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
|
|
|
+ enable_z_endstop(false);
|
|
|
+ go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ enable_z_endstop(true);
|
|
|
+ go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ update_current_position_xyz();
|
|
|
+ if (! endstop_z_hit_on_purpose()) {
|
|
|
+ continue;
|
|
|
+
|
|
|
+ current_position[X_AXIS] = center_old_x;
|
|
|
+ SERIAL_PROTOCOLPGM("Failed 3\n");
|
|
|
+ goto canceled;
|
|
|
+ */
|
|
|
+ }
|
|
|
+ a = current_position[X_AXIS];
|
|
|
+ enable_z_endstop(false);
|
|
|
+ go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ enable_z_endstop(true);
|
|
|
+ go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ update_current_position_xyz();
|
|
|
+ if (! endstop_z_hit_on_purpose()) {
|
|
|
+ continue;
|
|
|
+
|
|
|
+ current_position[X_AXIS] = center_old_x;
|
|
|
+ SERIAL_PROTOCOLPGM("Failed 4\n");
|
|
|
+ goto canceled;
|
|
|
+ */
|
|
|
+ }
|
|
|
+ b = current_position[X_AXIS];
|
|
|
+ if (verbosity_level >= 5) {
|
|
|
+ debug_output_point(PSTR("left" ), a, current_position[Y_AXIS], current_position[Z_AXIS]);
|
|
|
+ debug_output_point(PSTR("right"), b, current_position[Y_AXIS], current_position[Z_AXIS]);
|
|
|
+ }
|
|
|
+ float d = b - a;
|
|
|
+ if (d > dmax) {
|
|
|
+ xmax = 0.5f * (a + b);
|
|
|
+ ymax = y;
|
|
|
+ dmax = d;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ {
|
|
|
+
|
|
|
+
|
|
|
+ enable_z_endstop(false);
|
|
|
+ go_xy(xmax, ymax + IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS, homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ enable_z_endstop(true);
|
|
|
+ go_xy(xmax, max(ymax - IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS, Y_MIN_POS_FOR_BED_CALIBRATION), homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ update_current_position_xyz();
|
|
|
+ if (! endstop_z_hit_on_purpose()) {
|
|
|
+ current_position[Y_AXIS] = center_old_y;
|
|
|
+ goto canceled;
|
|
|
+ }
|
|
|
+ if (verbosity_level >= 5)
|
|
|
+ debug_output_point(PSTR("top" ), current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
|
|
|
+ if (current_position[Y_AXIS] - Y_MIN_POS_FOR_BED_CALIBRATION < 0.5f * dmax) {
|
|
|
+
|
|
|
+
|
|
|
+ if (current_position[Y_AXIS] < ymax || dmax < 0.5f * MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
|
|
|
+ if (verbosity_level >= 5) {
|
|
|
+ SERIAL_ECHOPGM("Partial point diameter too small: ");
|
|
|
+ SERIAL_ECHO(dmax);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ }
|
|
|
+ result = false;
|
|
|
+ } else {
|
|
|
+
|
|
|
+ float h = current_position[Y_AXIS] - ymax;
|
|
|
+ float r = dmax * dmax / (8.f * h) + 0.5f * h;
|
|
|
+ if (r < 0.8f * MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
|
|
|
+ if (verbosity_level >= 5) {
|
|
|
+ SERIAL_ECHOPGM("Partial point estimated radius too small: ");
|
|
|
+ SERIAL_ECHO(r);
|
|
|
+ SERIAL_ECHOPGM(", dmax:");
|
|
|
+ SERIAL_ECHO(dmax);
|
|
|
+ SERIAL_ECHOPGM(", h:");
|
|
|
+ SERIAL_ECHO(h);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ }
|
|
|
+ result = false;
|
|
|
+ } else {
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ r = 0.5f * (0.5f * dmax + r);
|
|
|
+ ymax = current_position[Y_AXIS] - r;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ } else {
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ result = xmax >= MIN_BED_SENSOR_POINT_RESPONSE_DMR;
|
|
|
+ if (y0 > Y_MIN_POS_FOR_BED_CALIBRATION + 0.2f)
|
|
|
+
|
|
|
+
|
|
|
+ ymax = 0.5f * ymax + 0.25f * (y0 + y1);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ enable_z_endstop(false);
|
|
|
+ current_position[X_AXIS] = xmax;
|
|
|
+ current_position[Y_AXIS] = ymax;
|
|
|
+ if (verbosity_level >= 20) {
|
|
|
+ SERIAL_ECHOPGM("Adjusted position: ");
|
|
|
+ SERIAL_ECHO(current_position[X_AXIS]);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ SERIAL_ECHO(current_position[Y_AXIS]);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ go_xy(current_position[X_AXIS], max(Y_MIN_POS, current_position[Y_AXIS]), homing_feedrate[X_AXIS] / 60.f);
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ if (result)
|
|
|
+ return true;
|
|
|
+
|
|
|
+
|
|
|
+canceled:
|
|
|
+
|
|
|
+ enable_z_endstop(false);
|
|
|
+ if (current_position[Y_AXIS] < Y_MIN_POS)
|
|
|
+ current_position[Y_AXIS] = Y_MIN_POS;
|
|
|
+ go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ return false;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+inline void scan_bed_induction_sensor_point()
|
|
|
+{
|
|
|
+ float center_old_x = current_position[X_AXIS];
|
|
|
+ float center_old_y = current_position[Y_AXIS];
|
|
|
+ float x0 = center_old_x - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
|
|
|
+ float x1 = center_old_x + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
|
|
|
+ float y0 = center_old_y - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
|
|
|
+ float y1 = center_old_y + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
|
|
|
+ float y = y0;
|
|
|
+
|
|
|
+ if (x0 < X_MIN_POS)
|
|
|
+ x0 = X_MIN_POS;
|
|
|
+ if (x1 > X_MAX_POS)
|
|
|
+ x1 = X_MAX_POS;
|
|
|
+ if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION)
|
|
|
+ y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
|
|
|
+ if (y1 > Y_MAX_POS)
|
|
|
+ y1 = Y_MAX_POS;
|
|
|
+
|
|
|
+ for (float y = y0; y < y1; y += IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
|
|
|
+ enable_z_endstop(false);
|
|
|
+ go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ enable_z_endstop(true);
|
|
|
+ go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ update_current_position_xyz();
|
|
|
+ if (endstop_z_hit_on_purpose())
|
|
|
+ debug_output_point(PSTR("left" ), current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
|
|
|
+ enable_z_endstop(false);
|
|
|
+ go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ enable_z_endstop(true);
|
|
|
+ go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ update_current_position_xyz();
|
|
|
+ if (endstop_z_hit_on_purpose())
|
|
|
+ debug_output_point(PSTR("right"), current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
|
|
|
+ }
|
|
|
+
|
|
|
+ enable_z_endstop(false);
|
|
|
+ current_position[X_AXIS] = center_old_x;
|
|
|
+ current_position[Y_AXIS] = center_old_y;
|
|
|
+ go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
|
|
|
+}
|
|
|
+
|
|
|
+#define MESH_BED_CALIBRATION_SHOW_LCD
|
|
|
+
|
|
|
+BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level, uint8_t &too_far_mask)
|
|
|
+{
|
|
|
+
|
|
|
+ refresh_cmd_timeout();
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ float *pts = &mbl.z_values[0][0];
|
|
|
+ float *vec_x = pts + 2 * 4;
|
|
|
+ float *vec_y = vec_x + 2;
|
|
|
+ float *cntr = vec_y + 2;
|
|
|
+ memset(pts, 0, sizeof(float) * 7 * 7);
|
|
|
+ uint8_t iteration = 0;
|
|
|
+ BedSkewOffsetDetectionResultType result;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ while (iteration < 3) {
|
|
|
+
|
|
|
+ SERIAL_ECHOPGM("Iteration: ");
|
|
|
+ MYSERIAL.println(int(iteration + 1));
|
|
|
+ if (verbosity_level >= 20) {
|
|
|
+ SERIAL_ECHOLNPGM("Vectors: ");
|
|
|
+
|
|
|
+ SERIAL_ECHOPGM("vec_x[0]:");
|
|
|
+ MYSERIAL.print(vec_x[0], 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ SERIAL_ECHOPGM("vec_x[1]:");
|
|
|
+ MYSERIAL.print(vec_x[1], 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ SERIAL_ECHOPGM("vec_y[0]:");
|
|
|
+ MYSERIAL.print(vec_y[0], 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ SERIAL_ECHOPGM("vec_y[1]:");
|
|
|
+ MYSERIAL.print(vec_y[1], 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ SERIAL_ECHOPGM("cntr[0]:");
|
|
|
+ MYSERIAL.print(cntr[0], 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ SERIAL_ECHOPGM("cntr[1]:");
|
|
|
+ MYSERIAL.print(cntr[1], 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ }
|
|
|
+#ifdef MESH_BED_CALIBRATION_SHOW_LCD
|
|
|
+ uint8_t next_line;
|
|
|
+ lcd_display_message_fullscreen_P(MSG_FIND_BED_OFFSET_AND_SKEW_LINE1, next_line);
|
|
|
+ if (next_line > 3)
|
|
|
+ next_line = 3;
|
|
|
+#endif
|
|
|
+
|
|
|
+
|
|
|
+ current_position[Z_AXIS] = MESH_HOME_Z_SEARCH + FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP * iteration * 0.3;
|
|
|
+ for (int k = 0; k < 4; ++k) {
|
|
|
+
|
|
|
+ refresh_cmd_timeout();
|
|
|
+#ifdef MESH_BED_CALIBRATION_SHOW_LCD
|
|
|
+ lcd_implementation_print_at(0, next_line, k + 1);
|
|
|
+ lcd_printPGM(MSG_FIND_BED_OFFSET_AND_SKEW_LINE2);
|
|
|
+
|
|
|
+ if (iteration > 0) {
|
|
|
+ lcd_print_at_PGM(0, next_line + 1, MSG_FIND_BED_OFFSET_AND_SKEW_ITERATION);
|
|
|
+ lcd_implementation_print(int(iteration + 1));
|
|
|
+ }
|
|
|
+#endif
|
|
|
+ float *pt = pts + k * 2;
|
|
|
+
|
|
|
+
|
|
|
+ go_to_current(homing_feedrate[Z_AXIS] / 60.f);
|
|
|
+ if (verbosity_level >= 20) {
|
|
|
+
|
|
|
+ current_position[Y_AXIS] = 0.f;
|
|
|
+ go_to_current(homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ SERIAL_ECHOLNPGM("At Y0");
|
|
|
+ delay_keep_alive(5000);
|
|
|
+ current_position[Y_AXIS] = Y_MIN_POS;
|
|
|
+ go_to_current(homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ SERIAL_ECHOLNPGM("At Y-4");
|
|
|
+ delay_keep_alive(5000);
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ current_position[X_AXIS] = pgm_read_float(bed_ref_points_4 + k * 2);
|
|
|
+ current_position[Y_AXIS] = pgm_read_float(bed_ref_points_4 + k * 2 + 1);
|
|
|
+
|
|
|
+ else {
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ current_position[X_AXIS] = vec_x[0] * pgm_read_float(bed_ref_points_4 + k * 2) + vec_y[0] * pgm_read_float(bed_ref_points_4 + k * 2 + 1) + cntr[0];
|
|
|
+ current_position[Y_AXIS] = vec_x[1] * pgm_read_float(bed_ref_points_4 + k * 2) + vec_y[1] * pgm_read_float(bed_ref_points_4 + k * 2 + 1) + cntr[1];
|
|
|
+
|
|
|
+
|
|
|
+ if (current_position[Y_AXIS] < Y_MIN_POS_FOR_BED_CALIBRATION)
|
|
|
+ current_position[Y_AXIS] = Y_MIN_POS_FOR_BED_CALIBRATION;
|
|
|
+
|
|
|
+ }*/
|
|
|
+ if (verbosity_level >= 20) {
|
|
|
+ SERIAL_ECHOPGM("current_position[X_AXIS]:");
|
|
|
+ MYSERIAL.print(current_position[X_AXIS], 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ SERIAL_ECHOPGM("current_position[Y_AXIS]:");
|
|
|
+ MYSERIAL.print(current_position[Y_AXIS], 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ SERIAL_ECHOPGM("current_position[Z_AXIS]:");
|
|
|
+ MYSERIAL.print(current_position[Z_AXIS], 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ go_to_current(homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ if (verbosity_level >= 10)
|
|
|
+ delay_keep_alive(3000);
|
|
|
+ if (!find_bed_induction_sensor_point_xy())
|
|
|
+ return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
|
|
|
+#if 1
|
|
|
+
|
|
|
+ if (k == 0) {
|
|
|
+
|
|
|
+ find_bed_induction_sensor_point_z();
|
|
|
+ int8_t i = 4;
|
|
|
+ for (;;) {
|
|
|
+ if (improve_bed_induction_sensor_point3(verbosity_level))
|
|
|
+ break;
|
|
|
+ if (--i == 0)
|
|
|
+ return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
|
|
|
+
|
|
|
+ current_position[Z_AXIS] -= 0.025f;
|
|
|
+ enable_endstops(false);
|
|
|
+ enable_z_endstop(false);
|
|
|
+ go_to_current(homing_feedrate[Z_AXIS]);
|
|
|
+ }
|
|
|
+ if (i == 0)
|
|
|
+
|
|
|
+ return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
|
|
|
+ }
|
|
|
+#endif
|
|
|
+ if (verbosity_level >= 10)
|
|
|
+ delay_keep_alive(3000);
|
|
|
+
|
|
|
+
|
|
|
+ if (verbosity_level >= 20) {
|
|
|
+ SERIAL_ECHOLNPGM("Measured:");
|
|
|
+ MYSERIAL.println(current_position[X_AXIS]);
|
|
|
+ MYSERIAL.println(current_position[Y_AXIS]);
|
|
|
+ }
|
|
|
+ pt[0] = (pt[0] * iteration) / (iteration + 1);
|
|
|
+ pt[0] += (current_position[X_AXIS]/(iteration + 1));
|
|
|
+ pt[1] = (pt[1] * iteration) / (iteration + 1);
|
|
|
+ pt[1] += (current_position[Y_AXIS] / (iteration + 1));
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if (verbosity_level >= 20) {
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ SERIAL_ECHOPGM("pt[0]:");
|
|
|
+ MYSERIAL.println(pt[0]);
|
|
|
+ SERIAL_ECHOPGM("pt[1]:");
|
|
|
+ MYSERIAL.println(pt[1]);
|
|
|
+ }
|
|
|
+
|
|
|
+ if (current_position[Y_AXIS] < Y_MIN_POS)
|
|
|
+ current_position[Y_AXIS] = Y_MIN_POS;
|
|
|
+
|
|
|
+ current_position[Z_AXIS] += 3.f + FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP * iteration * 0.3;
|
|
|
+
|
|
|
+
|
|
|
+ if (verbosity_level >= 10 && k == 0) {
|
|
|
+
|
|
|
+ current_position[Y_AXIS] = MANUAL_Y_HOME_POS;
|
|
|
+ go_to_current(homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ delay_keep_alive(3000);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if (verbosity_level >= 20) {
|
|
|
+
|
|
|
+ delay_keep_alive(3000);
|
|
|
+ for (int8_t mesh_point = 0; mesh_point < 4; ++mesh_point) {
|
|
|
+
|
|
|
+ refresh_cmd_timeout();
|
|
|
+
|
|
|
+
|
|
|
+ current_position[X_AXIS] = pts[mesh_point * 2];
|
|
|
+ current_position[Y_AXIS] = pts[mesh_point * 2 + 1];
|
|
|
+ go_to_current(homing_feedrate[X_AXIS] / 60);
|
|
|
+ delay_keep_alive(3000);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if (pts[1] < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH) {
|
|
|
+ too_far_mask |= 1 << 1;
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ SERIAL_ECHOPGM("WARNING: Front point not reachable. Y coordinate:");
|
|
|
+ MYSERIAL.print(pts[1]);
|
|
|
+ SERIAL_ECHOPGM(" < ");
|
|
|
+ MYSERIAL.println(Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH);
|
|
|
+ }
|
|
|
+
|
|
|
+ result = calculate_machine_skew_and_offset_LS(pts, 4, bed_ref_points_4, vec_x, vec_y, cntr, verbosity_level);
|
|
|
+ if (result >= 0) {
|
|
|
+ world2machine_update(vec_x, vec_y, cntr);
|
|
|
+#if 1
|
|
|
+
|
|
|
+ eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER + 0), cntr[0]);
|
|
|
+ eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER + 4), cntr[1]);
|
|
|
+ eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X + 0), vec_x[0]);
|
|
|
+ eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X + 4), vec_x[1]);
|
|
|
+ eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y + 0), vec_y[0]);
|
|
|
+ eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y + 4), vec_y[1]);
|
|
|
+#endif
|
|
|
+ if (verbosity_level >= 10) {
|
|
|
+
|
|
|
+ float l = sqrt(vec_x[0] * vec_x[0] + vec_x[1] * vec_x[1]);
|
|
|
+ SERIAL_ECHOLNPGM("X vector length:");
|
|
|
+ MYSERIAL.println(l);
|
|
|
+
|
|
|
+
|
|
|
+ l = sqrt(vec_y[0] * vec_y[0] + vec_y[1] * vec_y[1]);
|
|
|
+ SERIAL_ECHOLNPGM("Y vector length:");
|
|
|
+ MYSERIAL.println(l);
|
|
|
+
|
|
|
+ l = sqrt(cntr[0] * cntr[0] + cntr[1] * cntr[1]);
|
|
|
+ SERIAL_ECHOLNPGM("Zero point correction:");
|
|
|
+ MYSERIAL.println(l);
|
|
|
+
|
|
|
+
|
|
|
+ l = vec_x[0] * vec_y[0] + vec_x[1] * vec_y[1];
|
|
|
+ SERIAL_ECHOLNPGM("Perpendicularity");
|
|
|
+ MYSERIAL.println(fabs(l));
|
|
|
+ SERIAL_ECHOLNPGM("Saving bed calibration vectors to EEPROM");
|
|
|
+ }
|
|
|
+
|
|
|
+ world2machine_update_current();
|
|
|
+
|
|
|
+
|
|
|
+ if (verbosity_level >= 20) {
|
|
|
+
|
|
|
+ delay_keep_alive(3000);
|
|
|
+ for (int8_t mesh_point = 0; mesh_point < 9; ++mesh_point) {
|
|
|
+
|
|
|
+ refresh_cmd_timeout();
|
|
|
+
|
|
|
+
|
|
|
+ current_position[X_AXIS] = pgm_read_float(bed_ref_points + mesh_point * 2);
|
|
|
+ current_position[Y_AXIS] = pgm_read_float(bed_ref_points + mesh_point * 2 + 1);
|
|
|
+ go_to_current(homing_feedrate[X_AXIS] / 60);
|
|
|
+ delay_keep_alive(3000);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return result;
|
|
|
+ }
|
|
|
+ if (result == BED_SKEW_OFFSET_DETECTION_FITTING_FAILED && too_far_mask == 2) return result;
|
|
|
+ iteration++;
|
|
|
+ }
|
|
|
+ return result;
|
|
|
+}
|
|
|
+
|
|
|
+BedSkewOffsetDetectionResultType improve_bed_offset_and_skew(int8_t method, int8_t verbosity_level, uint8_t &too_far_mask)
|
|
|
+{
|
|
|
+
|
|
|
+ refresh_cmd_timeout();
|
|
|
+
|
|
|
+
|
|
|
+ too_far_mask = 0;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ float *pts = &mbl.z_values[0][0];
|
|
|
+ float *vec_x = pts + 2 * 9;
|
|
|
+ float *vec_y = vec_x + 2;
|
|
|
+ float *cntr = vec_y + 2;
|
|
|
+ memset(pts, 0, sizeof(float) * 7 * 7);
|
|
|
+
|
|
|
+
|
|
|
+ world2machine_initialize();
|
|
|
+ vec_x[0] = world2machine_rotation_and_skew[0][0];
|
|
|
+ vec_x[1] = world2machine_rotation_and_skew[1][0];
|
|
|
+ vec_y[0] = world2machine_rotation_and_skew[0][1];
|
|
|
+ vec_y[1] = world2machine_rotation_and_skew[1][1];
|
|
|
+ cntr[0] = world2machine_shift[0];
|
|
|
+ cntr[1] = world2machine_shift[1];
|
|
|
+
|
|
|
+ world2machine_reset();
|
|
|
+
|
|
|
+ bool endstops_enabled = enable_endstops(false);
|
|
|
+ bool endstop_z_enabled = enable_z_endstop(false);
|
|
|
+
|
|
|
+#ifdef MESH_BED_CALIBRATION_SHOW_LCD
|
|
|
+ uint8_t next_line;
|
|
|
+ lcd_display_message_fullscreen_P(MSG_IMPROVE_BED_OFFSET_AND_SKEW_LINE1, next_line);
|
|
|
+ if (next_line > 3)
|
|
|
+ next_line = 3;
|
|
|
+#endif
|
|
|
+
|
|
|
+
|
|
|
+ BedSkewOffsetDetectionResultType result = BED_SKEW_OFFSET_DETECTION_PERFECT;
|
|
|
+ for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
|
|
|
+
|
|
|
+ refresh_cmd_timeout();
|
|
|
+
|
|
|
+#ifdef MESH_BED_CALIBRATION_SHOW_LCD
|
|
|
+ lcd_implementation_print_at(0, next_line, mesh_point+1);
|
|
|
+ lcd_printPGM(MSG_IMPROVE_BED_OFFSET_AND_SKEW_LINE2);
|
|
|
+#endif
|
|
|
+
|
|
|
+
|
|
|
+ current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
|
|
|
+ enable_endstops(false);
|
|
|
+ enable_z_endstop(false);
|
|
|
+ go_to_current(homing_feedrate[Z_AXIS]/60);
|
|
|
+ if (verbosity_level >= 20) {
|
|
|
+
|
|
|
+ current_position[Y_AXIS] = 0.f;
|
|
|
+ go_to_current(homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ SERIAL_ECHOLNPGM("At Y0");
|
|
|
+ delay_keep_alive(5000);
|
|
|
+ current_position[Y_AXIS] = Y_MIN_POS;
|
|
|
+ go_to_current(homing_feedrate[X_AXIS] / 60.f);
|
|
|
+ SERIAL_ECHOLNPGM("At Y-4");
|
|
|
+ delay_keep_alive(5000);
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ current_position[X_AXIS] = vec_x[0] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[0] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[0];
|
|
|
+ current_position[Y_AXIS] = vec_x[1] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[1] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[1];
|
|
|
+
|
|
|
+ if (current_position[Y_AXIS] < Y_MIN_POS_FOR_BED_CALIBRATION)
|
|
|
+ current_position[Y_AXIS] = Y_MIN_POS_FOR_BED_CALIBRATION;
|
|
|
+ go_to_current(homing_feedrate[X_AXIS]/60);
|
|
|
+
|
|
|
+ if (verbosity_level >= 10)
|
|
|
+ delay_keep_alive(3000);
|
|
|
+ find_bed_induction_sensor_point_z();
|
|
|
+ if (verbosity_level >= 10)
|
|
|
+ delay_keep_alive(3000);
|
|
|
+
|
|
|
+ current_position[Z_AXIS] -= 0.025f;
|
|
|
+
|
|
|
+ int8_t n_errors = 3;
|
|
|
+ for (int8_t iter = 0; iter < 8; ) {
|
|
|
+ if (verbosity_level > 20) {
|
|
|
+ SERIAL_ECHOPGM("Improving bed point ");
|
|
|
+ SERIAL_ECHO(mesh_point);
|
|
|
+ SERIAL_ECHOPGM(", iteration ");
|
|
|
+ SERIAL_ECHO(iter);
|
|
|
+ SERIAL_ECHOPGM(", z");
|
|
|
+ MYSERIAL.print(current_position[Z_AXIS], 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ }
|
|
|
+ bool found = false;
|
|
|
+ if (mesh_point < 3) {
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ found = improve_bed_induction_sensor_point3(verbosity_level);
|
|
|
+ } else {
|
|
|
+ switch (method) {
|
|
|
+ case 0: found = improve_bed_induction_sensor_point(); break;
|
|
|
+ case 1: found = improve_bed_induction_sensor_point2(mesh_point < 3, verbosity_level); break;
|
|
|
+ default: break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if (found) {
|
|
|
+ if (iter > 3) {
|
|
|
+
|
|
|
+ pts[mesh_point*2 ] += current_position[X_AXIS];
|
|
|
+ pts[mesh_point*2+1] += current_position[Y_AXIS];
|
|
|
+ }
|
|
|
+ if (current_position[Y_AXIS] < Y_MIN_POS)
|
|
|
+ current_position[Y_AXIS] = Y_MIN_POS;
|
|
|
+ ++ iter;
|
|
|
+ } else if (n_errors -- == 0) {
|
|
|
+
|
|
|
+ result = BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
|
|
|
+ goto canceled;
|
|
|
+ } else {
|
|
|
+
|
|
|
+ current_position[Z_AXIS] -= 0.05f;
|
|
|
+ enable_endstops(false);
|
|
|
+ enable_z_endstop(false);
|
|
|
+ go_to_current(homing_feedrate[Z_AXIS]);
|
|
|
+ if (verbosity_level >= 5) {
|
|
|
+ SERIAL_ECHOPGM("Improving bed point ");
|
|
|
+ SERIAL_ECHO(mesh_point);
|
|
|
+ SERIAL_ECHOPGM(", iteration ");
|
|
|
+ SERIAL_ECHO(iter);
|
|
|
+ SERIAL_ECHOPGM(" failed. Lowering z to ");
|
|
|
+ MYSERIAL.print(current_position[Z_AXIS], 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if (verbosity_level >= 10)
|
|
|
+ delay_keep_alive(3000);
|
|
|
+ }
|
|
|
+
|
|
|
+ refresh_cmd_timeout();
|
|
|
+
|
|
|
+
|
|
|
+ for (int8_t i = 0; i < 18; ++ i)
|
|
|
+ pts[i] *= (1.f/4.f);
|
|
|
+
|
|
|
+ enable_endstops(false);
|
|
|
+ enable_z_endstop(false);
|
|
|
+
|
|
|
+ if (verbosity_level >= 5) {
|
|
|
+
|
|
|
+ current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
|
|
|
+ for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
|
|
|
+
|
|
|
+ refresh_cmd_timeout();
|
|
|
+
|
|
|
+
|
|
|
+ current_position[X_AXIS] = pts[mesh_point*2];
|
|
|
+ current_position[Y_AXIS] = pts[mesh_point*2+1];
|
|
|
+ if (verbosity_level >= 10) {
|
|
|
+ go_to_current(homing_feedrate[X_AXIS]/60);
|
|
|
+ delay_keep_alive(3000);
|
|
|
+ }
|
|
|
+ SERIAL_ECHOPGM("Final measured bed point ");
|
|
|
+ SERIAL_ECHO(mesh_point);
|
|
|
+ SERIAL_ECHOPGM(": ");
|
|
|
+ MYSERIAL.print(current_position[X_AXIS], 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(current_position[Y_AXIS], 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ {
|
|
|
+
|
|
|
+ for (uint8_t mesh_point = 0; mesh_point < 3; ++ mesh_point)
|
|
|
+ if (pts[mesh_point * 2 + 1] < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH)
|
|
|
+ too_far_mask |= 1 << mesh_point;
|
|
|
+ result = calculate_machine_skew_and_offset_LS(pts, 9, bed_ref_points, vec_x, vec_y, cntr, verbosity_level);
|
|
|
+ if (result < 0) {
|
|
|
+ SERIAL_ECHOLNPGM("Calculation of the machine skew and offset failed.");
|
|
|
+ goto canceled;
|
|
|
+ }
|
|
|
+
|
|
|
+ for (uint8_t mesh_point = 0; mesh_point < 3; ++ mesh_point) {
|
|
|
+ float y = vec_x[1] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[1] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[1];
|
|
|
+ if (y < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH)
|
|
|
+ too_far_mask |= 1 << mesh_point;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ world2machine_update(vec_x, vec_y, cntr);
|
|
|
+#if 1
|
|
|
+
|
|
|
+ eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER+0), cntr [0]);
|
|
|
+ eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER+4), cntr [1]);
|
|
|
+ eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +0), vec_x[0]);
|
|
|
+ eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +4), vec_x[1]);
|
|
|
+ eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +0), vec_y[0]);
|
|
|
+ eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +4), vec_y[1]);
|
|
|
+#endif
|
|
|
+
|
|
|
+
|
|
|
+ world2machine_update_current();
|
|
|
+
|
|
|
+ enable_endstops(false);
|
|
|
+ enable_z_endstop(false);
|
|
|
+
|
|
|
+ if (verbosity_level >= 5) {
|
|
|
+
|
|
|
+ delay_keep_alive(3000);
|
|
|
+ current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
|
|
|
+ for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
|
|
|
+
|
|
|
+ refresh_cmd_timeout();
|
|
|
+
|
|
|
+
|
|
|
+ current_position[X_AXIS] = pgm_read_float(bed_ref_points+mesh_point*2);
|
|
|
+ current_position[Y_AXIS] = pgm_read_float(bed_ref_points+mesh_point*2+1);
|
|
|
+ if (verbosity_level >= 10) {
|
|
|
+ go_to_current(homing_feedrate[X_AXIS]/60);
|
|
|
+ delay_keep_alive(3000);
|
|
|
+ }
|
|
|
+ {
|
|
|
+ float x, y;
|
|
|
+ world2machine(current_position[X_AXIS], current_position[Y_AXIS], x, y);
|
|
|
+ SERIAL_ECHOPGM("Final calculated bed point ");
|
|
|
+ SERIAL_ECHO(mesh_point);
|
|
|
+ SERIAL_ECHOPGM(": ");
|
|
|
+ MYSERIAL.print(x, 5);
|
|
|
+ SERIAL_ECHOPGM(", ");
|
|
|
+ MYSERIAL.print(y, 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if (! sample_mesh_and_store_reference())
|
|
|
+ goto canceled;
|
|
|
+
|
|
|
+ enable_endstops(endstops_enabled);
|
|
|
+ enable_z_endstop(endstop_z_enabled);
|
|
|
+
|
|
|
+ refresh_cmd_timeout();
|
|
|
+ return result;
|
|
|
+
|
|
|
+canceled:
|
|
|
+
|
|
|
+ refresh_cmd_timeout();
|
|
|
+
|
|
|
+ current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
|
|
|
+ go_to_current(homing_feedrate[Z_AXIS]/60);
|
|
|
+
|
|
|
+ reset_bed_offset_and_skew();
|
|
|
+ enable_endstops(endstops_enabled);
|
|
|
+ enable_z_endstop(endstop_z_enabled);
|
|
|
+ return result;
|
|
|
+}
|
|
|
+
|
|
|
+void go_home_with_z_lift()
|
|
|
+{
|
|
|
+
|
|
|
+ refresh_cmd_timeout();
|
|
|
+
|
|
|
+
|
|
|
+ current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
|
|
|
+ go_to_current(homing_feedrate[Z_AXIS]/60);
|
|
|
+
|
|
|
+ current_position[X_AXIS] = X_MIN_POS+0.2;
|
|
|
+ current_position[Y_AXIS] = Y_MIN_POS+0.2;
|
|
|
+
|
|
|
+ world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
|
|
|
+ go_to_current(homing_feedrate[X_AXIS]/60);
|
|
|
+
|
|
|
+ current_position[Z_AXIS] = Z_MIN_POS;
|
|
|
+ go_to_current(homing_feedrate[Z_AXIS]/60);
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+bool sample_mesh_and_store_reference()
|
|
|
+{
|
|
|
+ bool endstops_enabled = enable_endstops(false);
|
|
|
+ bool endstop_z_enabled = enable_z_endstop(false);
|
|
|
+
|
|
|
+
|
|
|
+ refresh_cmd_timeout();
|
|
|
+
|
|
|
+#ifdef MESH_BED_CALIBRATION_SHOW_LCD
|
|
|
+ uint8_t next_line;
|
|
|
+ lcd_display_message_fullscreen_P(MSG_MEASURE_BED_REFERENCE_HEIGHT_LINE1, next_line);
|
|
|
+ if (next_line > 3)
|
|
|
+ next_line = 3;
|
|
|
+
|
|
|
+ lcd_implementation_print_at(0, next_line, 1);
|
|
|
+ lcd_printPGM(MSG_MEASURE_BED_REFERENCE_HEIGHT_LINE2);
|
|
|
+#endif
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ {
|
|
|
+
|
|
|
+ current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
|
|
|
+ go_to_current(homing_feedrate[Z_AXIS]/60);
|
|
|
+ current_position[X_AXIS] = pgm_read_float(bed_ref_points);
|
|
|
+ current_position[Y_AXIS] = pgm_read_float(bed_ref_points+1);
|
|
|
+ world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
|
|
|
+ go_to_current(homing_feedrate[X_AXIS]/60);
|
|
|
+ memcpy(destination, current_position, sizeof(destination));
|
|
|
+ enable_endstops(true);
|
|
|
+ homeaxis(Z_AXIS);
|
|
|
+ enable_endstops(false);
|
|
|
+ find_bed_induction_sensor_point_z();
|
|
|
+ mbl.set_z(0, 0, current_position[Z_AXIS]);
|
|
|
+ }
|
|
|
+ for (int8_t mesh_point = 1; mesh_point != MESH_MEAS_NUM_X_POINTS * MESH_MEAS_NUM_Y_POINTS; ++ mesh_point) {
|
|
|
+
|
|
|
+ refresh_cmd_timeout();
|
|
|
+
|
|
|
+ current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
|
|
|
+ go_to_current(homing_feedrate[Z_AXIS]/60);
|
|
|
+ current_position[X_AXIS] = pgm_read_float(bed_ref_points+2*mesh_point);
|
|
|
+ current_position[Y_AXIS] = pgm_read_float(bed_ref_points+2*mesh_point+1);
|
|
|
+ world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
|
|
|
+ go_to_current(homing_feedrate[X_AXIS]/60);
|
|
|
+#ifdef MESH_BED_CALIBRATION_SHOW_LCD
|
|
|
+
|
|
|
+ lcd_implementation_print_at(0, next_line, mesh_point+1);
|
|
|
+ lcd_printPGM(MSG_MEASURE_BED_REFERENCE_HEIGHT_LINE2);
|
|
|
+#endif
|
|
|
+ find_bed_induction_sensor_point_z();
|
|
|
+
|
|
|
+ int8_t ix = mesh_point % MESH_MEAS_NUM_X_POINTS;
|
|
|
+ int8_t iy = mesh_point / MESH_MEAS_NUM_X_POINTS;
|
|
|
+ if (iy & 1) ix = (MESH_MEAS_NUM_X_POINTS - 1) - ix;
|
|
|
+ mbl.set_z(ix, iy, current_position[Z_AXIS]);
|
|
|
+ }
|
|
|
+ {
|
|
|
+
|
|
|
+ float zmin = mbl.z_values[0][0];
|
|
|
+ float zmax = zmax;
|
|
|
+ for (int8_t j = 0; j < 3; ++ j)
|
|
|
+ for (int8_t i = 0; i < 3; ++ i) {
|
|
|
+ zmin = min(zmin, mbl.z_values[j][i]);
|
|
|
+ zmax = min(zmax, mbl.z_values[j][i]);
|
|
|
+ }
|
|
|
+ if (zmax - zmin > 3.f) {
|
|
|
+
|
|
|
+
|
|
|
+ SERIAL_PROTOCOLLNPGM("Exreme span of the Z values!");
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ {
|
|
|
+ uint16_t addr = EEPROM_BED_CALIBRATION_Z_JITTER;
|
|
|
+ for (int8_t j = 0; j < 3; ++ j)
|
|
|
+ for (int8_t i = 0; i < 3; ++ i) {
|
|
|
+ if (i == 0 && j == 0)
|
|
|
+ continue;
|
|
|
+ float dif = mbl.z_values[j][i] - mbl.z_values[0][0];
|
|
|
+ int16_t dif_quantized = int16_t(floor(dif * 100.f + 0.5f));
|
|
|
+ eeprom_update_word((uint16_t*)addr, *reinterpret_cast<uint16_t*>(&dif_quantized));
|
|
|
+ #if 0
|
|
|
+ {
|
|
|
+ uint16_t z_offset_u = eeprom_read_word((uint16_t*)addr);
|
|
|
+ float dif2 = *reinterpret_cast<int16_t*>(&z_offset_u) * 0.01;
|
|
|
+
|
|
|
+ SERIAL_ECHOPGM("Bed point ");
|
|
|
+ SERIAL_ECHO(i);
|
|
|
+ SERIAL_ECHOPGM(",");
|
|
|
+ SERIAL_ECHO(j);
|
|
|
+ SERIAL_ECHOPGM(", differences: written ");
|
|
|
+ MYSERIAL.print(dif, 5);
|
|
|
+ SERIAL_ECHOPGM(", read: ");
|
|
|
+ MYSERIAL.print(dif2, 5);
|
|
|
+ SERIAL_ECHOLNPGM("");
|
|
|
+ }
|
|
|
+ #endif
|
|
|
+ addr += 2;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ mbl.upsample_3x3();
|
|
|
+ mbl.active = true;
|
|
|
+
|
|
|
+ go_home_with_z_lift();
|
|
|
+
|
|
|
+ enable_endstops(endstops_enabled);
|
|
|
+ enable_z_endstop(endstop_z_enabled);
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
+bool scan_bed_induction_points(int8_t verbosity_level)
|
|
|
+{
|
|
|
+
|
|
|
+ refresh_cmd_timeout();
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ float *pts = &mbl.z_values[0][0];
|
|
|
+ float *vec_x = pts + 2 * 9;
|
|
|
+ float *vec_y = vec_x + 2;
|
|
|
+ float *cntr = vec_y + 2;
|
|
|
+ memset(pts, 0, sizeof(float) * 7 * 7);
|
|
|
+
|
|
|
+
|
|
|
+ world2machine_initialize();
|
|
|
+ vec_x[0] = world2machine_rotation_and_skew[0][0];
|
|
|
+ vec_x[1] = world2machine_rotation_and_skew[1][0];
|
|
|
+ vec_y[0] = world2machine_rotation_and_skew[0][1];
|
|
|
+ vec_y[1] = world2machine_rotation_and_skew[1][1];
|
|
|
+ cntr[0] = world2machine_shift[0];
|
|
|
+ cntr[1] = world2machine_shift[1];
|
|
|
+
|
|
|
+ world2machine_reset();
|
|
|
+
|
|
|
+ bool endstops_enabled = enable_endstops(false);
|
|
|
+ bool endstop_z_enabled = enable_z_endstop(false);
|
|
|
+
|
|
|
+
|
|
|
+ for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
|
|
|
+
|
|
|
+ refresh_cmd_timeout();
|
|
|
+
|
|
|
+
|
|
|
+ current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
|
|
|
+ enable_endstops(false);
|
|
|
+ enable_z_endstop(false);
|
|
|
+ go_to_current(homing_feedrate[Z_AXIS]/60);
|
|
|
+
|
|
|
+
|
|
|
+ current_position[X_AXIS] = vec_x[0] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[0] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[0];
|
|
|
+ current_position[Y_AXIS] = vec_x[1] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[1] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[1];
|
|
|
+
|
|
|
+ if (current_position[Y_AXIS] < Y_MIN_POS_FOR_BED_CALIBRATION)
|
|
|
+ current_position[Y_AXIS] = Y_MIN_POS_FOR_BED_CALIBRATION;
|
|
|
+ go_to_current(homing_feedrate[X_AXIS]/60);
|
|
|
+ find_bed_induction_sensor_point_z();
|
|
|
+ scan_bed_induction_sensor_point();
|
|
|
+ }
|
|
|
+
|
|
|
+ refresh_cmd_timeout();
|
|
|
+
|
|
|
+ enable_endstops(false);
|
|
|
+ enable_z_endstop(false);
|
|
|
+
|
|
|
+
|
|
|
+ refresh_cmd_timeout();
|
|
|
+
|
|
|
+ enable_endstops(endstops_enabled);
|
|
|
+ enable_z_endstop(endstop_z_enabled);
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+static void shift_z(float delta)
|
|
|
+{
|
|
|
+ plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] - delta, current_position[E_AXIS], homing_feedrate[Z_AXIS]/40, active_extruder);
|
|
|
+ st_synchronize();
|
|
|
+ plan_set_z_position(current_position[Z_AXIS]);
|
|
|
+}
|
|
|
+
|
|
|
+#define BABYSTEP_LOADZ_BY_PLANNER
|
|
|
+
|
|
|
+
|
|
|
+static int babystepLoadZ = 0;
|
|
|
+
|
|
|
+void babystep_apply()
|
|
|
+{
|
|
|
+
|
|
|
+ if(calibration_status() < CALIBRATION_STATUS_LIVE_ADJUST)
|
|
|
+ {
|
|
|
+ check_babystep();
|
|
|
+
|
|
|
+
|
|
|
+ EEPROM_read_B(EEPROM_BABYSTEP_Z,&babystepLoadZ);
|
|
|
+
|
|
|
+ #if 0
|
|
|
+ SERIAL_ECHO("Z baby step: ");
|
|
|
+ SERIAL_ECHO(babystepLoadZ);
|
|
|
+ SERIAL_ECHO(", current Z: ");
|
|
|
+ SERIAL_ECHO(current_position[Z_AXIS]);
|
|
|
+ SERIAL_ECHO("correction: ");
|
|
|
+ SERIAL_ECHO(float(babystepLoadZ) / float(axis_steps_per_unit[Z_AXIS]));
|
|
|
+ SERIAL_ECHOLN("");
|
|
|
+ #endif
|
|
|
+ #ifdef BABYSTEP_LOADZ_BY_PLANNER
|
|
|
+ shift_z(- float(babystepLoadZ) / float(axis_steps_per_unit[Z_AXIS]));
|
|
|
+ #else
|
|
|
+ babystepsTodoZadd(babystepLoadZ);
|
|
|
+ #endif
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void babystep_undo()
|
|
|
+{
|
|
|
+#ifdef BABYSTEP_LOADZ_BY_PLANNER
|
|
|
+ shift_z(float(babystepLoadZ) / float(axis_steps_per_unit[Z_AXIS]));
|
|
|
+#else
|
|
|
+ babystepsTodoZsubtract(babystepLoadZ);
|
|
|
+#endif
|
|
|
+ babystepLoadZ = 0;
|
|
|
+}
|
|
|
+
|
|
|
+void babystep_reset()
|
|
|
+{
|
|
|
+ babystepLoadZ = 0;
|
|
|
}
|
PavelSindler