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 2130b223e..25863ae8e 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -29,14 +29,23 @@ #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 +#ifndef M_PI +const constexpr float M_PI = 3.1415926535897932384626433832795f; +#endif -#define _PI 3.14159265F +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 +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 = 1000; /// \returns positive value always #define ABS(a) \ @@ -71,6 +80,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; @@ -237,7 +251,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] @@ -265,11 +278,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)) { @@ -277,7 +288,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; @@ -360,60 +370,235 @@ 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; + } + + // v1 = v0 + a * t + // 0.01 = length of a step + 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 { + /// 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 (t1 == delay_us && acc != 0){ + if (acc > 0) + t1--; + else + t1++; + } + + //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; + + /// 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, delay_us); + } + --steps; +} + +/// 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; + + /// clear + do { + hist[i] = 0; + } while (i++ < 255); + + /// fill + 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){ + DBG(_n("zeros %d\n"), hist[0]); + return true; + } + + /// find threshold + uint8_t sum = 0; + i = 0; + do { + sum += hist[i]; + if (sum >= 16) + break; + } while (i++ < 255); + + /// avoid too much zeros + if (sum >= 24) + --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); + 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; uint16_t line_buffer[32]; - xyzcal_lineXYZ_to(cx, cy, 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); + 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; + uint16_t steps_to_go; + /// available restarts (if needed) + uint8_t restarts = 2; - xyzcal_lineXYZ_to((d & 1) ? (cx + 1024) : (cx - 1024), _Y, min_z, delay_us, 0); + do { + 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); - 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); - while (z < max_z && _PINDA){ - sm4_do_step(Z_AXIS_MASK); - delayMicroseconds(delay_us); - z++; - } - int16_t last_top_z = z; - - /// move down to trigger - sm4_set_dir(Z_AXIS, Z_MINUS); - while (z > min_z && !_PINDA){ - sm4_do_step(Z_AXIS_MASK); - delayMicroseconds(delay_us); - z--; - } - - count_position[2] = z; - if (d == 0){ - line_buffer[c] = (uint16_t)(z - 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); - } - - /// 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; + sm4_set_dir(X_AXIS, d); + for (uint8_t c = 0; c < 32; c++){ ///< X axis + + z_trig = 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; + } + 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; + } + } + /// 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; + } + 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); + --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); + } + + /// 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; + } + /// 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; + } } + /// do the min_z addjusting in the first row only + if (r == 0 && restarts){ + if (more_zeros(pixels, min_z)) + restarts = 0; + } + if (restarts) + break; + // DBG(_n("\n\n")); } - // DBG(_n("\n\n")); - } + } while (restarts--); } /// Returns rate of match @@ -460,7 +645,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; @@ -664,7 +849,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, 200, matrix32); print_image(matrix32); for (uint8_t i = 0; i < 12; i++){ @@ -684,7 +869,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;