From ccf26e4e7a7c3ca1d476f6d5f40c8da523fe0fe2 Mon Sep 17 00:00:00 2001 From: espr14 Date: Fri, 18 Dec 2020 20:04:00 +0100 Subject: [PATCH 01/18] Basic acceleration --- Firmware/xyzcal.cpp | 54 ++++++++++++++++++++++++++++++++++++--------- 1 file changed, 44 insertions(+), 10 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index 2130b223e..3a794f9bf 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -360,20 +360,36 @@ int8_t xyzcal_meassure_pinda_hysterezis(int16_t min_z, int16_t max_z, uint16_t d } #endif //XYZCAL_MEASSURE_PINDA_HYSTEREZIS +uint8_t slow_down_z(uint16_t delay_us){ + sm4_do_step(Z_AXIS_MASK); + delayMicroseconds(delay_us / 3 * 4); + sm4_do_step(Z_AXIS_MASK); + delayMicroseconds(delay_us * 2); + sm4_do_step(Z_AXIS_MASK); + delayMicroseconds(delay_us * 4); + return 3; +} + +uint8_t speed_up_z(int16_t &z, uint8_t direction, uint16_t delay_us){ + sm4_do_step(Z_AXIS_MASK); + delayMicroseconds(delay_us * 4); + sm4_do_step(Z_AXIS_MASK); + delayMicroseconds(delay_us * 2); + sm4_do_step(Z_AXIS_MASK); + delayMicroseconds(delay_us / 3 * 4); + return 3; +} + void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t* pixels){ if(!pixels) return; int16_t z = _Z; + int16_t z_trig; uint16_t line_buffer[32]; - xyzcal_lineXYZ_to(cx, cy, z, delay_us, 0); + xyzcal_lineXYZ_to(cx - 1024, cy - 1024, min_z, delay_us, 0); for (uint8_t r = 0; r < 32; r++){ ///< Y axis xyzcal_lineXYZ_to(_X, cy - 1024 + r * 64, z, delay_us, 0); - for (int8_t d = 0; d < 2; ++d){ ///< direction - - // xyzcal_lineXYZ_to((d & 1) ? (cx + 1024) : (cx - 1024), _Y, z, 2 * delay_us, 0); - // xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 1); - // xyzcal_lineXYZ_to(_X, _Y, max_z, delay_us, -1); - + for (int8_t d = 0; d < 2; ++d){ ///< direction xyzcal_lineXYZ_to((d & 1) ? (cx + 1024) : (cx - 1024), _Y, min_z, delay_us, 0); z = _Z; @@ -382,29 +398,47 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ /// move up to un-trigger (surpress hysteresis) sm4_set_dir(Z_AXIS, Z_PLUS); + z += speed_up_z(delay_us); while (z < max_z && _PINDA){ sm4_do_step(Z_AXIS_MASK); delayMicroseconds(delay_us); z++; } int16_t last_top_z = z; + z += slow_down_z(delay_us); /// move down to trigger sm4_set_dir(Z_AXIS, Z_MINUS); + + /// speed up + do (){ + if (z <= min_z || _PINDA) break; + sm4_do_step(Z_AXIS_MASK); + delayMicroseconds(delay_us * 4); + if (z <= min_z || _PINDA) break; + sm4_do_step(Z_AXIS_MASK); + delayMicroseconds(delay_us * 2); + if (z <= min_z || _PINDA) break; + sm4_do_step(Z_AXIS_MASK); + delayMicroseconds(delay_us / 3 * 4); + } while (0); + while (z > min_z && !_PINDA){ sm4_do_step(Z_AXIS_MASK); delayMicroseconds(delay_us); z--; } - + z_trig = z; + z -= slow_down_z(delay_us); + count_position[2] = z; if (d == 0){ - line_buffer[c] = (uint16_t)(z - min_z); + line_buffer[c] = (uint16_t)(z_trig - min_z); } else { /// data reversed in X // DBG(_n("%04x"), (line_buffer[31 - c] + (z - min_z)) / 2); /// save average of both directions - pixels[(uint16_t)r * 32 + (31 - c)] = (uint8_t)MIN((uint32_t)255, ((uint32_t)line_buffer[31 - c] + (z - min_z)) / 2); + pixels[(uint16_t)r * 32 + (31 - c)] = (uint8_t)MIN((uint32_t)255, ((uint32_t)line_buffer[31 - c] + (z_trig - min_z)) / 2); } /// move to the next point From f39a0999e1d0aefe7949c52e170c6f67e11591d0 Mon Sep 17 00:00:00 2001 From: espr14 Date: Fri, 18 Dec 2020 21:33:43 +0100 Subject: [PATCH 02/18] Prepare full acceleration control --- Firmware/xyzcal.cpp | 49 +++++++++++++++++++++++++++++++++++++++------ 1 file changed, 43 insertions(+), 6 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index 3a794f9bf..1c03c4c3f 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -36,6 +36,11 @@ #define Z_PLUS 0 #define Z_MINUS 1 +/// 10000 = 1 mm/s +#define MAX_DELAY 10000 +#define Z_ACCEL 300 +#define XY_ACCEL 1000 + #define _PI 3.14159265F /// \returns positive value always @@ -360,8 +365,40 @@ int8_t xyzcal_meassure_pinda_hysterezis(int16_t min_z, int16_t max_z, uint16_t d } #endif //XYZCAL_MEASSURE_PINDA_HYSTEREZIS -uint8_t slow_down_z(uint16_t delay_us){ - sm4_do_step(Z_AXIS_MASK); +void accelerate(uint8_t axis, int16_t acc, uint16_t &delay_us, uint16_t min_delay_us){ + sm4_do_step(axis); + + if (acc > 0 && delay_us == min_delay_us){ + delayMicroseconds(delay_us); + return; + } + + // v1 = v0 + a * t + // 0.01 = length of a step + const float d0 = delay_us * 0.000001f; + const float v1 = (0.01f / d0 + acc * d0); + uint16_t d1; + if (v1 <= 0.1f){ + d1 = MAX_DELAY; ///< already too slow so it wants to move back + } else { + d1 = MAX(min_delay_us, round_to_u16(0.01f / v1)); + } + + /// make sure delay has changed a bit at least + if (d1 == delay_us && acc != 0){ + if (acc > 0) + d1--; + else + d1++; + } + + delayMicroseconds(d1); + delay_us = d1; +} + + +uint8_t slow_down_z(uint8_t axis, uint16_t delay_us){ + sm4_do_step(axis); delayMicroseconds(delay_us / 3 * 4); sm4_do_step(Z_AXIS_MASK); delayMicroseconds(delay_us * 2); @@ -370,7 +407,7 @@ uint8_t slow_down_z(uint16_t delay_us){ return 3; } -uint8_t speed_up_z(int16_t &z, uint8_t direction, uint16_t delay_us){ +uint8_t speed_up_z(uint8_t axis, uint16_t delay_us){ sm4_do_step(Z_AXIS_MASK); delayMicroseconds(delay_us * 4); sm4_do_step(Z_AXIS_MASK); @@ -386,6 +423,7 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ int16_t z = _Z; int16_t z_trig; uint16_t line_buffer[32]; + uint16_t current_delay_us = MAX_DELAY; xyzcal_lineXYZ_to(cx - 1024, cy - 1024, min_z, delay_us, 0); for (uint8_t r = 0; r < 32; r++){ ///< Y axis xyzcal_lineXYZ_to(_X, cy - 1024 + r * 64, z, delay_us, 0); @@ -398,10 +436,9 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ /// move up to un-trigger (surpress hysteresis) sm4_set_dir(Z_AXIS, Z_PLUS); - z += speed_up_z(delay_us); + current_delay_us = MAX_DELAY; while (z < max_z && _PINDA){ - sm4_do_step(Z_AXIS_MASK); - delayMicroseconds(delay_us); + accelerate(Z_AXIS_MASK, Z_ACCEL, current_delay_us, delay_us); z++; } int16_t last_top_z = z; From 9b55ff9de1599ebf81cfb3f9853b00f950351e7f Mon Sep 17 00:00:00 2001 From: espr14 Date: Mon, 21 Dec 2020 13:05:26 +0100 Subject: [PATCH 03/18] Acceleration up and down --- Firmware/xyzcal.cpp | 163 +++++++++++++++++++++++++++++--------------- 1 file changed, 107 insertions(+), 56 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index 1c03c4c3f..fc7597b2f 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -36,8 +36,11 @@ #define Z_PLUS 0 #define Z_MINUS 1 -/// 10000 = 1 mm/s +/// Max. jerk in PrusaSlicer, 10000 = 1 mm/s #define MAX_DELAY 10000 +#define MIN_SPEED (0.01f / (MAX_DELAY * 0.000001f)) +/// 200 = 50 mm/s +#define Z_MIN_DELAY 200 #define Z_ACCEL 300 #define XY_ACCEL 1000 @@ -76,6 +79,11 @@ __typeof__ (max) max_ = (max); \ ( a_ < min_ ? min_ : (a_ <= max_ ? a_ : max_)); }) +/// \returns square of the value +#define SQR(a) \ + ({ __typeof__ (a) a_ = (a); \ + (a_ * a_); }) + /// position types typedef int16_t pos_i16_t; typedef long pos_i32_t; @@ -365,9 +373,11 @@ int8_t xyzcal_meassure_pinda_hysterezis(int16_t min_z, int16_t max_z, uint16_t d } #endif //XYZCAL_MEASSURE_PINDA_HYSTEREZIS +/// Accelerate up to max.speed (defined by @min_delay_us) void accelerate(uint8_t axis, int16_t acc, uint16_t &delay_us, uint16_t min_delay_us){ sm4_do_step(axis); + /// keep max speed (avoid extra computation) if (acc > 0 && delay_us == min_delay_us){ delayMicroseconds(delay_us); return; @@ -375,47 +385,66 @@ void accelerate(uint8_t axis, int16_t acc, uint16_t &delay_us, uint16_t min_dela // v1 = v0 + a * t // 0.01 = length of a step - const float d0 = delay_us * 0.000001f; - const float v1 = (0.01f / d0 + acc * d0); - uint16_t d1; - if (v1 <= 0.1f){ - d1 = MAX_DELAY; ///< already too slow so it wants to move back + const float t0 = delay_us * 0.000001f; + const float v1 = (0.01f / t0 + acc * t0); + uint16_t t1; + if (v1 <= 0.16f){ ///< slowest speed convertible to uint16_t delay + t1 = MAX_DELAY; ///< already too slow so it wants to move back } else { - d1 = MAX(min_delay_us, round_to_u16(0.01f / v1)); + /// don't exceed max.speed + t1 = MAX(min_delay_us, round_to_u16(0.01f / v1 * 1000000.f)); } /// make sure delay has changed a bit at least - if (d1 == delay_us && acc != 0){ + if (t1 == delay_us && acc != 0){ if (acc > 0) - d1--; + t1--; else - d1++; + t1++; } - delayMicroseconds(d1); - delay_us = d1; + //DBG(_n("%d "), t1); + + delayMicroseconds(t1); + delay_us = t1; } +void go_and_stop(uint8_t axis, int16_t dec, uint16_t &delay_us, uint16_t &steps){ + if (steps <= 0 || dec <= 0) + return; -uint8_t slow_down_z(uint8_t axis, uint16_t delay_us){ - sm4_do_step(axis); - delayMicroseconds(delay_us / 3 * 4); - sm4_do_step(Z_AXIS_MASK); - delayMicroseconds(delay_us * 2); - sm4_do_step(Z_AXIS_MASK); - delayMicroseconds(delay_us * 4); - return 3; + /// deceleration distance in steps, s = 1/2 v^2 / a + uint16_t s = round_to_u16(100 * 0.5f * SQR(0.01f) / (SQR((float)delay_us) * dec)); + if (steps > s){ + /// go steady + sm4_do_step(axis) + delayMicroseconds(delay_us); + } else { + /// decelerate + accelerate(axis, -dec, &delay_us, min_delay_us); + } + --steps; } -uint8_t speed_up_z(uint8_t axis, uint16_t delay_us){ - sm4_do_step(Z_AXIS_MASK); - delayMicroseconds(delay_us * 4); - sm4_do_step(Z_AXIS_MASK); - delayMicroseconds(delay_us * 2); - sm4_do_step(Z_AXIS_MASK); - delayMicroseconds(delay_us / 3 * 4); - return 3; -} +// uint8_t slow_down_z(uint8_t axis, uint16_t delay_us){ +// sm4_do_step(axis); +// delayMicroseconds(delay_us / 3 * 4); +// sm4_do_step(Z_AXIS_MASK); +// delayMicroseconds(delay_us * 2); +// sm4_do_step(Z_AXIS_MASK); +// delayMicroseconds(delay_us * 4); +// return 3; +// } + +// uint8_t speed_up_z(uint8_t axis, uint16_t delay_us){ +// sm4_do_step(Z_AXIS_MASK); +// delayMicroseconds(delay_us * 4); +// sm4_do_step(Z_AXIS_MASK); +// delayMicroseconds(delay_us * 2); +// sm4_do_step(Z_AXIS_MASK); +// delayMicroseconds(delay_us / 3 * 4); +// return 3; +// } void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t* pixels){ if(!pixels) @@ -423,8 +452,12 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ int16_t z = _Z; int16_t z_trig; uint16_t line_buffer[32]; - uint16_t current_delay_us = MAX_DELAY; + uint16_t current_delay_us = MAX_DELAY; ///< defines current speed xyzcal_lineXYZ_to(cx - 1024, cy - 1024, min_z, delay_us, 0); + uint16_t min_decel_z; + int16_t start_z; + int16_t last_top_z; + for (uint8_t r = 0; r < 32; r++){ ///< Y axis xyzcal_lineXYZ_to(_X, cy - 1024 + r * 64, z, delay_us, 0); for (int8_t d = 0; d < 2; ++d){ ///< direction @@ -433,40 +466,58 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ z = _Z; sm4_set_dir(X_AXIS, d); for (uint8_t c = 0; c < 32; c++){ ///< X axis - + /// move up to un-trigger (surpress hysteresis) sm4_set_dir(Z_AXIS, Z_PLUS); + /// speed up from stop, go half the way current_delay_us = MAX_DELAY; - while (z < max_z && _PINDA){ - accelerate(Z_AXIS_MASK, Z_ACCEL, current_delay_us, delay_us); - z++; + for (start_z = z; z < (max_z + start_z) / 2; ++z){ + if (!_PINDA){ + last_top_z = z; + break; + } + accelerate(Z_AXIS_MASK, Z_ACCEL, current_delay_us, Z_MIN_DELAY); + } + + if(_PINDA){ + uint16_t steps_to_go = MAX(0, max_z - z); + while (_PINDA && z < max_z){ + go_and_stop(Z_AXIS_MASK, Z_ACCEL, current_delay_us, steps_to_go); + ++z; + } + last_top_z = z; + } + /// slow down to stop + while (current_delay_us < MAX_DELAY){ + accelerate(Z_AXIS_MASK, -Z_ACCEL, current_delay_us, Z_MIN_DELAY); + ++z; } - int16_t last_top_z = z; - z += slow_down_z(delay_us); /// move down to trigger sm4_set_dir(Z_AXIS, Z_MINUS); - /// speed up - do (){ - if (z <= min_z || _PINDA) break; - sm4_do_step(Z_AXIS_MASK); - delayMicroseconds(delay_us * 4); - if (z <= min_z || _PINDA) break; - sm4_do_step(Z_AXIS_MASK); - delayMicroseconds(delay_us * 2); - if (z <= min_z || _PINDA) break; - sm4_do_step(Z_AXIS_MASK); - delayMicroseconds(delay_us / 3 * 4); - } while (0); - - while (z > min_z && !_PINDA){ - sm4_do_step(Z_AXIS_MASK); - delayMicroseconds(delay_us); - z--; + current_delay_us = MAX_DELAY; + for (start_z = z; z > (min_z + start_z) / 2; --z){ + if (_PINDA){ + z_trig = z; + break; + } + accelerate(Z_AXIS_MASK, Z_ACCEL, current_delay_us, Z_MIN_DELAY); + } + /// slow down + if(!_PINDA){ + uint16_t steps_to_go = MAX(0, z - min_z); + while (!_PINDA && z > min_z){ + go_and_stop(Z_AXIS_MASK, Z_ACCEL, current_delay_us, steps_to_go); + --z; + } + z_trig = z; + } + /// slow down to stop + while (z > min_z && current_delay_us < MAX_DELAY){ + accelerate(Z_AXIS_MASK, -Z_ACCEL, current_delay_us, Z_MIN_DELAY); + --z; } - z_trig = z; - z -= slow_down_z(delay_us); count_position[2] = z; if (d == 0){ @@ -735,7 +786,7 @@ bool xyzcal_scan_and_process(void){ uint8_t *matrix32 = (uint8_t *)block_buffer; uint16_t *pattern = (uint16_t *)(matrix32 + 32 * 32); - xyzcal_scan_pixels_32x32_Zhop(x, y, z - 72, 2400, 300, matrix32); + xyzcal_scan_pixels_32x32_Zhop(x, y, z - 72, 2400, 600, matrix32); print_image(matrix32); for (uint8_t i = 0; i < 12; i++){ From 27ff6b0057380e467f67867851a5ebcba23961f9 Mon Sep 17 00:00:00 2001 From: espr14 Date: Mon, 21 Dec 2020 13:11:48 +0100 Subject: [PATCH 04/18] Fix build --- Firmware/xyzcal.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index fc7597b2f..18b4de9e1 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -417,11 +417,11 @@ void go_and_stop(uint8_t axis, int16_t dec, uint16_t &delay_us, uint16_t &steps) uint16_t s = round_to_u16(100 * 0.5f * SQR(0.01f) / (SQR((float)delay_us) * dec)); if (steps > s){ /// go steady - sm4_do_step(axis) + sm4_do_step(axis); delayMicroseconds(delay_us); } else { /// decelerate - accelerate(axis, -dec, &delay_us, min_delay_us); + accelerate(axis, -dec, delay_us, delay_us); } --steps; } @@ -454,7 +454,6 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ uint16_t line_buffer[32]; uint16_t current_delay_us = MAX_DELAY; ///< defines current speed xyzcal_lineXYZ_to(cx - 1024, cy - 1024, min_z, delay_us, 0); - uint16_t min_decel_z; int16_t start_z; int16_t last_top_z; @@ -467,6 +466,9 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ sm4_set_dir(X_AXIS, d); for (uint8_t c = 0; c < 32; c++){ ///< X axis + z_trig = min_z; + last_top_z = max_z; + /// move up to un-trigger (surpress hysteresis) sm4_set_dir(Z_AXIS, Z_PLUS); /// speed up from stop, go half the way From 9a59369c77b28ad45dcd7ee82e57e014413545b9 Mon Sep 17 00:00:00 2001 From: espr14 Date: Mon, 21 Dec 2020 15:32:09 +0100 Subject: [PATCH 05/18] Diagonal acceleration --- Firmware/xyzcal.cpp | 36 +++++++++++++++++++++++++++++------- 1 file changed, 29 insertions(+), 7 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index 18b4de9e1..d82877d8a 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -455,7 +455,8 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ uint16_t current_delay_us = MAX_DELAY; ///< defines current speed xyzcal_lineXYZ_to(cx - 1024, cy - 1024, min_z, delay_us, 0); int16_t start_z; - int16_t last_top_z; + // int16_t last_top_z; + uint16_t steps_to_go; for (uint8_t r = 0; r < 32; r++){ ///< Y axis xyzcal_lineXYZ_to(_X, cy - 1024 + r * 64, z, delay_us, 0); @@ -475,7 +476,7 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ current_delay_us = MAX_DELAY; for (start_z = z; z < (max_z + start_z) / 2; ++z){ if (!_PINDA){ - last_top_z = z; + // last_top_z = z; break; } accelerate(Z_AXIS_MASK, Z_ACCEL, current_delay_us, Z_MIN_DELAY); @@ -487,7 +488,7 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ go_and_stop(Z_AXIS_MASK, Z_ACCEL, current_delay_us, steps_to_go); ++z; } - last_top_z = z; + // last_top_z = z; } /// slow down to stop while (current_delay_us < MAX_DELAY){ @@ -508,7 +509,7 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ } /// slow down if(!_PINDA){ - uint16_t steps_to_go = MAX(0, z - min_z); + steps_to_go = MAX(0, z - min_z); while (!_PINDA && z > min_z){ go_and_stop(Z_AXIS_MASK, Z_ACCEL, current_delay_us, steps_to_go); --z; @@ -531,9 +532,30 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ pixels[(uint16_t)r * 32 + (31 - c)] = (uint8_t)MIN((uint32_t)255, ((uint32_t)line_buffer[31 - c] + (z_trig - min_z)) / 2); } - /// move to the next point - xyzcal_lineXYZ_to(((d & 1) ? 1 : -1) * (64 * (16 - c) - 32) + cx, _Y, (last_top_z + z) / 2, delay_us, 0); - z = _Z; + /// move to the next point and move Z up diagonally + current_delay_us = MAX_DELAY; + // const int8_t dir = (d & 1) ? -1 : 1; + const int16_t end_x = ((d & 1) ? 1 : -1) * (64 * (16 - c) - 32) + cx; + const int16_t length_x = ABS(end_x - _X); + const int16_t half_x = length_x / 2; + int16_t x = 0; + + sm4_set_dir(Z_AXIS, Z_PLUS); + /// speed up + for (x = 0; x <= half_x; ++x, ++z){ + accelerate(X_AXIS_MASK | Z_AXIS_MASK, Z_ACCEL, current_delay_us, Z_MIN_DELAY); + } + /// slow down + steps_to_go = length_x - x; + for (; x < length_x; ++x, ++z){ + go_and_stop(X_AXIS_MASK | Z_AXIS_MASK, Z_ACCEL, current_delay_us, steps_to_go); + } + + count_position[0] = end_x; + count_position[2] = z; + + // xyzcal_lineXYZ_to(((d & 1) ? 1 : -1) * (64 * (16 - c) - 32) + cx, _Y, (last_top_z + z) / 2, delay_us, 0); + // z = _Z; } } // DBG(_n("\n\n")); From 3c9168f8f6c5c27c73b8b8442c4a7922b748d737 Mon Sep 17 00:00:00 2001 From: espr14 Date: Mon, 21 Dec 2020 15:52:25 +0100 Subject: [PATCH 06/18] Optimize diagonal --- Firmware/xyzcal.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index d82877d8a..b25be9711 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -539,16 +539,18 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ const int16_t length_x = ABS(end_x - _X); const int16_t half_x = length_x / 2; int16_t x = 0; - + /// don't go up if PINDA not triggered + int8_t axis = _PINDA ? X_AXIS_MASK | Z_AXIS_MASK : X_AXIS_MASK; + sm4_set_dir(Z_AXIS, Z_PLUS); /// speed up for (x = 0; x <= half_x; ++x, ++z){ - accelerate(X_AXIS_MASK | Z_AXIS_MASK, Z_ACCEL, current_delay_us, Z_MIN_DELAY); + accelerate(axis, Z_ACCEL, current_delay_us, Z_MIN_DELAY); } /// slow down steps_to_go = length_x - x; for (; x < length_x; ++x, ++z){ - go_and_stop(X_AXIS_MASK | Z_AXIS_MASK, Z_ACCEL, current_delay_us, steps_to_go); + go_and_stop(axis, Z_ACCEL, current_delay_us, steps_to_go); } count_position[0] = end_x; From de23a845b44d89f803d6a048254095fe27cd5354 Mon Sep 17 00:00:00 2001 From: espr14 Date: Mon, 21 Dec 2020 15:53:51 +0100 Subject: [PATCH 07/18] Remove unused --- Firmware/xyzcal.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index b25be9711..af68ea229 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -455,7 +455,6 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ uint16_t current_delay_us = MAX_DELAY; ///< defines current speed xyzcal_lineXYZ_to(cx - 1024, cy - 1024, min_z, delay_us, 0); int16_t start_z; - // int16_t last_top_z; uint16_t steps_to_go; for (uint8_t r = 0; r < 32; r++){ ///< Y axis @@ -468,7 +467,6 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ for (uint8_t c = 0; c < 32; c++){ ///< X axis z_trig = min_z; - last_top_z = max_z; /// move up to un-trigger (surpress hysteresis) sm4_set_dir(Z_AXIS, Z_PLUS); @@ -476,7 +474,6 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ current_delay_us = MAX_DELAY; for (start_z = z; z < (max_z + start_z) / 2; ++z){ if (!_PINDA){ - // last_top_z = z; break; } accelerate(Z_AXIS_MASK, Z_ACCEL, current_delay_us, Z_MIN_DELAY); @@ -488,7 +485,6 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ go_and_stop(Z_AXIS_MASK, Z_ACCEL, current_delay_us, steps_to_go); ++z; } - // last_top_z = z; } /// slow down to stop while (current_delay_us < MAX_DELAY){ @@ -552,12 +548,8 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ for (; x < length_x; ++x, ++z){ go_and_stop(axis, Z_ACCEL, current_delay_us, steps_to_go); } - count_position[0] = end_x; count_position[2] = z; - - // xyzcal_lineXYZ_to(((d & 1) ? 1 : -1) * (64 * (16 - c) - 32) + cx, _Y, (last_top_z + z) / 2, delay_us, 0); - // z = _Z; } } // DBG(_n("\n\n")); From 744763f0a9f1f20546d09eda6560553497ce434f Mon Sep 17 00:00:00 2001 From: espr14 Date: Mon, 21 Dec 2020 17:34:54 +0100 Subject: [PATCH 08/18] Fix diagonal movement --- Firmware/xyzcal.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index af68ea229..df6f12b30 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -41,7 +41,7 @@ #define MIN_SPEED (0.01f / (MAX_DELAY * 0.000001f)) /// 200 = 50 mm/s #define Z_MIN_DELAY 200 -#define Z_ACCEL 300 +#define Z_ACCEL 5000 #define XY_ACCEL 1000 #define _PI 3.14159265F @@ -536,17 +536,22 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ const int16_t half_x = length_x / 2; int16_t x = 0; /// don't go up if PINDA not triggered - int8_t axis = _PINDA ? X_AXIS_MASK | Z_AXIS_MASK : X_AXIS_MASK; + const bool up = _PINDA; + int8_t axis = up ? X_AXIS_MASK | Z_AXIS_MASK : X_AXIS_MASK; sm4_set_dir(Z_AXIS, Z_PLUS); /// speed up - for (x = 0; x <= half_x; ++x, ++z){ + for (x = 0; x <= half_x; ++x){ accelerate(axis, Z_ACCEL, current_delay_us, Z_MIN_DELAY); + if (up) + ++z; } /// slow down steps_to_go = length_x - x; - for (; x < length_x; ++x, ++z){ + for (; x < length_x; ++x){ go_and_stop(axis, Z_ACCEL, current_delay_us, steps_to_go); + if (up) + ++z; } count_position[0] = end_x; count_position[2] = z; @@ -600,7 +605,7 @@ uint8_t xyzcal_find_pattern_12x12_in_32x32(uint8_t* pixels, uint16_t* pattern, u } // DBG(_n("\n")); } - DBG(_n("max_c=%f max_r=%f max_match=%d pixel\n"), max_c, max_r, max_match); + DBG(_n("max_c=%d max_r=%d max_match=%d pixel\n"), max_c, max_r, max_match); *pc = max_c; *pr = max_r; From 1610c96fc4d49f9d34f3ab6ead9b1b30b176c1e8 Mon Sep 17 00:00:00 2001 From: espr14 Date: Mon, 21 Dec 2020 18:05:52 +0100 Subject: [PATCH 09/18] Fix and report circle divergence --- Firmware/mesh_bed_calibration.cpp | 2 +- Firmware/xyzcal.cpp | 27 ++++----------------------- 2 files changed, 5 insertions(+), 24 deletions(-) diff --git a/Firmware/mesh_bed_calibration.cpp b/Firmware/mesh_bed_calibration.cpp index 12fca582e..897e3a9e6 100644 --- a/Firmware/mesh_bed_calibration.cpp +++ b/Firmware/mesh_bed_calibration.cpp @@ -2271,7 +2271,7 @@ BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level /*} else { // if first iteration failed, count corrected point coordinates as initial - // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew(). + // Use the corrected coordinate, which is a result of find_bed_offset_and_skew(). 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]; diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index df6f12b30..4e55996a3 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -37,7 +37,7 @@ #define Z_MINUS 1 /// Max. jerk in PrusaSlicer, 10000 = 1 mm/s -#define MAX_DELAY 10000 +#define MAX_DELAY 1000 #define MIN_SPEED (0.01f / (MAX_DELAY * 0.000001f)) /// 200 = 50 mm/s #define Z_MIN_DELAY 200 @@ -426,26 +426,6 @@ void go_and_stop(uint8_t axis, int16_t dec, uint16_t &delay_us, uint16_t &steps) --steps; } -// uint8_t slow_down_z(uint8_t axis, uint16_t delay_us){ -// sm4_do_step(axis); -// delayMicroseconds(delay_us / 3 * 4); -// sm4_do_step(Z_AXIS_MASK); -// delayMicroseconds(delay_us * 2); -// sm4_do_step(Z_AXIS_MASK); -// delayMicroseconds(delay_us * 4); -// return 3; -// } - -// uint8_t speed_up_z(uint8_t axis, uint16_t delay_us){ -// sm4_do_step(Z_AXIS_MASK); -// delayMicroseconds(delay_us * 4); -// sm4_do_step(Z_AXIS_MASK); -// delayMicroseconds(delay_us * 2); -// sm4_do_step(Z_AXIS_MASK); -// delayMicroseconds(delay_us / 3 * 4); -// return 3; -// } - void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t* pixels){ if(!pixels) return; @@ -528,7 +508,7 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ pixels[(uint16_t)r * 32 + (31 - c)] = (uint8_t)MIN((uint32_t)255, ((uint32_t)line_buffer[31 - c] + (z_trig - min_z)) / 2); } - /// move to the next point and move Z up diagonally + /// move to the next point and move Z up diagonally (if needed) current_delay_us = MAX_DELAY; // const int8_t dir = (d & 1) ? -1 : 1; const int16_t end_x = ((d & 1) ? 1 : -1) * (64 * (16 - c) - 32) + cx; @@ -829,7 +809,8 @@ bool xyzcal_scan_and_process(void){ 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){ + 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; From c397e9249ae8aa7239d172be7d2194718c4c4c74 Mon Sep 17 00:00:00 2001 From: espr14 Date: Mon, 21 Dec 2020 19:33:36 +0100 Subject: [PATCH 10/18] Restart scanning --- Firmware/xyzcal.cpp | 217 ++++++++++++++++++++++++++------------------ 1 file changed, 131 insertions(+), 86 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index 4e55996a3..d68bfbc1a 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -426,8 +426,41 @@ void go_and_stop(uint8_t axis, int16_t dec, uint16_t &delay_us, uint16_t &steps) --steps; } +/// Count number of zeros on the 32 byte array +/// If the number is less than 16, it changes @min_z so it will be next time +bool more_zeros(uint8_t* pixels, int16_t &min_z){ + uint8_t hist[256]; + uint8_t i = 0; + + /// clear + do { + hist[i] = 0; + } while (i++ < 255); + + /// fill + for (i = 0; i < 32; ++i){ + ++hist[pixels[i]]; + } + + /// already more zeros on the line + if (hist[0] >= 16) + return true; + + /// find threshold + uint8_t sum = 0; + i = 0; + do { + sum += hist[i]; + if (sum >= 16) + break; + } while (i++ < 255); + + min_z += i; + return false; +} + void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t* pixels){ - if(!pixels) + if (!pixels) return; int16_t z = _Z; int16_t z_trig; @@ -436,109 +469,121 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ xyzcal_lineXYZ_to(cx - 1024, cy - 1024, min_z, delay_us, 0); int16_t start_z; uint16_t steps_to_go; + uint8_t restart = 0; ///< restart if needed but just once + int16_t corr_min_z = min_z; ///< shifted min_z if it's too low - for (uint8_t r = 0; r < 32; r++){ ///< Y axis - xyzcal_lineXYZ_to(_X, cy - 1024 + r * 64, z, delay_us, 0); - for (int8_t d = 0; d < 2; ++d){ ///< direction - xyzcal_lineXYZ_to((d & 1) ? (cx + 1024) : (cx - 1024), _Y, min_z, delay_us, 0); + do { + if (restart == 1) + restart = 2; + for (uint8_t r = 0; r < 32; r++){ ///< Y axis + xyzcal_lineXYZ_to(_X, cy - 1024 + r * 64, z, delay_us, 0); + for (int8_t d = 0; d < 2; ++d){ ///< direction + xyzcal_lineXYZ_to((d & 1) ? (cx + 1024) : (cx - 1024), _Y, corr_min_z, delay_us, 0); - z = _Z; - sm4_set_dir(X_AXIS, d); - for (uint8_t c = 0; c < 32; c++){ ///< X axis + z = _Z; + sm4_set_dir(X_AXIS, d); + for (uint8_t c = 0; c < 32; c++){ ///< X axis - z_trig = min_z; + z_trig = corr_min_z; - /// move up to un-trigger (surpress hysteresis) - sm4_set_dir(Z_AXIS, Z_PLUS); - /// speed up from stop, go half the way - current_delay_us = MAX_DELAY; - for (start_z = z; z < (max_z + start_z) / 2; ++z){ - if (!_PINDA){ - break; + /// move up to un-trigger (surpress hysteresis) + sm4_set_dir(Z_AXIS, Z_PLUS); + /// speed up from stop, go half the way + current_delay_us = MAX_DELAY; + for (start_z = z; z < (max_z + start_z) / 2; ++z){ + if (!_PINDA){ + break; + } + accelerate(Z_AXIS_MASK, Z_ACCEL, current_delay_us, Z_MIN_DELAY); } - accelerate(Z_AXIS_MASK, Z_ACCEL, current_delay_us, Z_MIN_DELAY); - } - if(_PINDA){ - uint16_t steps_to_go = MAX(0, max_z - z); - while (_PINDA && z < max_z){ - go_and_stop(Z_AXIS_MASK, Z_ACCEL, current_delay_us, steps_to_go); + if(_PINDA){ + uint16_t steps_to_go = MAX(0, max_z - z); + while (_PINDA && z < max_z){ + go_and_stop(Z_AXIS_MASK, Z_ACCEL, current_delay_us, steps_to_go); + ++z; + } + } + /// slow down to stop + while (current_delay_us < MAX_DELAY){ + accelerate(Z_AXIS_MASK, -Z_ACCEL, current_delay_us, Z_MIN_DELAY); ++z; } - } - /// slow down to stop - while (current_delay_us < MAX_DELAY){ - accelerate(Z_AXIS_MASK, -Z_ACCEL, current_delay_us, Z_MIN_DELAY); - ++z; - } - /// move down to trigger - sm4_set_dir(Z_AXIS, Z_MINUS); - /// speed up - current_delay_us = MAX_DELAY; - for (start_z = z; z > (min_z + start_z) / 2; --z){ - if (_PINDA){ - z_trig = z; - break; + /// move down to trigger + sm4_set_dir(Z_AXIS, Z_MINUS); + /// speed up + current_delay_us = MAX_DELAY; + for (start_z = z; z > (corr_min_z + start_z) / 2; --z){ + if (_PINDA){ + z_trig = z; + break; + } + accelerate(Z_AXIS_MASK, Z_ACCEL, current_delay_us, Z_MIN_DELAY); } - accelerate(Z_AXIS_MASK, Z_ACCEL, current_delay_us, Z_MIN_DELAY); - } - /// slow down - if(!_PINDA){ - steps_to_go = MAX(0, z - min_z); - while (!_PINDA && z > min_z){ - go_and_stop(Z_AXIS_MASK, Z_ACCEL, current_delay_us, steps_to_go); + /// slow down + if(!_PINDA){ + steps_to_go = MAX(0, z - corr_min_z); + while (!_PINDA && z > corr_min_z){ + go_and_stop(Z_AXIS_MASK, Z_ACCEL, current_delay_us, steps_to_go); + --z; + } + z_trig = z; + } + /// slow down to stop + while (z > corr_min_z && current_delay_us < MAX_DELAY){ + accelerate(Z_AXIS_MASK, -Z_ACCEL, current_delay_us, Z_MIN_DELAY); --z; } - z_trig = z; - } - /// slow down to stop - while (z > min_z && current_delay_us < MAX_DELAY){ - accelerate(Z_AXIS_MASK, -Z_ACCEL, current_delay_us, Z_MIN_DELAY); - --z; - } - count_position[2] = z; - if (d == 0){ - line_buffer[c] = (uint16_t)(z_trig - min_z); - } else { - /// data reversed in X - // DBG(_n("%04x"), (line_buffer[31 - c] + (z - min_z)) / 2); - /// save average of both directions - pixels[(uint16_t)r * 32 + (31 - c)] = (uint8_t)MIN((uint32_t)255, ((uint32_t)line_buffer[31 - c] + (z_trig - min_z)) / 2); - } + count_position[2] = z; + if (d == 0){ + line_buffer[c] = (uint16_t)(z_trig - corr_min_z); + } else { + /// data reversed in X + // DBG(_n("%04x"), (line_buffer[31 - c] + (z - corr_min_z)) / 2); + /// save average of both directions + pixels[(uint16_t)r * 32 + (31 - c)] = (uint8_t)MIN((uint32_t)255, ((uint32_t)line_buffer[31 - c] + (z_trig - corr_min_z)) / 2); + } - /// move to the next point and move Z up diagonally (if needed) - current_delay_us = MAX_DELAY; - // const int8_t dir = (d & 1) ? -1 : 1; - const int16_t end_x = ((d & 1) ? 1 : -1) * (64 * (16 - c) - 32) + cx; - const int16_t length_x = ABS(end_x - _X); - const int16_t half_x = length_x / 2; - int16_t x = 0; - /// don't go up if PINDA not triggered - const bool up = _PINDA; - int8_t axis = up ? X_AXIS_MASK | Z_AXIS_MASK : X_AXIS_MASK; + /// move to the next point and move Z up diagonally (if needed) + current_delay_us = MAX_DELAY; + // const int8_t dir = (d & 1) ? -1 : 1; + const int16_t end_x = ((d & 1) ? 1 : -1) * (64 * (16 - c) - 32) + cx; + const int16_t length_x = ABS(end_x - _X); + const int16_t half_x = length_x / 2; + int16_t x = 0; + /// don't go up if PINDA not triggered + const bool up = _PINDA; + int8_t axis = up ? X_AXIS_MASK | Z_AXIS_MASK : X_AXIS_MASK; - sm4_set_dir(Z_AXIS, Z_PLUS); - /// speed up - for (x = 0; x <= half_x; ++x){ - accelerate(axis, Z_ACCEL, current_delay_us, Z_MIN_DELAY); - if (up) - ++z; + sm4_set_dir(Z_AXIS, Z_PLUS); + /// speed up + for (x = 0; x <= half_x; ++x){ + accelerate(axis, Z_ACCEL, current_delay_us, Z_MIN_DELAY); + if (up) + ++z; + } + /// slow down + steps_to_go = length_x - x; + for (; x < length_x; ++x){ + go_and_stop(axis, Z_ACCEL, current_delay_us, steps_to_go); + if (up) + ++z; + } + count_position[0] = end_x; + count_position[2] = z; } - /// slow down - steps_to_go = length_x - x; - for (; x < length_x; ++x){ - go_and_stop(axis, Z_ACCEL, current_delay_us, steps_to_go); - if (up) - ++z; - } - count_position[0] = end_x; - count_position[2] = z; } + if (r == 0 && restart == 0){ + if (!more_zeros(pixels, corr_min_z)) + restart = 1; + } + if (restart == 1) + break; + // DBG(_n("\n\n")); } - // DBG(_n("\n\n")); - } + } while (restart == 1); } /// Returns rate of match From 20659ec818ca3964f7276651781c20b943a1a616 Mon Sep 17 00:00:00 2001 From: espr14 Date: Tue, 22 Dec 2020 12:51:22 +0100 Subject: [PATCH 11/18] Add debug output --- Firmware/xyzcal.cpp | 26 ++++++++++++++++++++++---- 1 file changed, 22 insertions(+), 4 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index d68bfbc1a..86a2440b3 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -427,7 +427,7 @@ void go_and_stop(uint8_t axis, int16_t dec, uint16_t &delay_us, uint16_t &steps) } /// Count number of zeros on the 32 byte array -/// If the number is less than 16, it changes @min_z so it will be next time +/// If the number is less than 16, it moves @min_z up bool more_zeros(uint8_t* pixels, int16_t &min_z){ uint8_t hist[256]; uint8_t i = 0; @@ -441,11 +441,22 @@ bool more_zeros(uint8_t* pixels, int16_t &min_z){ for (i = 0; i < 32; ++i){ ++hist[pixels[i]]; } - + + /// print histogram + i = 0; + DBG(_n("hist: ")); + do { + DBG(_n("%d "), hist[i]); + } while (i++ < 255); + DBG(_n("\n")); + + /// already more zeros on the line - if (hist[0] >= 16) + if (hist[0] >= 16){ + DBG(_n("zeros: %d\n"), hist[0]); return true; - + } + /// find threshold uint8_t sum = 0; i = 0; @@ -455,7 +466,14 @@ bool more_zeros(uint8_t* pixels, int16_t &min_z){ break; } while (i++ < 255); + /// avoid too much zeros + if (sum >= 24) + --i; + + DBG(_n("sum %d, index %d\n"), sum, i); + DBG(_n("min_z %d\n"), min_z); min_z += i; + DBG(_n("min_z %d\n"), min_z); return false; } From 90c8045f7dc0a98e50f7cc95255bc661ed364acc Mon Sep 17 00:00:00 2001 From: espr14 Date: Tue, 22 Dec 2020 13:59:50 +0100 Subject: [PATCH 12/18] Clean the code --- Firmware/xyzcal.cpp | 62 +++++++++++++++++++++++++-------------------- 1 file changed, 35 insertions(+), 27 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index 86a2440b3..f99c0c9b3 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -442,13 +442,13 @@ bool more_zeros(uint8_t* pixels, int16_t &min_z){ ++hist[pixels[i]]; } - /// print histogram - i = 0; - DBG(_n("hist: ")); - do { - DBG(_n("%d "), hist[i]); - } while (i++ < 255); - DBG(_n("\n")); + // /// print histogram + // i = 0; + // DBG(_n("hist: ")); + // do { + // DBG(_n("%d "), hist[i]); + // } while (i++ < 255); + // DBG(_n("\n")); /// already more zeros on the line @@ -471,12 +471,18 @@ bool more_zeros(uint8_t* pixels, int16_t &min_z){ --i; DBG(_n("sum %d, index %d\n"), sum, i); - DBG(_n("min_z %d\n"), min_z); + // DBG(_n("min_z %d\n"), min_z); min_z += i; - DBG(_n("min_z %d\n"), min_z); + // DBG(_n("min_z %d\n"), min_z); return false; } +enum { + RESTART_INIT, + DO_RESTART, + WAS_RESTARTED +}; + void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t* pixels){ if (!pixels) return; @@ -487,22 +493,23 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ xyzcal_lineXYZ_to(cx - 1024, cy - 1024, min_z, delay_us, 0); int16_t start_z; uint16_t steps_to_go; - uint8_t restart = 0; ///< restart if needed but just once - int16_t corr_min_z = min_z; ///< shifted min_z if it's too low + /// restart if needed but just once + /// 0 = init, 1 = do restart, 2 = restart was done, don't restart any more + uint8_t restart = RESTART_INIT; do { - if (restart == 1) - restart = 2; + if (restart == DO_RESTART) + restart = WAS_RESTARTED; for (uint8_t r = 0; r < 32; r++){ ///< Y axis xyzcal_lineXYZ_to(_X, cy - 1024 + r * 64, z, delay_us, 0); for (int8_t d = 0; d < 2; ++d){ ///< direction - xyzcal_lineXYZ_to((d & 1) ? (cx + 1024) : (cx - 1024), _Y, corr_min_z, delay_us, 0); + xyzcal_lineXYZ_to((d & 1) ? (cx + 1024) : (cx - 1024), _Y, min_z, delay_us, 0); z = _Z; sm4_set_dir(X_AXIS, d); for (uint8_t c = 0; c < 32; c++){ ///< X axis - z_trig = corr_min_z; + z_trig = min_z; /// move up to un-trigger (surpress hysteresis) sm4_set_dir(Z_AXIS, Z_PLUS); @@ -532,7 +539,7 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ sm4_set_dir(Z_AXIS, Z_MINUS); /// speed up current_delay_us = MAX_DELAY; - for (start_z = z; z > (corr_min_z + start_z) / 2; --z){ + for (start_z = z; z > (min_z + start_z) / 2; --z){ if (_PINDA){ z_trig = z; break; @@ -541,27 +548,27 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ } /// slow down if(!_PINDA){ - steps_to_go = MAX(0, z - corr_min_z); - while (!_PINDA && z > corr_min_z){ + steps_to_go = MAX(0, z - min_z); + while (!_PINDA && z > min_z){ go_and_stop(Z_AXIS_MASK, Z_ACCEL, current_delay_us, steps_to_go); --z; } z_trig = z; } /// slow down to stop - while (z > corr_min_z && current_delay_us < MAX_DELAY){ + while (z > min_z && current_delay_us < MAX_DELAY){ accelerate(Z_AXIS_MASK, -Z_ACCEL, current_delay_us, Z_MIN_DELAY); --z; } count_position[2] = z; if (d == 0){ - line_buffer[c] = (uint16_t)(z_trig - corr_min_z); + line_buffer[c] = (uint16_t)(z_trig - min_z); } else { /// data reversed in X - // DBG(_n("%04x"), (line_buffer[31 - c] + (z - corr_min_z)) / 2); + // DBG(_n("%04x"), (line_buffer[31 - c] + (z - min_z)) / 2); /// save average of both directions - pixels[(uint16_t)r * 32 + (31 - c)] = (uint8_t)MIN((uint32_t)255, ((uint32_t)line_buffer[31 - c] + (z_trig - corr_min_z)) / 2); + pixels[(uint16_t)r * 32 + (31 - c)] = (uint8_t)MIN((uint32_t)255, ((uint32_t)line_buffer[31 - c] + (z_trig - min_z)) / 2); } /// move to the next point and move Z up diagonally (if needed) @@ -593,15 +600,16 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ count_position[2] = z; } } - if (r == 0 && restart == 0){ - if (!more_zeros(pixels, corr_min_z)) - restart = 1; + /// do this in the first row only + if (r == 0 && restart == RESTART_INIT){ + if (!more_zeros(pixels, min_z)) + restart = DO_RESTART; } - if (restart == 1) + if (restart == DO_RESTART) break; // DBG(_n("\n\n")); } - } while (restart == 1); + } while (restart == DO_RESTART); } /// Returns rate of match From 25d6e247781d6c3590bf0f2721e87941bfe4c7cb Mon Sep 17 00:00:00 2001 From: espr14 Date: Tue, 22 Dec 2020 14:16:20 +0100 Subject: [PATCH 13/18] Add comments --- Firmware/xyzcal.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index f99c0c9b3..22cec6184 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -426,8 +426,9 @@ void go_and_stop(uint8_t axis, int16_t dec, uint16_t &delay_us, uint16_t &steps) --steps; } -/// Count number of zeros on the 32 byte array +/// Count number of zeros in the 32 byte array /// If the number is less than 16, it moves @min_z up +/// Zeros make measuring faster but there cannot be too much bool more_zeros(uint8_t* pixels, int16_t &min_z){ uint8_t hist[256]; uint8_t i = 0; From 8df2eccf45f0d03c5e2baaa3ecf0ed354db75cd6 Mon Sep 17 00:00:00 2001 From: espr14 Date: Tue, 22 Dec 2020 14:36:37 +0100 Subject: [PATCH 14/18] Remove defines and unused vars --- Firmware/xyzcal.cpp | 33 +++++++++++++-------------------- 1 file changed, 13 insertions(+), 20 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index 22cec6184..8c7a3f53b 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -29,22 +29,19 @@ #define _Z ((int16_t)count_position[Z_AXIS]) #define _E ((int16_t)count_position[E_AXIS]) -#define X_PLUS 0 -#define X_MINUS 1 -#define Y_PLUS 0 -#define Y_MINUS 1 -#define Z_PLUS 0 -#define Z_MINUS 1 +const constexpr uint8_t X_PLUS = 0; +const constexpr uint8_t X_MINUS = 1; +const constexpr uint8_t Y_PLUS = 0; +const constexpr uint8_t Y_MINUS = 1; +const constexpr uint8_t Z_PLUS = 0; +const constexpr uint8_t Z_MINUS = 1; /// Max. jerk in PrusaSlicer, 10000 = 1 mm/s -#define MAX_DELAY 1000 -#define MIN_SPEED (0.01f / (MAX_DELAY * 0.000001f)) +const constexpr uint16_t MAX_DELAY = 1000; +const constexpr float MIN_SPEED = 0.01f / (MAX_DELAY * 0.000001f); /// 200 = 50 mm/s -#define Z_MIN_DELAY 200 -#define Z_ACCEL 5000 -#define XY_ACCEL 1000 - -#define _PI 3.14159265F +const constexpr uint16_t Z_MIN_DELAY = 200; +const constexpr uint16_t Z_ACCEL = 5000; /// \returns positive value always #define ABS(a) \ @@ -250,7 +247,6 @@ bool xyzcal_spiral2(int16_t cx, int16_t cy, int16_t z0, int16_t dz, int16_t radi { bool ret = false; float r = 0; //radius - uint8_t n = 0; //point number uint16_t ad = 0; //angle [deg] float ar; //angle [rad] uint8_t dad = 0; //delta angle [deg] @@ -278,11 +274,9 @@ 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)_PI / 180; - float _cos = cos(ar); - float _sin = sin(ar); - int x = (int)(cx + (_cos * r)); - int y = (int)(cy + (_sin * r)); + ar = (ad + rotation)* (float)M_PI / 180; + int x = (int)(cx + (cos(ar) * r)); + int y = (int)(cy + (sin(ar) * r)); int z = (int)(z0 - ((float)((int32_t)dz * ad) / 720)); if (xyzcal_lineXYZ_to(x, y, z, delay_us, check_pinda)) { @@ -290,7 +284,6 @@ bool xyzcal_spiral2(int16_t cx, int16_t cy, int16_t z0, int16_t dz, int16_t radi ret = true; break; } - n++; ad += dad; } if (pad) *pad = ad; From e24a763e46b1b62c2fd76d8d2bfcc50a98875b14 Mon Sep 17 00:00:00 2001 From: espr14 Date: Tue, 22 Dec 2020 14:45:55 +0100 Subject: [PATCH 15/18] Cleanup --- Firmware/xyzcal.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index 8c7a3f53b..3c2a6bb2d 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -29,6 +29,10 @@ #define _Z ((int16_t)count_position[Z_AXIS]) #define _E ((int16_t)count_position[E_AXIS]) +#ifndef M_PI +const constexpr float M_PI = 3.1415926535897932384626433832795f; +#endif + const constexpr uint8_t X_PLUS = 0; const constexpr uint8_t X_MINUS = 1; const constexpr uint8_t Y_PLUS = 0; @@ -447,7 +451,7 @@ bool more_zeros(uint8_t* pixels, int16_t &min_z){ /// already more zeros on the line if (hist[0] >= 16){ - DBG(_n("zeros: %d\n"), hist[0]); + DBG(_n("zeros %d\n"), hist[0]); return true; } @@ -464,7 +468,7 @@ bool more_zeros(uint8_t* pixels, int16_t &min_z){ if (sum >= 24) --i; - DBG(_n("sum %d, index %d\n"), sum, i); + DBG(_n("zeros %d, index %d\n"), sum, i); // DBG(_n("min_z %d\n"), min_z); min_z += i; // DBG(_n("min_z %d\n"), min_z); From 25d138d198d5a532634bbb6ea60133c1570ba48b Mon Sep 17 00:00:00 2001 From: espr14 Date: Tue, 22 Dec 2020 15:10:01 +0100 Subject: [PATCH 16/18] Revert max.speed --- Firmware/xyzcal.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index 3c2a6bb2d..783740e1d 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -858,7 +858,7 @@ bool xyzcal_scan_and_process(void){ uint8_t *matrix32 = (uint8_t *)block_buffer; uint16_t *pattern = (uint16_t *)(matrix32 + 32 * 32); - xyzcal_scan_pixels_32x32_Zhop(x, y, z - 72, 2400, 600, matrix32); + xyzcal_scan_pixels_32x32_Zhop(x, y, z - 72, 2400, 200, matrix32); print_image(matrix32); for (uint8_t i = 0; i < 12; i++){ From 979525f0283dad989b356eecb56d9078a2696498 Mon Sep 17 00:00:00 2001 From: espr14 Date: Tue, 22 Dec 2020 19:34:12 +0100 Subject: [PATCH 17/18] Add more restarts --- Firmware/xyzcal.cpp | 25 ++++++++----------------- 1 file changed, 8 insertions(+), 17 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index 783740e1d..5c21b4fbb 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -475,12 +475,6 @@ bool more_zeros(uint8_t* pixels, int16_t &min_z){ return false; } -enum { - RESTART_INIT, - DO_RESTART, - WAS_RESTARTED -}; - void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t* pixels){ if (!pixels) return; @@ -491,13 +485,10 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ xyzcal_lineXYZ_to(cx - 1024, cy - 1024, min_z, delay_us, 0); int16_t start_z; uint16_t steps_to_go; - /// restart if needed but just once - /// 0 = init, 1 = do restart, 2 = restart was done, don't restart any more - uint8_t restart = RESTART_INIT; + /// available restarts (if needed) + uint8_t restarts = 2; do { - if (restart == DO_RESTART) - restart = WAS_RESTARTED; for (uint8_t r = 0; r < 32; r++){ ///< Y axis xyzcal_lineXYZ_to(_X, cy - 1024 + r * 64, z, delay_us, 0); for (int8_t d = 0; d < 2; ++d){ ///< direction @@ -598,16 +589,16 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ count_position[2] = z; } } - /// do this in the first row only - if (r == 0 && restart == RESTART_INIT){ - if (!more_zeros(pixels, min_z)) - restart = DO_RESTART; + /// do the min_z addjusting in the first row only + if (r == 0 && restarts){ + if (more_zeros(pixels, min_z)) + restarts = 0; } - if (restart == DO_RESTART) + if (restarts) break; // DBG(_n("\n\n")); } - } while (restart == DO_RESTART); + } while (restarts--); } /// Returns rate of match From e6eb3be2472538694827301d516a757be5be7924 Mon Sep 17 00:00:00 2001 From: espr14 Date: Wed, 23 Dec 2020 16:56:04 +0100 Subject: [PATCH 18/18] Set jerk = 1, accel. = 1000 --- Firmware/xyzcal.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index 5c21b4fbb..25863ae8e 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -41,11 +41,11 @@ const constexpr uint8_t Z_PLUS = 0; const constexpr uint8_t Z_MINUS = 1; /// Max. jerk in PrusaSlicer, 10000 = 1 mm/s -const constexpr uint16_t MAX_DELAY = 1000; +const constexpr uint16_t MAX_DELAY = 10000; const constexpr float MIN_SPEED = 0.01f / (MAX_DELAY * 0.000001f); /// 200 = 50 mm/s const constexpr uint16_t Z_MIN_DELAY = 200; -const constexpr uint16_t Z_ACCEL = 5000; +const constexpr uint16_t Z_ACCEL = 1000; /// \returns positive value always #define ABS(a) \