#include "Marlin.h" #include "Configuration.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]; // Weight of the Y coordinate for the least squares fitting of the bed induction sensor targets. // Only used for the first row of the points, which may not befully in reach of the sensor. #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) // Scaling of the real machine axes against the programmed dimensions in the firmware. // The correction is tiny, here around 0.5mm on 250mm length. //#define MACHINE_AXIS_SCALE_X ((250.f - 0.5f) / 250.f) //#define MACHINE_AXIS_SCALE_Y ((250.f - 0.5f) / 250.f) #define MACHINE_AXIS_SCALE_X 1.f #define MACHINE_AXIS_SCALE_Y 1.f // 0.12 degrees equals to an offset of 0.5mm on 250mm length. #define BED_SKEW_ANGLE_MILD (0.12f * M_PI / 180.f) // 0.25 degrees equals to an offset of 1.1mm on 250mm length. #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 (MANUAL_Y_HOME_POS-0.2f) #define Y_MIN_POS_FOR_BED_CALIBRATION (Y_MIN_POS) // Distances toward the print bed edge may not be accurate. #define Y_MIN_POS_CALIBRATION_POINT_ACCURATE (Y_MIN_POS + 3.f) // When the measured point center is out of reach of the sensor, Y coordinate will be ignored // by the Least Squares fitting and the X coordinate will be weighted low. #define Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH (Y_MIN_POS - 0.5f) // Positions of the bed reference points in the machine coordinates, referenced to the P.I.N.D.A sensor. // The points are ordered in a zig-zag fashion to speed up the calibration. 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 }; // Positions of the bed reference points in the machine coordinates, referenced to the P.I.N.D.A sensor. // The points are the following: center front, center right, center rear, center left. 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; } // Weight of a point coordinate in a least squares optimization. // The first row of points may not be fully reachable // and the y values may be shortened a bit by the bed carriage // pulling the belt up. static inline float point_weight_x(const uint8_t i, const float &y) { float w = 1.f; if (i < 3) { 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) { // If the point is fully outside, give it some weight. w = WEIGHT_FIRST_ROW_X_LOW; } else { // Linearly interpolate the weight from 1 to WEIGHT_FIRST_ROW_X. 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; } // Weight of a point coordinate in a least squares optimization. // The first row of points may not be fully reachable // and the y values may be shortened a bit by the bed carriage // pulling the belt up. static inline float point_weight_y(const uint8_t i, const float &y) { float w = 1.f; if (i < 3) { 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) { // If the point is fully outside, give it some weight. w = WEIGHT_FIRST_ROW_Y_LOW; } else { // Linearly interpolate the weight from 1 to WEIGHT_FIRST_ROW_X. 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; } // Non-Linear Least Squares fitting of the bed to the measured induction points // using the Gauss-Newton method. // This method will maintain a unity length of the machine axes, // which is the correct approach if the sensor points are not measured precisely. BedSkewOffsetDetectionResultType calculate_machine_skew_and_offset_LS( // Matrix of maximum 9 2D points (18 floats) const float *measured_pts, uint8_t npts, const float *true_pts, // Resulting correction matrix. float *vec_x, float *vec_y, float *cntr, // Temporary values, 49-18-(2*3)=25 floats // , float *temp int8_t verbosity_level ) { if (verbosity_level >= 10) { // Show the initial state, before the fitting. 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); } // Run some iterations of the Gauss-Newton method of non-linear least squares. // Initial set of parameters: // X,Y offset cntr[0] = 0.f; cntr[1] = 0.f; // Rotation of the machine X axis from the bed X axis. float a1 = 0; // Rotation of the machine Y axis from the bed Y axis. 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; // Prepare the Normal equation for the Gauss-Newton method. 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; // J^T times J for (uint8_t i = 0; i < npts; ++i) { // First for the residuum in the x axis: 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, measured_pts[2 * i + 1]); acc += a * b * w; } // Second for the residuum in the y axis. // The first row of the points have a low weight, because their position may not be known // with a sufficient accuracy. 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, measured_pts[2 * i + 1]); acc += a * b * w; } } A[r][c] = acc; } // J^T times f(x) 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, 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, measured_pts[2 * i + 1]); acc += j * fy * w; } } b[r] = -acc; } // Solve for h by a Gauss iteration method. 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]; } // and update the current position with h. // It may be better to use the Levenberg-Marquart method here, // but because we are very close to the solution alread, // the simple Gauss-Newton non-linear Least Squares method works well enough. cntr[0] += h[0]; cntr[1] += h[1]; a1 += h[2]; a2 += h[3]; if (verbosity_level >= 20) { SERIAL_ECHOPGM("iteration: "); MYSERIAL.print(iter, 0); 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) { // Show the adjusted state, before the fitting. 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: "); } // Measure the 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 (i < 3) { float w = point_weight_y(i, 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; } else { if (err > BED_CALIBRATION_POINT_OFFSET_MAX_EUCLIDIAN) result = BED_SKEW_OFFSET_DETECTION_FITTING_FAILED; } if (verbosity_level >= 10) { 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("); 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(err); 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."); // Just disable the 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."); // Orthogonalize 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; // Refresh the offset. cntr[0] = 0.f; cntr[1] = 0.f; float wx = 0.f; float wy = 0.f; for (int8_t i = 0; i < 9; ++ 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, y); cntr[0] += w * (pgm_read_float(true_pts + i * 2) - x); wx += w; w = point_weight_y(i, y); cntr[1] += w * (pgm_read_float(true_pts + i * 2 + 1) - y); wy += w; } cntr[0] /= wx; cntr[1] /= wy; } #endif // Invert the transformation matrix made of vec_x, vec_y and cntr. { 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) { // Show the adjusted state, before the fitting. 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(""); } 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); // Reset the 8 16bit offsets. 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]; // No correction. world2machine_correction_mode = WORLD2MACHINE_CORRECTION_NONE; if (world2machine_shift[0] != 0.f || world2machine_shift[1] != 0.f) // Shift correction. 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) { // Rotation & skew correction. world2machine_correction_mode |= WORLD2MACHINE_CORRECTION_SKEW; // Invert the world2machine matrix. 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) { // Reset the machine correction matrix. 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); // Wait for the motors to stop and update the current position with the absolute values. 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 { // Length of the vec_x shall be close to unity. float l = sqrt(vec_x[0] * vec_x[0] + vec_x[1] * vec_x[1]); if (l < 0.9 || l > 1.1) { SERIAL_ECHOLNPGM("Invalid bed correction matrix. Length of the X vector out of range."); reset = true; } // Length of the vec_y shall be close to unity. l = sqrt(vec_y[0] * vec_y[0] + vec_y[1] * vec_y[1]); if (l < 0.9 || l > 1.1) { SERIAL_ECHOLNPGM("Invalid bed correction matrix. Length of the X vector out of range."); reset = true; } // Correction of the zero point shall be reasonably small. l = sqrt(cntr[0] * cntr[0] + cntr[1] * cntr[1]); if (l > 15.f) { SERIAL_ECHOLNPGM("Invalid bed correction matrix. Shift out of range."); reset = true; } // vec_x and vec_y shall be nearly perpendicular. 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(""); */ } } // When switching from absolute to corrected coordinates, // this will get the absolute coordinates from the servos, // applies the inverse world2machine transformation // and stores the result into current_position[x,y]. 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]); } // At the current position, find the Z stop. inline bool find_bed_induction_sensor_point_z(float minimum_z, uint8_t n_iter) { // SERIAL_ECHOLNPGM("find_bed_induction_sensor_point_z 1"); bool endstops_enabled = enable_endstops(true); bool endstop_z_enabled = enable_z_endstop(false); float z = 0.f; endstop_z_hit_on_purpose(); // move down until you find the bed current_position[Z_AXIS] = minimum_z; go_to_current(homing_feedrate[Z_AXIS]/60); // we have to let the planner know where we are right now as it is not where we said to go. update_current_position_z(); if (! endstop_z_hit_on_purpose()) goto error; for (uint8_t i = 0; i < n_iter; ++ i) { // Move up the retract distance. current_position[Z_AXIS] += .5f; go_to_current(homing_feedrate[Z_AXIS]/60); // Move back down slowly to find bed. current_position[Z_AXIS] = minimum_z; go_to_current(homing_feedrate[Z_AXIS]/(4*60)); // we have to let the planner know where we are right now as it is not where we said to go. update_current_position_z(); if (! endstop_z_hit_on_purpose()) goto error; // SERIAL_ECHOPGM("Bed find_bed_induction_sensor_point_z low, height: "); // MYSERIAL.print(current_position[Z_AXIS], 5); // SERIAL_ECHOLNPGM(""); 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); // SERIAL_ECHOLNPGM("find_bed_induction_sensor_point_z 3"); return true; error: // SERIAL_ECHOLNPGM("find_bed_induction_sensor_point_z 4"); enable_endstops(endstops_enabled); enable_z_endstop(endstop_z_enabled); return false; } // Search around the current_position[X,Y], // look for the induction sensor response. // Adjust the current_position[X,Y,Z] to the center of the target dot and its response Z coordinate. #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() { 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(current_position[X_AXIS], current_position[Y_AXIS], MESH_HOME_Z_SEARCH, homing_feedrate[Z_AXIS]/60); go_xyz(x0, y0, current_position[Z_AXIS], feedrate); // Continously lower the Z axis. endstops_hit_on_purpose(); enable_z_endstop(true); while (current_position[Z_AXIS] > -10.f) { // Do nsteps_y zig-zag movements. current_position[Y_AXIS] = y0; for (i = 0; i < nsteps_y; current_position[Y_AXIS] += (y1 - y0) / float(nsteps_y - 1), ++ i) { // Run with a slightly decreasing Z axis, zig-zag movement. Stop at the Z end-stop. 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) { // Run with a slightly decreasing Z axis, zig-zag movement. Stop at the Z end-stop. 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: // SERIAL_ECHOLN("First hit"); // we have to let the planner know where we are right now as it is not where we said to go. update_current_position_xyz(); // Search in this plane for the first hit. Zig-zag first in X, then in Y axis. for (int8_t iter = 0; iter < 3; ++ iter) { if (iter > 0) { // Slightly lower the Z axis to get a reliable trigger. 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); } // Do nsteps_y zig-zag movements. 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) { // SERIAL_ECHOLN("Search in Y - not found"); continue; } // SERIAL_ECHOLN("Search in Y - found"); 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) { // SERIAL_ECHOLN("Search in Y2 - not found"); continue; } // SERIAL_ECHOLN("Search in Y2 - found"); b = current_position[Y_AXIS]; current_position[Y_AXIS] = 0.5f * (a + b); // Search in the X direction along a cross. 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()) { // SERIAL_ECHOLN("Search X span 0 - not found"); continue; } // SERIAL_ECHOLN("Search X span 0 - found"); 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()) { // SERIAL_ECHOLN("Search X span 1 - not found"); continue; } // SERIAL_ECHOLN("Search X span 1 - found"); b = current_position[X_AXIS]; // Go to the center. 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 // Search in the Y direction along a cross. 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()) { // SERIAL_ECHOLN("Search Y2 span 0 - not found"); continue; } // SERIAL_ECHOLN("Search Y2 span 0 - found"); 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()) { // SERIAL_ECHOLN("Search Y2 span 1 - not found"); continue; } // SERIAL_ECHOLN("Search Y2 span 1 - found"); b = current_position[Y_AXIS]; // Go to the center. 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; } // Search around the current_position[X,Y,Z]. // It is expected, that the induction sensor is switched on at the current position. // Look around this center point by painting a star around the point. 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; } // Trim the vector from center_old_[x,y] to destination[x,y] by the bed dimensions. 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) { // Exiting the bed at xmin. 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) { // Exiting the bed at xmax. 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) { // Exiting the bed at ymin. 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) { // Exiting the bed at xmax. t = (Y_MAX_POS - center_y) / l; destination[X_AXIS] = center_old_x + t * vx; destination[Y_AXIS] = Y_MAX_POS; } // Move away from the measurement point. enable_endstops(false); go_xy(destination[X_AXIS], destination[Y_AXIS], feedrate); // Move towards the measurement point, until the induction sensor triggers. enable_endstops(true); go_xy(center_old_x, center_old_y, feedrate); update_current_position_xyz(); // if (! endstop_z_hit_on_purpose()) return false; center_x += current_position[X_AXIS]; center_y += current_position[Y_AXIS]; } // Calculate the new center, move to the new center. 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(""); } // Search around the current_position[X,Y,Z]. // It is expected, that the induction sensor is switched on at the current position. // Look around this center point by painting a star around the point. #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; // Search in the X direction along a cross. 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(""); } // We force the calibration routine to move the Z axis slightly down to make the response more pronounced. if (b - a < 0.5f * MIN_BED_SENSOR_POINT_RESPONSE_DMR) { // Don't use the new X value. current_position[X_AXIS] = center_old_x; goto canceled; } else { // Use the new value, but force the Z axis to go a bit lower. 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]); } // Go to the center. 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; // Search in the Y direction along a cross. enable_z_endstop(false); go_xy(current_position[X_AXIS], y0, homing_feedrate[X_AXIS] / 60.f); if (lift_z_on_min_y) { // The first row of points are very close to the end stop. // Lift the sensor to disengage the trigger. This is necessary because of the sensor hysteresis. go_xyz(current_position[X_AXIS], y0, current_position[Z_AXIS]+1.5f, homing_feedrate[Z_AXIS] / 60.f); // and go back. 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) { // Already triggering before we started the move. // Shift the trigger point slightly outwards. // a = current_position[Y_AXIS] - 1.5f; 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) { // We force the calibration routine to move the Z axis slightly down to make the response more pronounced. 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) { // Don't use the new Y value. current_position[Y_AXIS] = center_old_y; goto canceled; } else { // Use the new value, but force the Z axis to go a bit lower. 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]); } // Go to the center. 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); } // If point is small but not too small, then force the Z axis to be lowered a bit, // but use the new value. This is important when the initial position was off in one axis, // for example if the initial calibration was shifted in the Y axis systematically. // Then this first step will center. return ! point_small; canceled: // Go back to the center. enable_z_endstop(false); go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f); return false; } // Searching the front points, where one cannot move the sensor head in front of the sensor point. // Searching in a zig-zag movement in a plane for the maximum width of the response. // This function may set the current_position[Y_AXIS] below Y_MIN_POS, if the function succeeded. // If this function failed, the Y coordinate will never be outside the working space. #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; // Was the sensor point detected too far in the minus Y axis? // If yes, the center of the induction point cannot be reached by the machine. { 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(""); } // Search in the positive Y direction, until a maximum diameter is found. // (the next diameter is smaller than the current one.) 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; // SERIAL_PROTOCOLPGM("Failed 1\n"); // current_position[X_AXIS] = center_old_x; // 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; // SERIAL_PROTOCOLPGM("Failed 2\n"); // current_position[X_AXIS] = center_old_x; // 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) { 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; } { // Find the positive Y hit. This gives the extreme Y value for the search of the maximum diameter in the -Y direction. 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) { // Either the induction sensor is too high, or the induction sensor target is out of reach. current_position[Y_AXIS] = center_old_y; goto canceled; } // Search in the negative Y direction, until a maximum diameter is found. dmax = 0.f; // if (y0 + 1.f < y1) // y1 = y0 + 1.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) { // Only the hit in the positive direction found. xmax = xmax1; ymax = y0; } else { // Both positive and negative directions found. 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; } } } { // Compare the distance in the Y+ direction with the diameter in the X direction. // Find the positive Y hit once again, this time along the Y axis going through the X point with the highest diameter. 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) { // Probably not even a half circle was detected. The induction point is likely too far in the minus Y direction. // First verify, if the measurement has been done at a sufficient height. If no, lower the Z axis a bit. 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 { // Estimate the circle radius from the maximum diameter and height: 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 { // The point may end up outside of the machine working space. // That is all right as it helps to improve the accuracy of the measurement point // due to averaging. // For the y correction, use an average of dmax/2 and the estimated radius. r = 0.5f * (0.5f * dmax + r); ymax = current_position[Y_AXIS] - r; } } } else { // If the diameter of the detected spot was smaller than a minimum allowed, // the induction sensor is probably too high. Returning false will force // the sensor to be lowered a tiny bit. result = xmax >= MIN_BED_SENSOR_POINT_RESPONSE_DMR; if (y0 > Y_MIN_POS_FOR_BED_CALIBRATION + 0.2f) // Only in case both left and right y tangents are known, use them. // If y0 is close to the bed edge, it may not be symmetric to the right tangent. ymax = 0.5f * ymax + 0.25f * (y0 + y1); } } // Go to the center. 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(""); } // Don't clamp current_position[Y_AXIS], because the out-of-reach Y coordinate may actually be true. // Only clamp the coordinate to go. go_xy(current_position[X_AXIS], max(Y_MIN_POS, current_position[Y_AXIS]), homing_feedrate[X_AXIS] / 60.f); // delay_keep_alive(3000); } if (result) return true; // otherwise clamp the Y coordinate canceled: // Go back to the center. 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; } // Scan the mesh bed induction points one by one by a left-right zig-zag movement, // write the trigger coordinates to the serial line. // Useful for visualizing the behavior of the bed induction detector. 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) { // Don't let the manage_inactivity() function remove power from the motors. refresh_cmd_timeout(); // Reusing the z_values memory for the measurement cache. // 7x7=49 floats, good for 16 (x,y,z) vectors. 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); // SERIAL_ECHOLNPGM("find_bed_offset_and_skew verbosity level: "); // SERIAL_ECHO(int(verbosity_level)); // SERIAL_ECHOPGM(""); #ifdef MESH_BED_CALIBRATION_SHOW_LCD lcd_implementation_clear(); lcd_print_at_PGM(0, 0, MSG_FIND_BED_OFFSET_AND_SKEW_LINE1); #endif /* MESH_BED_CALIBRATION_SHOW_LCD */ // Collect the rear 2x3 points. current_position[Z_AXIS] = MESH_HOME_Z_SEARCH; for (int k = 0; k < 4; ++ k) { // Don't let the manage_inactivity() function remove power from the motors. refresh_cmd_timeout(); #ifdef MESH_BED_CALIBRATION_SHOW_LCD lcd_print_at_PGM(0, 1, MSG_FIND_BED_OFFSET_AND_SKEW_LINE2); lcd_implementation_print_at(0, 2, k+1); lcd_printPGM(MSG_FIND_BED_OFFSET_AND_SKEW_LINE3); #endif /* MESH_BED_CALIBRATION_SHOW_LCD */ float *pt = pts + k * 2; // Go up to z_initial. go_to_current(homing_feedrate[Z_AXIS] / 60.f); if (verbosity_level >= 20) { // Go to Y0, wait, then go to Y-4. 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); } // Go to the measurement point position. 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); 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) { // Improve the position of the 1st row sensor points by a zig-zag movement. 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; // Try to move the Z axis down a bit to increase a chance of the sensor to trigger. current_position[Z_AXIS] -= 0.025f; enable_endstops(false); enable_z_endstop(false); go_to_current(homing_feedrate[Z_AXIS]); } if (i == 0) // not found return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND; } #endif if (verbosity_level >= 10) delay_keep_alive(3000); // Save the detected point position and then clamp the Y coordinate, which may have been estimated // to lie outside the machine working space. pt[0] = current_position[X_AXIS]; pt[1] = current_position[Y_AXIS]; if (current_position[Y_AXIS] < Y_MIN_POS) current_position[Y_AXIS] = Y_MIN_POS; // Start searching for the other points at 3mm above the last point. current_position[Z_AXIS] += 3.f; cntr[0] += pt[0]; cntr[1] += pt[1]; if (verbosity_level >= 10 && k == 0) { // Show the zero. Test, whether the Y motor skipped steps. 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) { // Test the positions. Are the positions reproducible? Now the calibration is active in the planner. delay_keep_alive(3000); for (int8_t mesh_point = 0; mesh_point < 4; ++ mesh_point) { // Don't let the manage_inactivity() function remove power from the motors. refresh_cmd_timeout(); // Go to the measurement point. // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew(). 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); } } BedSkewOffsetDetectionResultType 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 // Fearlessly store the calibration values into the eeprom. 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 // Correct the current_position to match the transformed coordinate system after world2machine_rotation_and_skew and world2machine_shift were set. world2machine_update_current(); if (verbosity_level >= 20) { // Test the positions. Are the positions reproducible? Now the calibration is active in the planner. delay_keep_alive(3000); for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) { // Don't let the manage_inactivity() function remove power from the motors. refresh_cmd_timeout(); // Go to the measurement point. // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew(). 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; } BedSkewOffsetDetectionResultType improve_bed_offset_and_skew(int8_t method, int8_t verbosity_level, uint8_t &too_far_mask) { // Don't let the manage_inactivity() function remove power from the motors. refresh_cmd_timeout(); // Mask of the first three points. Are they too far? too_far_mask = 0; // Reusing the z_values memory for the measurement cache. // 7x7=49 floats, good for 16 (x,y,z) vectors. 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); // Cache the current correction matrix. 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]; // and reset the correction matrix, so the planner will not do anything. world2machine_reset(); bool endstops_enabled = enable_endstops(false); bool endstop_z_enabled = enable_z_endstop(false); #ifdef MESH_BED_CALIBRATION_SHOW_LCD lcd_implementation_clear(); lcd_print_at_PGM(0, 0, MSG_IMPROVE_BED_OFFSET_AND_SKEW_LINE1); #endif /* MESH_BED_CALIBRATION_SHOW_LCD */ // Collect a matrix of 9x9 points. BedSkewOffsetDetectionResultType result = BED_SKEW_OFFSET_DETECTION_PERFECT; for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) { // Don't let the manage_inactivity() function remove power from the motors. refresh_cmd_timeout(); // Print the decrasing ID of the measurement point. #ifdef MESH_BED_CALIBRATION_SHOW_LCD lcd_print_at_PGM(0, 1, MSG_IMPROVE_BED_OFFSET_AND_SKEW_LINE2); lcd_implementation_print_at(0, 2, mesh_point+1); lcd_printPGM(MSG_IMPROVE_BED_OFFSET_AND_SKEW_LINE3); #endif /* MESH_BED_CALIBRATION_SHOW_LCD */ // Move up. 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) { // Go to Y0, wait, then go to Y-4. 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); } // Go to the measurement point. // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew(). 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]; // The calibration points are very close to the min Y. 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 its Z position by running the normal vertical search. if (verbosity_level >= 10) delay_keep_alive(3000); find_bed_induction_sensor_point_z(); if (verbosity_level >= 10) delay_keep_alive(3000); // Try to move the Z axis down a bit to increase a chance of the sensor to trigger. current_position[Z_AXIS] -= 0.025f; // Improve the point position by searching its center in a current plane. 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) { // Because the sensor cannot move in front of the first row // of the sensor points, the y position cannot be measured // by a cross center method. // Use a zig-zag search for the first row of the points. 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) { // Average the last 4 measurements. 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) { // Give up. result = BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND; goto canceled; } else { // Try to move the Z axis down a bit to increase a chance of the sensor to trigger. 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); } // Don't let the manage_inactivity() function remove power from the motors. refresh_cmd_timeout(); // Average the last 4 measurements. 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) { // Test the positions. Are the positions reproducible? for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) { // Don't let the manage_inactivity() function remove power from the motors. refresh_cmd_timeout(); // Go to the measurement point. // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew(). 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(""); } } { // First fill in the too_far_mask from the measured points. 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; } // In case of success, update the too_far_mask from the calculated points. 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 // Fearlessly store the calibration values into the eeprom. 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 // Correct the current_position to match the transformed coordinate system after world2machine_rotation_and_skew and world2machine_shift were set. world2machine_update_current(); enable_endstops(false); enable_z_endstop(false); if (verbosity_level >= 5) { // Test the positions. Are the positions reproducible? Now the calibration is active in the planner. delay_keep_alive(3000); for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) { // Don't let the manage_inactivity() function remove power from the motors. refresh_cmd_timeout(); // Go to the measurement point. // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew(). 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(""); } } } // Sample Z heights for the mesh bed leveling. // In addition, store the results into an eeprom, to be used later for verification of the bed leveling process. { // The first point defines the reference. 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) { 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); find_bed_induction_sensor_point_z(); // Get cords of measuring point 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; // Zig zag mbl.set_z(ix, iy, current_position[Z_AXIS]); } { // Verify the span of the Z values. 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) { // The span of the Z offsets is extreme. Give up. // Homing failed on some of the points. SERIAL_PROTOCOLLNPGM("Exreme span of the Z values!"); goto canceled; } } // Store the correction values to EEPROM. // Offsets of the Z heiths of the calibration points from the first point. // The offsets are saved as 16bit signed int, scaled to tenths of microns. { 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(&dif_quantized)); { uint16_t z_offset_u = eeprom_read_word((uint16_t*)addr); float dif2 = *reinterpret_cast(&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(""); } addr += 2; } } mbl.upsample_3x3(); mbl.active = true; // Don't let the manage_inactivity() function remove power from the motors. refresh_cmd_timeout(); // Go home. current_position[Z_AXIS] = Z_MIN_POS; 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; // Clamp to the physical coordinates. world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]); go_to_current(homing_feedrate[X_AXIS]/60); enable_endstops(endstops_enabled); enable_z_endstop(endstop_z_enabled); // Don't let the manage_inactivity() function remove power from the motors. refresh_cmd_timeout(); return result; canceled: // Don't let the manage_inactivity() function remove power from the motors. refresh_cmd_timeout(); // Print head up. current_position[Z_AXIS] = MESH_HOME_Z_SEARCH; go_to_current(homing_feedrate[Z_AXIS]/60); // Store the identity matrix to EEPROM. reset_bed_offset_and_skew(); enable_endstops(endstops_enabled); enable_z_endstop(endstop_z_enabled); return result; } bool scan_bed_induction_points(int8_t verbosity_level) { // Don't let the manage_inactivity() function remove power from the motors. refresh_cmd_timeout(); // Reusing the z_values memory for the measurement cache. // 7x7=49 floats, good for 16 (x,y,z) vectors. 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); // Cache the current correction matrix. 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]; // and reset the correction matrix, so the planner will not do anything. world2machine_reset(); bool endstops_enabled = enable_endstops(false); bool endstop_z_enabled = enable_z_endstop(false); // Collect a matrix of 9x9 points. for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) { // Don't let the manage_inactivity() function remove power from the motors. refresh_cmd_timeout(); // Move up. current_position[Z_AXIS] = MESH_HOME_Z_SEARCH; enable_endstops(false); enable_z_endstop(false); go_to_current(homing_feedrate[Z_AXIS]/60); // Go to the measurement point. // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew(). 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]; // The calibration points are very close to the min Y. 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(); } // Don't let the manage_inactivity() function remove power from the motors. refresh_cmd_timeout(); enable_endstops(false); enable_z_endstop(false); // Don't let the manage_inactivity() function remove power from the motors. refresh_cmd_timeout(); enable_endstops(endstops_enabled); enable_z_endstop(endstop_z_enabled); return true; }