diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp index 240bc1b9..0f8b93b0 100644 --- a/Firmware/Marlin_main.cpp +++ b/Firmware/Marlin_main.cpp @@ -2367,7 +2367,7 @@ bool gcode_M45(bool onlyZ, int8_t verbosity_level) current_position[Z_AXIS] = MESH_HOME_Z_SEARCH; plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[Z_AXIS] / 40, active_extruder); st_synchronize(); -#ifndef NEW_XYZCAL +//#ifndef NEW_XYZCAL if (result >= 0) { #ifdef HEATBED_V2 @@ -2390,7 +2390,9 @@ bool gcode_M45(bool onlyZ, int8_t verbosity_level) // if (result >= 0) babystep_apply(); #endif //HEATBED_V2 } -#endif //NEW_XYZCAL +//#endif //NEW_XYZCAL + lcd_update_enable(true); + lcd_update(2); lcd_bed_calibration_show_result(result, point_too_far_mask); if (result >= 0) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index c6a1750a..b59918f7 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -470,6 +470,82 @@ int16_t xyzcal_find_pattern_12x12_in_32x32(uint8_t* pixels, uint16_t* pattern, u return max_match; } +#define MAX_DIAMETR 600 + +int8_t xyzcal_find_point_center2(uint16_t delay_us) +{ + printf_P(PSTR("xyzcal_find_point_center2\n")); + int16_t x0 = _X; + int16_t y0 = _Y; + int16_t z0 = _Z; + printf_P(PSTR(" x0=%d\n"), x0); + printf_P(PSTR(" y0=%d\n"), y0); + printf_P(PSTR(" z0=%d\n"), z0); + + xyzcal_lineXYZ_to(_X, _Y, z0 + 400, 500, -1); + xyzcal_lineXYZ_to(_X, _Y, z0 - 400, 500, 1); + xyzcal_lineXYZ_to(_X, _Y, z0 + 400, 500, -1); + xyzcal_lineXYZ_to(_X, _Y, z0 - 400, 500, 1); + + z0 = _Z; + +// xyzcal_lineXYZ_to(x0, y0, z0 - 100, 500, 1); +// z0 = _Z; +// printf_P(PSTR(" z0=%d\n"), z0); +// xyzcal_lineXYZ_to(x0, y0, z0 + 100, 500, -1); +// z0 += _Z; +// z0 /= 2; + printf_P(PSTR(" z0=%d\n"), z0); +// xyzcal_lineXYZ_to(x0, y0, z0 - 100, 500, 1); +// z0 = _Z - 10; + + xyzcal_lineXYZ_to(x0 - MAX_DIAMETR, y0, z0, delay_us, -1); + int16_t dx1 = x0 - _X; + if (dx1 >= MAX_DIAMETR) + { + printf_P(PSTR("!!! dx1 = %d\n"), dx1); + return 0; + } + xyzcal_lineXYZ_to(x0, y0, z0, delay_us, 0); + xyzcal_lineXYZ_to(x0 + MAX_DIAMETR, y0, z0, delay_us, -1); + int16_t dx2 = _X - x0; + if (dx2 >= MAX_DIAMETR) + { + printf_P(PSTR("!!! dx2 = %d\n"), dx2); + return 0; + } + xyzcal_lineXYZ_to(x0, y0, z0, delay_us, 0); + xyzcal_lineXYZ_to(x0 , y0 - MAX_DIAMETR, z0, delay_us, -1); + int16_t dy1 = y0 - _Y; + if (dy1 >= MAX_DIAMETR) + { + printf_P(PSTR("!!! dy1 = %d\n"), dy1); + return 0; + } + xyzcal_lineXYZ_to(x0, y0, z0, delay_us, 0); + xyzcal_lineXYZ_to(x0, y0 + MAX_DIAMETR, z0, delay_us, -1); + int16_t dy2 = _Y - y0; + if (dy2 >= MAX_DIAMETR) + { + printf_P(PSTR("!!! dy2 = %d\n"), dy2); + return 0; + } + printf_P(PSTR("dx1=%d\n"), dx1); + printf_P(PSTR("dx2=%d\n"), dx2); + printf_P(PSTR("dy1=%d\n"), dy1); + printf_P(PSTR("dy2=%d\n"), dy2); + + x0 += (dx2 - dx1) / 2; + y0 += (dy2 - dy1) / 2; + + printf_P(PSTR(" x0=%d\n"), x0); + printf_P(PSTR(" y0=%d\n"), y0); + + xyzcal_lineXYZ_to(x0, y0, z0, delay_us, 0); + + return 1; +} + #ifdef XYZCAL_FIND_POINT_CENTER int8_t xyzcal_find_point_center(int16_t x0, int16_t y0, int16_t z0, int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t turns) { @@ -671,7 +747,22 @@ bool xyzcal_find_bed_induction_sensor_point_xy(void) xyzcal_lineXYZ_to(x, y, z, 200, 0); if (xyzcal_scan_and_process()) { - ret = true; + if (xyzcal_find_point_center2(500)) + { + uint32_t x_avg = 0; + uint32_t y_avg = 0; + uint8_t n; for (n = 0; n < 4; n++) + { + if (!xyzcal_find_point_center2(1000)) break; + x_avg += _X; + y_avg += _Y; + } + if (n == 4) + { + xyzcal_lineXYZ_to(x_avg >> 2, y_avg >> 2, _Z, 200, 0); + ret = true; + } + } } } xyzcal_meassure_leave(); diff --git a/Firmware/xyzcal.h b/Firmware/xyzcal.h index a91b1c18..5590396e 100644 --- a/Firmware/xyzcal.h +++ b/Firmware/xyzcal.h @@ -27,6 +27,8 @@ extern int16_t xyzcal_match_pattern_12x12_in_32x32(uint16_t* pattern, uint8_t* p extern int16_t xyzcal_find_pattern_12x12_in_32x32(uint8_t* pixels, uint16_t* pattern, uint8_t* pc, uint8_t* pr); +extern int8_t xyzcal_find_point_center2(uint16_t delay_us); + //extern int8_t xyzcal_find_point_center(int16_t x0, int16_t y0, int16_t z0, int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t turns); extern bool xyzcal_searchZ(void);