From 180af46fe46947a21526655068948abcffef6636 Mon Sep 17 00:00:00 2001 From: espr14 Date: Tue, 12 Jan 2021 00:27:06 +0100 Subject: [PATCH 1/5] Try double height --- Firmware/mesh_bed_calibration.cpp | 2 +- Firmware/xyzcal.cpp | 79 ++++++++++++++++--------------- 2 files changed, 42 insertions(+), 39 deletions(-) diff --git a/Firmware/mesh_bed_calibration.cpp b/Firmware/mesh_bed_calibration.cpp index 4f4261161..a0efc3aae 100644 --- a/Firmware/mesh_bed_calibration.cpp +++ b/Firmware/mesh_bed_calibration.cpp @@ -372,7 +372,7 @@ BedSkewOffsetDetectionResultType calculate_machine_skew_and_offset_LS( { angleDiff = fabs(a2 - a1); /// XY skew and Y-bed skew - DBG(_n("Measured skews: %f° %f°\n"), degrees(a2 - a1), degrees(a2)); + DBG(_n("Measured skews: %f %f\n"), degrees(a2 - a1), degrees(a2)); eeprom_update_float((float *)(EEPROM_XYZ_CAL_SKEW), angleDiff); //storing xyz cal. skew to be able to show in support menu later if (angleDiff > bed_skew_angle_mild) result = (angleDiff > bed_skew_angle_extreme) ? diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index c68804890..1370542bc 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -293,7 +293,7 @@ bool xyzcal_spiral2(int16_t cx, int16_t cy, int16_t z0, int16_t dz, int16_t radi dad = dad_max - ((719 - ad) / k); r = (float)(((uint32_t)(719 - ad)) * (-radius)) / 720; } - ar = (ad + rotation)* (float)M_PI / 180; + ar = radians(ad + rotation); int x = (int)(cx + (cos(ar) * r)); int y = (int)(cy + (sin(ar) * r)); int z = (int)(z0 - ((float)((int32_t)dz * ad) / 720)); @@ -831,9 +831,8 @@ float median(float *points, const uint8_t num_points){ void dynamic_circle(uint8_t *matrix_32x32, float &x, float &y, float &r, uint8_t iterations){ /// circle of 10.5 diameter has 33 in circumference, don't go much above const constexpr uint8_t num_points = 33; - float pi_2_div_num_points = 2 * M_PI / num_points; + const float pi_2_div_num_points = 2 * M_PI / num_points; const constexpr uint8_t target_z = 32; ///< target z height of the circle - float angle; float max_change = 0.5f; ///< avoids too fast changes (avoid oscillation) const uint8_t blocks = num_points; float shifts_x[blocks]; @@ -848,7 +847,7 @@ void dynamic_circle(uint8_t *matrix_32x32, float &x, float &y, float &r, uint8_t /// read points on the circle for (uint8_t p = 0; p < num_points; ++p){ - angle = p * pi_2_div_num_points; + const float angle = p * pi_2_div_num_points; const float height = get_value(matrix_32x32, r * cos(angle) + x, r * sin(angle) + y) - target_z; // DBG(_n("%f "), point); @@ -858,7 +857,8 @@ void dynamic_circle(uint8_t *matrix_32x32, float &x, float &y, float &r, uint8_t } // DBG(_n(" points\n")); - const float norm = 1.f / 32.f; + const float reducer = 32.f; ///< reduces speed of convergency to avoid oscillation + const float norm = 1.f / reducer; x += CLAMP(median(shifts_x, blocks) * norm, -max_change, max_change); y += CLAMP(median(shifts_y, blocks) * norm, -max_change, max_change); r += CLAMP(median(shifts_r, blocks) * norm * .5f, -max_change, max_change); @@ -912,49 +912,52 @@ bool xyzcal_scan_and_process(void){ bool ret = false; int16_t x = _X; int16_t y = _Y; - int16_t z = _Z; + const int16_t z = _Z; uint8_t *matrix32 = (uint8_t *)block_buffer; uint16_t *pattern08 = (uint16_t *)(matrix32 + 32 * 32); uint16_t *pattern10 = (uint16_t *)(pattern08 + 12); - xyzcal_scan_pixels_32x32_Zhop(x, y, z - 72, 2400, 200, matrix32); - print_image(matrix32); - for (uint8_t i = 0; i < 12; i++){ pattern08[i] = pgm_read_word((uint16_t*)(xyzcal_point_pattern_08 + i)); pattern10[i] = pgm_read_word((uint16_t*)(xyzcal_point_pattern_10 + i)); } - - /// SEARCH FOR BINARY CIRCLE - uint8_t uc = 0; - uint8_t ur = 0; - - - /// max match = 132, 1/2 good = 66, 2/3 good = 88 - if (find_patterns(matrix32, pattern08, pattern10, uc, ur) >= 88){ - /// find precise circle - /// move to the center of the pattern (+5.5) - float xf = uc + 5.5f; - float yf = ur + 5.5f; - float radius = 5; ///< default radius - const uint8_t iterations = 20; - dynamic_circle(matrix32, xf, yf, radius, iterations); - if (ABS(xf - (uc + 5.5f)) > 3 || ABS(yf - (ur + 5.5f)) > 3 || ABS(radius - 5) > 3){ - DBG(_n(" [%f %f][%f] mm divergence\n"), xf - (uc + 5.5f), yf - (ur + 5.5f), radius - 5); - /// dynamic algorithm diverged, use original position instead - xf = uc + 5.5f; - yf = ur + 5.5f; - } - /// move to the center of area and convert to position - xf = (float)x + (xf - 15.5f) * 64; - yf = (float)y + (yf - 15.5f) * 64; - DBG(_n(" [%f %f] mm pattern center\n"), pos_2_mm(xf), pos_2_mm(yf)); - x = round_to_i16(xf); - y = round_to_i16(yf); - xyzcal_lineXYZ_to(x, y, z, 200, 0); - ret = true; + /// Lower z if pattern not found + for (int8_t lower = 0; lower < 60; lower += 50){ + xyzcal_scan_pixels_32x32_Zhop(x, y, z - lower, 2400, 200, matrix32); + print_image(matrix32); + + /// SEARCH FOR BINARY CIRCLE + uint8_t uc = 0; + uint8_t ur = 0; + + /// max match = 132, 1/2 good = 66, 2/3 good = 88 + if (find_patterns(matrix32, pattern08, pattern10, uc, ur) >= 88){ + /// find precise circle + /// move to the center of the pattern (+5.5) + float xf = uc + 5.5f; + float yf = ur + 5.5f; + float radius = 4.5f; ///< default radius + const uint8_t iterations = 20; + dynamic_circle(matrix32, xf, yf, radius, iterations); + if (ABS(xf - (uc + 5.5f)) > 3 || ABS(yf - (ur + 5.5f)) > 3 || ABS(radius - 5) > 3){ + DBG(_n(" [%f %f][%f] mm divergence\n"), xf - (uc + 5.5f), yf - (ur + 5.5f), radius - 5); + /// dynamic algorithm diverged, use original position instead + xf = uc + 5.5f; + yf = ur + 5.5f; + } + + /// move to the center of area and convert to position + xf = (float)x + (xf - 15.5f) * 64; + yf = (float)y + (yf - 15.5f) * 64; + DBG(_n(" [%f %f] mm pattern center\n"), pos_2_mm(xf), pos_2_mm(yf)); + x = round_to_i16(xf); + y = round_to_i16(yf); + xyzcal_lineXYZ_to(x, y, z, 200, 0); + ret = true; + break; + } } /// wipe buffer From 546812294ec91d5ad0011c3de7201d6464fa21e0 Mon Sep 17 00:00:00 2001 From: espr14 Date: Tue, 12 Jan 2021 00:34:28 +0100 Subject: [PATCH 2/5] Clean serial output --- Firmware/xyzcal.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index 1370542bc..8365fc94b 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -682,7 +682,7 @@ uint8_t xyzcal_find_pattern_12x12_in_32x32(uint8_t* pixels, uint16_t* pattern, u } // DBG(_n("\n")); } - DBG(_n("max_c=%d max_r=%d max_match=%d pixel\n"), max_c, max_r, max_match); + DBG(_n("Pattern center [%f %f], match %f%%\n"), max_c + 5.5f, max_r + 5.5f, max_match / 1.32f); *pc = max_c; *pr = max_r; @@ -839,7 +839,7 @@ void dynamic_circle(uint8_t *matrix_32x32, float &x, float &y, float &r, uint8_t float shifts_y[blocks]; float shifts_r[blocks]; - DBG(_n(" [%f, %f][%f] start circle\n"), x, y, r); + // DBG(_n(" [%f, %f][%f] start circle\n"), x, y, r); for (int8_t i = iterations; i > 0; --i){ From 9dceb488b68bb131e512fe4c7a96c65cc28d4ef2 Mon Sep 17 00:00:00 2001 From: espr14 Date: Tue, 12 Jan 2021 16:04:04 +0100 Subject: [PATCH 3/5] Revert double scanning --- Firmware/xyzcal.cpp | 73 +++++++++++++++++++++++---------------------- 1 file changed, 37 insertions(+), 36 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index 8365fc94b..f6cb1ea10 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -923,40 +923,38 @@ bool xyzcal_scan_and_process(void){ pattern10[i] = pgm_read_word((uint16_t*)(xyzcal_point_pattern_10 + i)); } - /// Lower z if pattern not found - for (int8_t lower = 0; lower < 60; lower += 50){ - xyzcal_scan_pixels_32x32_Zhop(x, y, z - lower, 2400, 200, matrix32); - print_image(matrix32); - - /// SEARCH FOR BINARY CIRCLE - uint8_t uc = 0; - uint8_t ur = 0; - - /// max match = 132, 1/2 good = 66, 2/3 good = 88 - if (find_patterns(matrix32, pattern08, pattern10, uc, ur) >= 88){ - /// find precise circle - /// move to the center of the pattern (+5.5) - float xf = uc + 5.5f; - float yf = ur + 5.5f; - float radius = 4.5f; ///< default radius - const uint8_t iterations = 20; - dynamic_circle(matrix32, xf, yf, radius, iterations); - if (ABS(xf - (uc + 5.5f)) > 3 || ABS(yf - (ur + 5.5f)) > 3 || ABS(radius - 5) > 3){ - DBG(_n(" [%f %f][%f] mm divergence\n"), xf - (uc + 5.5f), yf - (ur + 5.5f), radius - 5); - /// dynamic algorithm diverged, use original position instead - xf = uc + 5.5f; - yf = ur + 5.5f; - } + xyzcal_scan_pixels_32x32_Zhop(x, y, z, 2400, 200, matrix32); + print_image(matrix32); - /// move to the center of area and convert to position - xf = (float)x + (xf - 15.5f) * 64; - yf = (float)y + (yf - 15.5f) * 64; - DBG(_n(" [%f %f] mm pattern center\n"), pos_2_mm(xf), pos_2_mm(yf)); - x = round_to_i16(xf); - y = round_to_i16(yf); - xyzcal_lineXYZ_to(x, y, z, 200, 0); - ret = true; - break; + /// SEARCH FOR BINARY CIRCLE + uint8_t uc = 0; + uint8_t ur = 0; + + /// max match = 132, 1/2 good = 66, 2/3 good = 88 + if (find_patterns(matrix32, pattern08, pattern10, uc, ur) >= 88){ + /// find precise circle + /// move to the center of the pattern (+5.5) + float xf = uc + 5.5f; + float yf = ur + 5.5f; + float radius = 4.5f; ///< default radius + const uint8_t iterations = 20; + dynamic_circle(matrix32, xf, yf, radius, iterations); + if (ABS(xf - (uc + 5.5f)) > 3 || ABS(yf - (ur + 5.5f)) > 3 || ABS(radius - 5) > 3) + { + DBG(_n(" [%f %f][%f] mm divergence\n"), xf - (uc + 5.5f), yf - (ur + 5.5f), radius - 5); + /// dynamic algorithm diverged, use original position instead + xf = uc + 5.5f; + yf = ur + 5.5f; + } + + /// move to the center of area and convert to position + xf = (float)x + (xf - 15.5f) * 64; + yf = (float)y + (yf - 15.5f) * 64; + DBG(_n(" [%f %f] mm pattern center\n"), pos_2_mm(xf), pos_2_mm(yf)); + x = round_to_i16(xf); + y = round_to_i16(yf); + xyzcal_lineXYZ_to(x, y, z, 200, 0); + ret = true; } } @@ -971,9 +969,11 @@ bool xyzcal_find_bed_induction_sensor_point_xy(void){ DBG(_n("xyzcal_find_bed_induction_sensor_point_xy x=%ld y=%ld z=%ld\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS]); st_synchronize(); - pos_i16_t x = _X; - pos_i16_t y = _Y; - pos_i16_t z = _Z; + const pos_i16_t x = _X; + const pos_i16_t y = _Y; + const pos_i16_t z = _Z; + ///< magic constant, lowers min_z after searchZ to obtain more dense data in scan + const pos_i16_t lower_z = 72; uint8_t point = xyzcal_xycoords2point(x, y); x = pgm_read_word((uint16_t *)(xyzcal_point_xcoords + point)); @@ -983,6 +983,7 @@ bool xyzcal_find_bed_induction_sensor_point_xy(void){ xyzcal_lineXYZ_to(x, y, z, 200, 0); if (xyzcal_searchZ()){ + xyzcal_lineXYZ_to(_X, _Y, _Z - lower_z, 200, 0); xyzcal_lineXYZ_to(x, y, _Z, 200, 0); ret = xyzcal_scan_and_process(); } From f3faf651c576f8bf7a53f6d7fa5489948d32bd82 Mon Sep 17 00:00:00 2001 From: espr14 Date: Tue, 12 Jan 2021 16:12:34 +0100 Subject: [PATCH 4/5] Move down before scanning --- Firmware/xyzcal.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index f6cb1ea10..98cd04a4d 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -984,7 +984,6 @@ bool xyzcal_find_bed_induction_sensor_point_xy(void){ if (xyzcal_searchZ()){ xyzcal_lineXYZ_to(_X, _Y, _Z - lower_z, 200, 0); - xyzcal_lineXYZ_to(x, y, _Z, 200, 0); ret = xyzcal_scan_and_process(); } xyzcal_meassure_leave(); From 21d6f970ef7e139840dc5535fb09f03c3ff99ed6 Mon Sep 17 00:00:00 2001 From: espr14 Date: Tue, 12 Jan 2021 16:26:46 +0100 Subject: [PATCH 5/5] Fix build --- Firmware/xyzcal.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index 98cd04a4d..47eecb942 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -955,7 +955,6 @@ bool xyzcal_scan_and_process(void){ y = round_to_i16(yf); xyzcal_lineXYZ_to(x, y, z, 200, 0); ret = true; - } } /// wipe buffer @@ -969,8 +968,8 @@ bool xyzcal_find_bed_induction_sensor_point_xy(void){ DBG(_n("xyzcal_find_bed_induction_sensor_point_xy x=%ld y=%ld z=%ld\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS]); st_synchronize(); - const pos_i16_t x = _X; - const pos_i16_t y = _Y; + pos_i16_t x = _X; + pos_i16_t y = _Y; const pos_i16_t z = _Z; ///< magic constant, lowers min_z after searchZ to obtain more dense data in scan const pos_i16_t lower_z = 72;