From e86ba5e26d8d15a28cdd7c9d96b908cb0fdf4cfd Mon Sep 17 00:00:00 2001 From: espr14 Date: Mon, 28 Dec 2020 16:45:39 +0100 Subject: [PATCH 01/15] Scan only once --- Firmware/xyzcal.cpp | 209 +++++++++++++++++++++++++------------------- 1 file changed, 121 insertions(+), 88 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index 7ad6dd757..75d358b5e 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -226,6 +226,9 @@ uint16_t xyzcal_calc_delay(uint16_t, uint16_t) #endif //SM4_ACCEL_TEST /// Moves printer to absolute position [x,y,z] defined in integer position system +/// check_pinda == 0: ordinary move +/// check_pinda == 1: stop when PINDA triggered +/// check_pinda == -1: stop when PINDA untriggered bool xyzcal_lineXYZ_to(int16_t x, int16_t y, int16_t z, uint16_t delay_us, int8_t check_pinda) { // DBG(_n("xyzcal_lineXYZ_to x=%d y=%d z=%d check=%d\n"), x, y, z, check_pinda); @@ -370,6 +373,26 @@ int8_t xyzcal_meassure_pinda_hysterezis(int16_t min_z, int16_t max_z, uint16_t d } #endif //XYZCAL_MEASSURE_PINDA_HYSTEREZIS +void print_hysterezis(int16_t min_z, int16_t max_z, int16_t step){ + int16_t delay_us = 600; + int16_t trigger = 0; + int16_t untrigger = 0; + DBG(_n("Hysterezis\n")); + + xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 0); + + for (int16_t z = min_z; z <= max_z; z += step){ + xyzcal_lineXYZ_to(_X, _Y, z, delay_us, -1); + untrigger = _Z; + xyzcal_lineXYZ_to(_X, _Y, z, delay_us, 0); + xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 1); + trigger = _Z; + //xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 0); + + DBG(_n("min, trigger, untrigger, max: [%d %d %d %d]\n"), _Z, trigger, untrigger, z); + } +} + /// 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); @@ -423,7 +446,19 @@ void go_and_stop(uint8_t axis, int16_t dec, uint16_t &delay_us, uint16_t &steps) --steps; } -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){ +/// \returns steps done +uint16_t stop_smoothly(uint8_t axis, int16_t dec, uint16_t &delay_us, uint16_t min_delay_us){ + if (dec <= 0) + return 0; + uint16_t steps = 0; + while (delay_us < MAX_DELAY){ + accelerate(axis, -dec, delay_us, min_delay_us); + ++steps; + } + return steps; +} + +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; @@ -436,104 +471,103 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ 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); + int8_t d = r % 2 ? 1 : 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 = _Z; + sm4_set_dir(X_AXIS, d); + for (uint8_t c = 0; c < 32; c++){ ///< X axis - z_trig = min_z; + 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); + /// 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); + if (_PINDA){ + 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; } + } + z += stop_smoothly(Z_AXIS_MASK, Z_ACCEL, current_delay_us, Z_MIN_DELAY); - /// 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; - } + /// 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; } - /// 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); + 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; } - - 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; + z_trig = z; } - } + /// slow down to stop but not lower than min_z + 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); + // } + + pixels[(uint16_t)r * 32 + (r % 2 ? (31 - c) : c)] = (uint8_t)MIN((uint32_t)255, (uint32_t)z_trig - min_z); + + /// 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; + } + // } // DBG(_n("\n\n")); } } @@ -846,8 +880,7 @@ bool xyzcal_find_bed_induction_sensor_point_xy(void){ xyzcal_lineXYZ_to(x, y, z, 200, 0); if (xyzcal_searchZ()){ - int16_t z = _Z; - xyzcal_lineXYZ_to(x, y, z, 200, 0); + xyzcal_lineXYZ_to(x, y, _Z, 200, 0); ret = xyzcal_scan_and_process(); } xyzcal_meassure_leave(); From c11e8654daa24e1935df85a4aa738ad6fcf4a854 Mon Sep 17 00:00:00 2001 From: espr14 Date: Tue, 29 Dec 2020 12:39:35 +0100 Subject: [PATCH 02/15] Revert both directions, refactor code --- Firmware/xyzcal.cpp | 258 +++++++++++++++++++++++++++----------------- 1 file changed, 157 insertions(+), 101 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index 75d358b5e..5563d34b5 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -29,6 +29,11 @@ #define _Z ((int16_t)count_position[Z_AXIS]) #define _E ((int16_t)count_position[E_AXIS]) +#define _X_ (count_position[X_AXIS]) +#define _Y_ (count_position[Y_AXIS]) +#define _Z_ (count_position[Z_AXIS]) +#define _E_ (count_position[E_AXIS]) + #ifndef M_PI const constexpr float M_PI = 3.1415926535897932384626433832795f; #endif @@ -40,6 +45,13 @@ const constexpr uint8_t Y_MINUS = 1; const constexpr uint8_t Z_PLUS = 0; const constexpr uint8_t Z_MINUS = 1; +const constexpr uint8_t X_PLUS_MASK = 0; +const constexpr uint8_t X_MINUS_MASK = X_AXIS_MASK; +const constexpr uint8_t Y_PLUS_MASK = 0; +const constexpr uint8_t Y_MINUS_MASK = Y_AXIS_MASK; +const constexpr uint8_t Z_PLUS_MASK = 0; +const constexpr uint8_t Z_MINUS_MASK = Z_AXIS_MASK; + /// 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); @@ -393,8 +405,28 @@ void print_hysterezis(int16_t min_z, int16_t max_z, int16_t step){ } } +void update_position_1_step(uint8_t axis, uint8_t dir){ + int8_t add = dir ? -1 : 1; + if (axis & X_AXIS_MASK) + _X_ += dir & X_AXIS_MASK ? -1 : 1; + if (axis & Y_AXIS_MASK) + _Y_ += dir & Y_AXIS_MASK ? -1 : 1; + if (axis & Z_AXIS_MASK) + _Z_ += dir & Z_AXIS_MASK ? -1 : 1; +} + +void set_axis_dir(uint8_t axis, uint8_t dir){ + if (axis & X_AXIS_MASK) + sm4_set_dir(X_AXIS_MASK, dir & X_AXIS_MASK); + if (axis & Y_AXIS_MASK) + sm4_set_dir(Y_AXIS_MASK, dir & Y_AXIS_MASK); + if (axis & Z_AXIS_MASK) + sm4_set_dir(Z_AXIS_MASK, dir & Z_AXIS_MASK); +} + /// 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){ +/// does not update global positions +void accelerate_1_step(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) @@ -429,9 +461,24 @@ void accelerate(uint8_t axis, int16_t acc, uint16_t &delay_us, uint16_t min_dela delay_us = t1; } -void go_and_stop(uint8_t axis, int16_t dec, uint16_t &delay_us, uint16_t &steps){ +/// Goes defined number of steps while accelerating +/// updates global positions +void accelerate(uint8_t axis, uint8_t dir, int16_t acc, uint16_t &delay_us, uint16_t min_delay_us, uint16_t steps){ + set_axis_dir(axis, dir); + while (steps--){ + accelerate_1_step(axis, acc, delay_us, min_delay_us); + update_position_1_step(axis, dir); + } +} + +/// keeps speed and then it decelerates to a complete stop (if possible) +/// it goes defined number of steps +/// returns after each step +/// \returns true if step was done +/// does not update global positions +bool go_and_stop_1_step(uint8_t axis, int16_t dec, uint16_t &delay_us, uint16_t &steps){ if (steps <= 0 || dec <= 0) - return; + return false; /// 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)); @@ -441,27 +488,37 @@ void go_and_stop(uint8_t axis, int16_t dec, uint16_t &delay_us, uint16_t &steps) delayMicroseconds(delay_us); } else { /// decelerate - accelerate(axis, -dec, delay_us, delay_us); + accelerate_1_step(axis, -dec, delay_us, delay_us); } --steps; + return true; } -/// \returns steps done -uint16_t stop_smoothly(uint8_t axis, int16_t dec, uint16_t &delay_us, uint16_t min_delay_us){ - if (dec <= 0) - return 0; - uint16_t steps = 0; - while (delay_us < MAX_DELAY){ - accelerate(axis, -dec, delay_us, min_delay_us); - ++steps; +/// \param dir sets direction of movement +/// updates global positions +void go_and_stop(uint8_t axis, uint8_t dir, int16_t dec, uint16_t &delay_us, uint16_t steps){ + set_axis_dir(axis, dir); + while (go_and_stop_1_step(axis, dec, delay_us, steps)){ + update_position_1_step(axis, dir); + } +} + +/// goes all the way to stop +/// \returns steps done +/// updates global positions +void stop_smoothly(uint8_t axis, uint8_t dir, int16_t dec, uint16_t &delay_us){ + if (dec <= 0) + return; + set_axis_dir(axis, dir); + while (delay_us < MAX_DELAY){ + accelerate_1_step(axis, -dec, delay_us, delay_us); + update_position_1_step(axis, dir); } - return steps; } 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]; uint16_t current_delay_us = MAX_DELAY; ///< defines current speed @@ -470,104 +527,103 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ 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); - int8_t d = r % 2 ? 1 : 0; - xyzcal_lineXYZ_to((d & 1) ? (cx + 1024) : (cx - 1024), _Y, min_z, delay_us, 0); + xyzcal_lineXYZ_to(_X, cy - 1024 + r * 64, _Z, delay_us, 0); + for (uint8_t d = 0; d < 2; ++d){ + xyzcal_lineXYZ_to((d & 1) ? (cx + 1024) : (cx - 1024), _Y, min_z, delay_us, 0); + sm4_set_dir(X_AXIS, d); + for (uint8_t c = 0; c < 32; c++){ ///< X axis + z_trig = min_z; - 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; + /// 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_1_step(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){ - 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; - } - } - z += stop_smoothly(Z_AXIS_MASK, Z_ACCEL, current_delay_us, Z_MIN_DELAY); - - /// 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; + steps_to_go = MAX(0, max_z - _Z); + while (_PINDA && _Z < max_z){ + go_and_stop_1_step(Z_AXIS_MASK, Z_ACCEL, current_delay_us, steps_to_go); + ++_Z_; + } } - 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; + stop_smoothly(Z_AXIS_MASK, Z_PLUS_MASK, Z_ACCEL, current_delay_us); + + /// 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_1_step(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_1_step(Z_AXIS_MASK, Z_ACCEL, current_delay_us, steps_to_go); + --_Z_; + } + z_trig = _Z; + } + /// slow down to stop but not lower than min_z + while (_Z > min_z && current_delay_us < MAX_DELAY){ + accelerate_1_step(Z_AXIS_MASK, -Z_ACCEL, current_delay_us, Z_MIN_DELAY); + --_Z_; } - z_trig = z; - } - /// slow down to stop but not lower than min_z - 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); + } - // 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; + uint8_t dir = Z_PLUS_MASK | (d & 1 ? X_MINUS_MASK : X_PLUS_MASK); - pixels[(uint16_t)r * 32 + (r % 2 ? (31 - c) : c)] = (uint8_t)MIN((uint32_t)255, (uint32_t)z_trig - min_z); + // sm4_set_dir(Z_AXIS, Z_PLUS); + /// speed up + accelerate(axis, dir, Z_ACCEL, current_delay_us, Z_MIN_DELAY, half_x); + + // for (x = 0; x <= half_x; ++x) + // { + // accelerate_1_step(axis, Z_ACCEL, current_delay_us, Z_MIN_DELAY); + // if (up) + // ++_Z_; + // } + + /// slow down + go_and_stop(axis, dir, Z_ACCEL, current_delay_us, length_x - half_x); - /// 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; + // steps_to_go = length_x - x; + // for (; x < length_x; ++x){ + // go_and_stop_1_step(axis, Z_ACCEL, current_delay_us, steps_to_go); + // if (up) + // ++_Z_; + // } + // count_position[0] = end_x; } - /// 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; - } - // } + } // DBG(_n("\n\n")); } } From b6e78bf900277763cfe4fd635f3f3769b9dcb0b7 Mon Sep 17 00:00:00 2001 From: espr14 Date: Tue, 29 Dec 2020 14:08:30 +0100 Subject: [PATCH 03/15] Fix direction --- Firmware/xyzcal.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index 5563d34b5..8d5519d28 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -601,7 +601,7 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ int8_t axis = up ? X_AXIS_MASK | Z_AXIS_MASK : X_AXIS_MASK; uint8_t dir = Z_PLUS_MASK | (d & 1 ? X_MINUS_MASK : X_PLUS_MASK); - // sm4_set_dir(Z_AXIS, Z_PLUS); + sm4_set_dir(Z_AXIS, Z_PLUS); /// speed up accelerate(axis, dir, Z_ACCEL, current_delay_us, Z_MIN_DELAY, half_x); @@ -615,7 +615,7 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ /// slow down go_and_stop(axis, dir, Z_ACCEL, current_delay_us, length_x - half_x); - // steps_to_go = length_x - x; + // steps_to_go = length_x - half_x; // for (; x < length_x; ++x){ // go_and_stop_1_step(axis, Z_ACCEL, current_delay_us, steps_to_go); // if (up) From 6461206e08bc776ecfc79b355d409e681923b990 Mon Sep 17 00:00:00 2001 From: espr14 Date: Tue, 29 Dec 2020 14:19:05 +0100 Subject: [PATCH 04/15] Fix direction setting --- Firmware/xyzcal.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index 8d5519d28..bc4e8dc86 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -417,11 +417,11 @@ void update_position_1_step(uint8_t axis, uint8_t dir){ void set_axis_dir(uint8_t axis, uint8_t dir){ if (axis & X_AXIS_MASK) - sm4_set_dir(X_AXIS_MASK, dir & X_AXIS_MASK); + sm4_set_dir(X_AXIS, dir & X_AXIS_MASK); if (axis & Y_AXIS_MASK) - sm4_set_dir(Y_AXIS_MASK, dir & Y_AXIS_MASK); + sm4_set_dir(Y_AXIS, dir & Y_AXIS_MASK); if (axis & Z_AXIS_MASK) - sm4_set_dir(Z_AXIS_MASK, dir & Z_AXIS_MASK); + sm4_set_dir(Z_AXIS, dir & Z_AXIS_MASK); } /// Accelerate up to max.speed (defined by @min_delay_us) @@ -601,7 +601,7 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ int8_t axis = up ? X_AXIS_MASK | Z_AXIS_MASK : X_AXIS_MASK; uint8_t dir = Z_PLUS_MASK | (d & 1 ? X_MINUS_MASK : X_PLUS_MASK); - sm4_set_dir(Z_AXIS, Z_PLUS); + // sm4_set_dir(Z_AXIS, Z_PLUS); /// speed up accelerate(axis, dir, Z_ACCEL, current_delay_us, Z_MIN_DELAY, half_x); From a01dfe26fe8fad4f1b7f11d441639ecf01d7f66f Mon Sep 17 00:00:00 2001 From: espr14 Date: Tue, 29 Dec 2020 17:13:11 +0100 Subject: [PATCH 05/15] Add Manhattan movement --- Firmware/xyzcal.cpp | 74 ++++++++++++++++++++++++++------------------- 1 file changed, 43 insertions(+), 31 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index bc4e8dc86..8c749aaf5 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -406,7 +406,6 @@ void print_hysterezis(int16_t min_z, int16_t max_z, int16_t step){ } void update_position_1_step(uint8_t axis, uint8_t dir){ - int8_t add = dir ? -1 : 1; if (axis & X_AXIS_MASK) _X_ += dir & X_AXIS_MASK ? -1 : 1; if (axis & Y_AXIS_MASK) @@ -424,10 +423,19 @@ void set_axis_dir(uint8_t axis, uint8_t dir){ sm4_set_dir(Z_AXIS, dir & Z_AXIS_MASK); } +void go_step(uint8_t axis){ + if (axis & X_AXIS_MASK) + sm4_do_step(X_AXIS); + if (axis & Y_AXIS_MASK) + sm4_do_step(Y_AXIS); + if (axis & Z_AXIS_MASK) + sm4_do_step(Z_AXIS); +} + /// Accelerate up to max.speed (defined by @min_delay_us) /// does not update global positions void accelerate_1_step(uint8_t axis, int16_t acc, uint16_t &delay_us, uint16_t min_delay_us){ - sm4_do_step(axis); + go_step(axis); /// keep max speed (avoid extra computation) if (acc > 0 && delay_us == min_delay_us){ @@ -484,7 +492,7 @@ bool go_and_stop_1_step(uint8_t axis, int16_t dec, uint16_t &delay_us, uint16_t 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); + go_step(axis); delayMicroseconds(delay_us); } else { /// decelerate @@ -516,20 +524,44 @@ void stop_smoothly(uint8_t axis, uint8_t dir, int16_t dec, uint16_t &delay_us){ } } +void go_start_stop(uint8_t axis, uint8_t dir, int16_t acc, uint16_t min_delay_us, uint16_t steps){ + uint16_t current_delay_us = MAX_DELAY; + const uint16_t half = steps / 2; + accelerate(axis, dir, acc, current_delay_us, min_delay_us, half); + go_and_stop(axis, dir, -acc, current_delay_us, steps - half); +} + +/// moves X, Y, Z one after each other +/// starts and ends at 0 speed +void go_manhattan(int16_t x, int16_t y, int16_t z, int16_t acc, uint16_t min_delay_us){ + int32_t length; + + DBG(_n("x %d -> %d\n"), x, _X); + length = x - _X; + go_start_stop(X_AXIS_MASK, length < 0 ? X_MINUS_MASK : X_PLUS_MASK, acc, min_delay_us, ABS(length)); + + DBG(_n("y %d -> %d\n"), y, _Y); + length = y - _Y; + go_start_stop(Y_AXIS_MASK, length < 0 ? Y_MINUS_MASK : Y_PLUS_MASK, acc, min_delay_us, ABS(length)); + + DBG(_n("z %d -> %d\n"), z, _Z); + length = z - _Z; + go_start_stop(Z_AXIS_MASK, length < 0 ? Z_MINUS_MASK : Z_PLUS_MASK, acc, min_delay_us, ABS(length)); +} + 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_trig; 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); int16_t start_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); for (uint8_t d = 0; d < 2; ++d){ - xyzcal_lineXYZ_to((d & 1) ? (cx + 1024) : (cx - 1024), _Y, min_z, delay_us, 0); + go_manhattan((d & 1) ? (cx + 1024) : (cx - 1024), cy - 1024 + r * 64, _Z, Z_ACCEL, Z_MIN_DELAY); + // xyzcal_lineXYZ_to((d & 1) ? (cx + 1024) : (cx - 1024), cy - 1024 + r * 64, _Z, delay_us, 0); sm4_set_dir(X_AXIS, d); for (uint8_t c = 0; c < 32; c++){ ///< X axis z_trig = min_z; @@ -585,7 +617,7 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ } else { /// data reversed in X // DBG(_n("%04x"), (line_buffer[31 - c] + (z - min_z)) / 2); - /// save average of both directions + /// save average of both directions (filters effect of hysteresis) pixels[(uint16_t)r * 32 + (31 - c)] = (uint8_t)MIN((uint32_t)255, ((uint32_t)line_buffer[31 - c] + (z_trig - min_z)) / 2); } @@ -595,33 +627,13 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ 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 + /// don't go up if PINDA not triggered (optimization) const bool up = _PINDA; - int8_t axis = up ? X_AXIS_MASK | Z_AXIS_MASK : X_AXIS_MASK; - uint8_t dir = Z_PLUS_MASK | (d & 1 ? X_MINUS_MASK : X_PLUS_MASK); + const int8_t axis = up ? X_AXIS_MASK | Z_AXIS_MASK : X_AXIS_MASK; + const uint8_t dir = Z_PLUS_MASK | (d & 1 ? X_MINUS_MASK : X_PLUS_MASK); - // sm4_set_dir(Z_AXIS, Z_PLUS); - /// speed up - accelerate(axis, dir, Z_ACCEL, current_delay_us, Z_MIN_DELAY, half_x); - - // for (x = 0; x <= half_x; ++x) - // { - // accelerate_1_step(axis, Z_ACCEL, current_delay_us, Z_MIN_DELAY); - // if (up) - // ++_Z_; - // } - - /// slow down + accelerate(axis, dir, Z_ACCEL, current_delay_us, Z_MIN_DELAY, half_x); go_and_stop(axis, dir, Z_ACCEL, current_delay_us, length_x - half_x); - - // steps_to_go = length_x - half_x; - // for (; x < length_x; ++x){ - // go_and_stop_1_step(axis, Z_ACCEL, current_delay_us, steps_to_go); - // if (up) - // ++_Z_; - // } - // count_position[0] = end_x; } } // DBG(_n("\n\n")); From 922769cefb64ab2b6afaba4991abbe86a18bc9d9 Mon Sep 17 00:00:00 2001 From: espr14 Date: Tue, 29 Dec 2020 17:14:18 +0100 Subject: [PATCH 06/15] axis -> axes --- Firmware/xyzcal.cpp | 56 ++++++++++++++++++++++----------------------- 1 file changed, 28 insertions(+), 28 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index 8c749aaf5..411b48908 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -414,28 +414,28 @@ void update_position_1_step(uint8_t axis, uint8_t dir){ _Z_ += dir & Z_AXIS_MASK ? -1 : 1; } -void set_axis_dir(uint8_t axis, uint8_t dir){ - if (axis & X_AXIS_MASK) +void set_axes_dir(uint8_t axes, uint8_t dir){ + if (axes & X_AXIS_MASK) sm4_set_dir(X_AXIS, dir & X_AXIS_MASK); - if (axis & Y_AXIS_MASK) + if (axes & Y_AXIS_MASK) sm4_set_dir(Y_AXIS, dir & Y_AXIS_MASK); - if (axis & Z_AXIS_MASK) + if (axes & Z_AXIS_MASK) sm4_set_dir(Z_AXIS, dir & Z_AXIS_MASK); } -void go_step(uint8_t axis){ - if (axis & X_AXIS_MASK) +void go_step(uint8_t axes){ + if (axes & X_AXIS_MASK) sm4_do_step(X_AXIS); - if (axis & Y_AXIS_MASK) + if (axes & Y_AXIS_MASK) sm4_do_step(Y_AXIS); - if (axis & Z_AXIS_MASK) + if (axes & Z_AXIS_MASK) sm4_do_step(Z_AXIS); } /// Accelerate up to max.speed (defined by @min_delay_us) /// does not update global positions -void accelerate_1_step(uint8_t axis, int16_t acc, uint16_t &delay_us, uint16_t min_delay_us){ - go_step(axis); +void accelerate_1_step(uint8_t axes, int16_t acc, uint16_t &delay_us, uint16_t min_delay_us){ + go_step(axes); /// keep max speed (avoid extra computation) if (acc > 0 && delay_us == min_delay_us){ @@ -471,11 +471,11 @@ void accelerate_1_step(uint8_t axis, int16_t acc, uint16_t &delay_us, uint16_t m /// Goes defined number of steps while accelerating /// updates global positions -void accelerate(uint8_t axis, uint8_t dir, int16_t acc, uint16_t &delay_us, uint16_t min_delay_us, uint16_t steps){ - set_axis_dir(axis, dir); +void accelerate(uint8_t axes, uint8_t dir, int16_t acc, uint16_t &delay_us, uint16_t min_delay_us, uint16_t steps){ + set_axes_dir(axes, dir); while (steps--){ - accelerate_1_step(axis, acc, delay_us, min_delay_us); - update_position_1_step(axis, dir); + accelerate_1_step(axes, acc, delay_us, min_delay_us); + update_position_1_step(axes, dir); } } @@ -484,7 +484,7 @@ void accelerate(uint8_t axis, uint8_t dir, int16_t acc, uint16_t &delay_us, uint /// returns after each step /// \returns true if step was done /// does not update global positions -bool go_and_stop_1_step(uint8_t axis, int16_t dec, uint16_t &delay_us, uint16_t &steps){ +bool go_and_stop_1_step(uint8_t axes, int16_t dec, uint16_t &delay_us, uint16_t &steps){ if (steps <= 0 || dec <= 0) return false; @@ -492,11 +492,11 @@ bool go_and_stop_1_step(uint8_t axis, int16_t dec, uint16_t &delay_us, uint16_t uint16_t s = round_to_u16(100 * 0.5f * SQR(0.01f) / (SQR((float)delay_us) * dec)); if (steps > s){ /// go steady - go_step(axis); + go_step(axes); delayMicroseconds(delay_us); } else { /// decelerate - accelerate_1_step(axis, -dec, delay_us, delay_us); + accelerate_1_step(axes, -dec, delay_us, delay_us); } --steps; return true; @@ -504,31 +504,31 @@ bool go_and_stop_1_step(uint8_t axis, int16_t dec, uint16_t &delay_us, uint16_t /// \param dir sets direction of movement /// updates global positions -void go_and_stop(uint8_t axis, uint8_t dir, int16_t dec, uint16_t &delay_us, uint16_t steps){ - set_axis_dir(axis, dir); - while (go_and_stop_1_step(axis, dec, delay_us, steps)){ - update_position_1_step(axis, dir); +void go_and_stop(uint8_t axes, uint8_t dir, int16_t dec, uint16_t &delay_us, uint16_t steps){ + set_axes_dir(axes, dir); + while (go_and_stop_1_step(axes, dec, delay_us, steps)){ + update_position_1_step(axes, dir); } } /// goes all the way to stop /// \returns steps done /// updates global positions -void stop_smoothly(uint8_t axis, uint8_t dir, int16_t dec, uint16_t &delay_us){ +void stop_smoothly(uint8_t axes, uint8_t dir, int16_t dec, uint16_t &delay_us){ if (dec <= 0) return; - set_axis_dir(axis, dir); + set_axes_dir(axes, dir); while (delay_us < MAX_DELAY){ - accelerate_1_step(axis, -dec, delay_us, delay_us); - update_position_1_step(axis, dir); + accelerate_1_step(axes, -dec, delay_us, delay_us); + update_position_1_step(axes, dir); } } -void go_start_stop(uint8_t axis, uint8_t dir, int16_t acc, uint16_t min_delay_us, uint16_t steps){ +void go_start_stop(uint8_t axes, uint8_t dir, int16_t acc, uint16_t min_delay_us, uint16_t steps){ uint16_t current_delay_us = MAX_DELAY; const uint16_t half = steps / 2; - accelerate(axis, dir, acc, current_delay_us, min_delay_us, half); - go_and_stop(axis, dir, -acc, current_delay_us, steps - half); + accelerate(axes, dir, acc, current_delay_us, min_delay_us, half); + go_and_stop(axes, dir, -acc, current_delay_us, steps - half); } /// moves X, Y, Z one after each other From c1e6567461b53fdd064e0614aaab2cb14e2e9629 Mon Sep 17 00:00:00 2001 From: espr14 Date: Tue, 29 Dec 2020 17:17:32 +0100 Subject: [PATCH 07/15] Fix step axis coding --- Firmware/xyzcal.cpp | 13 ++----------- 1 file changed, 2 insertions(+), 11 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index 411b48908..8c563f5f8 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -423,19 +423,10 @@ void set_axes_dir(uint8_t axes, uint8_t dir){ sm4_set_dir(Z_AXIS, dir & Z_AXIS_MASK); } -void go_step(uint8_t axes){ - if (axes & X_AXIS_MASK) - sm4_do_step(X_AXIS); - if (axes & Y_AXIS_MASK) - sm4_do_step(Y_AXIS); - if (axes & Z_AXIS_MASK) - sm4_do_step(Z_AXIS); -} - /// Accelerate up to max.speed (defined by @min_delay_us) /// does not update global positions void accelerate_1_step(uint8_t axes, int16_t acc, uint16_t &delay_us, uint16_t min_delay_us){ - go_step(axes); + sm4_do_step(axes); /// keep max speed (avoid extra computation) if (acc > 0 && delay_us == min_delay_us){ @@ -492,7 +483,7 @@ bool go_and_stop_1_step(uint8_t axes, int16_t dec, uint16_t &delay_us, uint16_t uint16_t s = round_to_u16(100 * 0.5f * SQR(0.01f) / (SQR((float)delay_us) * dec)); if (steps > s){ /// go steady - go_step(axes); + sm4_do_step(axes); delayMicroseconds(delay_us); } else { /// decelerate From c2ab8785f4a8d67a0927ad686bd9b8fd78ff536a Mon Sep 17 00:00:00 2001 From: espr14 Date: Tue, 29 Dec 2020 17:31:34 +0100 Subject: [PATCH 08/15] Revert Manhattan --- Firmware/xyzcal.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index 8c563f5f8..bb49e1cc0 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -551,8 +551,8 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ for (uint8_t r = 0; r < 32; r++){ ///< Y axis for (uint8_t d = 0; d < 2; ++d){ - go_manhattan((d & 1) ? (cx + 1024) : (cx - 1024), cy - 1024 + r * 64, _Z, Z_ACCEL, Z_MIN_DELAY); - // xyzcal_lineXYZ_to((d & 1) ? (cx + 1024) : (cx - 1024), cy - 1024 + r * 64, _Z, delay_us, 0); + // go_manhattan((d & 1) ? (cx + 1024) : (cx - 1024), cy - 1024 + r * 64, _Z, Z_ACCEL, Z_MIN_DELAY); + xyzcal_lineXYZ_to((d & 1) ? (cx + 1024) : (cx - 1024), cy - 1024 + r * 64, _Z, delay_us, 0); sm4_set_dir(X_AXIS, d); for (uint8_t c = 0; c < 32; c++){ ///< X axis z_trig = min_z; @@ -620,11 +620,11 @@ 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; /// don't go up if PINDA not triggered (optimization) const bool up = _PINDA; - const int8_t axis = up ? X_AXIS_MASK | Z_AXIS_MASK : X_AXIS_MASK; + const uint8_t axes = up ? X_AXIS_MASK | Z_AXIS_MASK : X_AXIS_MASK; const uint8_t dir = Z_PLUS_MASK | (d & 1 ? X_MINUS_MASK : X_PLUS_MASK); - accelerate(axis, dir, Z_ACCEL, current_delay_us, Z_MIN_DELAY, half_x); - go_and_stop(axis, dir, Z_ACCEL, current_delay_us, length_x - half_x); + accelerate(axes, dir, Z_ACCEL, current_delay_us, Z_MIN_DELAY, half_x); + go_and_stop(axes, dir, Z_ACCEL, current_delay_us, length_x - half_x); } } // DBG(_n("\n\n")); From c14e7255746f2ad0d68674246813d61b9a387c42 Mon Sep 17 00:00:00 2001 From: espr14 Date: Tue, 29 Dec 2020 18:27:33 +0100 Subject: [PATCH 09/15] Fix moves --- Firmware/xyzcal.cpp | 42 +++++++++++++++++++++++------------------- 1 file changed, 23 insertions(+), 19 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index bb49e1cc0..fb7b1f5a2 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -516,6 +516,8 @@ void stop_smoothly(uint8_t axes, uint8_t dir, int16_t dec, uint16_t &delay_us){ } void go_start_stop(uint8_t axes, uint8_t dir, int16_t acc, uint16_t min_delay_us, uint16_t steps){ + if (steps == 0) + return; uint16_t current_delay_us = MAX_DELAY; const uint16_t half = steps / 2; accelerate(axes, dir, acc, current_delay_us, min_delay_us, half); @@ -527,17 +529,18 @@ void go_start_stop(uint8_t axes, uint8_t dir, int16_t acc, uint16_t min_delay_us void go_manhattan(int16_t x, int16_t y, int16_t z, int16_t acc, uint16_t min_delay_us){ int32_t length; - DBG(_n("x %d -> %d\n"), x, _X); + // DBG(_n("x %d -> %d, "), x, _X); length = x - _X; go_start_stop(X_AXIS_MASK, length < 0 ? X_MINUS_MASK : X_PLUS_MASK, acc, min_delay_us, ABS(length)); - DBG(_n("y %d -> %d\n"), y, _Y); + // DBG(_n("y %d -> %d, "), y, _Y); length = y - _Y; go_start_stop(Y_AXIS_MASK, length < 0 ? Y_MINUS_MASK : Y_PLUS_MASK, acc, min_delay_us, ABS(length)); - DBG(_n("z %d -> %d\n"), z, _Z); + // DBG(_n("z %d -> %d\n"), z, _Z); length = z - _Z; go_start_stop(Z_AXIS_MASK, length < 0 ? Z_MINUS_MASK : Z_PLUS_MASK, acc, min_delay_us, ABS(length)); + // DBG(_n("\n")); } 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){ @@ -551,10 +554,25 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ for (uint8_t r = 0; r < 32; r++){ ///< Y axis for (uint8_t d = 0; d < 2; ++d){ - // go_manhattan((d & 1) ? (cx + 1024) : (cx - 1024), cy - 1024 + r * 64, _Z, Z_ACCEL, Z_MIN_DELAY); - xyzcal_lineXYZ_to((d & 1) ? (cx + 1024) : (cx - 1024), cy - 1024 + r * 64, _Z, delay_us, 0); + go_manhattan((d & 1) ? (cx + 992) : (cx - 992), cy - 992 + r * 64, _Z, Z_ACCEL, Z_MIN_DELAY); + xyzcal_lineXYZ_to((d & 1) ? (cx + 992) : (cx - 992), cy - 992 + r * 64, _Z, delay_us, 0); sm4_set_dir(X_AXIS, d); for (uint8_t c = 0; c < 32; c++){ ///< X axis + + /// move to the next point and move Z up diagonally (if needed) + current_delay_us = MAX_DELAY; + 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; + /// don't go up if PINDA not triggered (optimization) + const bool up = _PINDA; + const uint8_t axes = up ? X_AXIS_MASK | Z_AXIS_MASK : X_AXIS_MASK; + const uint8_t dir = Z_PLUS_MASK | (d & 1 ? X_MINUS_MASK : X_PLUS_MASK); + + accelerate(axes, dir, Z_ACCEL, current_delay_us, Z_MIN_DELAY, half_x); + go_and_stop(axes, dir, Z_ACCEL, current_delay_us, length_x - half_x); + + z_trig = min_z; /// move up to un-trigger (surpress hysteresis) @@ -611,20 +629,6 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ /// save average of both directions (filters effect of hysteresis) 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; - /// don't go up if PINDA not triggered (optimization) - const bool up = _PINDA; - const uint8_t axes = up ? X_AXIS_MASK | Z_AXIS_MASK : X_AXIS_MASK; - const uint8_t dir = Z_PLUS_MASK | (d & 1 ? X_MINUS_MASK : X_PLUS_MASK); - - accelerate(axes, dir, Z_ACCEL, current_delay_us, Z_MIN_DELAY, half_x); - go_and_stop(axes, dir, Z_ACCEL, current_delay_us, length_x - half_x); } } // DBG(_n("\n\n")); From 03b87ae68bb0eec0c1ab9a3cf32b0d2340ad6e01 Mon Sep 17 00:00:00 2001 From: espr14 Date: Wed, 30 Dec 2020 13:08:47 +0100 Subject: [PATCH 10/15] Add 8 pixel circle pattern --- Firmware/xyzcal.cpp | 48 ++++++++++++++++++++++++++++++++++++++------- 1 file changed, 41 insertions(+), 7 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index fb7b1f5a2..7e81602cb 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -552,13 +552,16 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ int16_t start_z; uint16_t steps_to_go; + DBG(_n("Scan countdown: ")); + for (uint8_t r = 0; r < 32; r++){ ///< Y axis for (uint8_t d = 0; d < 2; ++d){ go_manhattan((d & 1) ? (cx + 992) : (cx - 992), cy - 992 + r * 64, _Z, Z_ACCEL, Z_MIN_DELAY); xyzcal_lineXYZ_to((d & 1) ? (cx + 992) : (cx - 992), cy - 992 + r * 64, _Z, delay_us, 0); sm4_set_dir(X_AXIS, d); + DBG(_n("%d "), 64 - (r * 2 + d)); ///< to keep OctoPrint connection alive + for (uint8_t c = 0; c < 32; c++){ ///< X axis - /// move to the next point and move Z up diagonally (if needed) current_delay_us = MAX_DELAY; const int16_t end_x = ((d & 1) ? 1 : -1) * (64 * (16 - c) - 32) + cx; @@ -631,8 +634,8 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ } } } - // DBG(_n("\n\n")); } + DBG(_n("\n")); } /// Returns rate of match @@ -705,7 +708,8 @@ const int16_t xyzcal_point_xcoords[4] PROGMEM = {1200, 22000, 22000, 1200}; const int16_t xyzcal_point_ycoords[4] PROGMEM = {700, 700, 19800, 19800}; #endif //((MOTHERBOARD == BOARD_RAMBO_MINI_1_0) || (MOTHERBOARD == BOARD_RAMBO_MINI_1_3)) -const uint16_t xyzcal_point_pattern[12] PROGMEM = {0x000, 0x0f0, 0x1f8, 0x3fc, 0x7fe, 0x7fe, 0x7fe, 0x7fe, 0x3fc, 0x1f8, 0x0f0, 0x000}; +const uint16_t xyzcal_point_pattern_10[12] PROGMEM = {0x000, 0x0f0, 0x1f8, 0x3fc, 0x7fe, 0x7fe, 0x7fe, 0x7fe, 0x3fc, 0x1f8, 0x0f0, 0x000}; +const uint16_t xyzcal_point_pattern_08[12] PROGMEM = {0x000, 0x000, 0x0f0, 0x1f8, 0x3fc, 0x3fc, 0x3fc, 0x3fc, 0x1f8, 0x0f0, 0x000, 0x000}; bool xyzcal_searchZ(void) { @@ -814,6 +818,8 @@ 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); + for (int8_t i = iterations; i > 0; --i){ // DBG(_n(" [%f, %f][%f] circle\n"), x, y, r); @@ -871,6 +877,30 @@ void print_image(uint8_t *matrix_32x32){ DBG(_n("\n")); } +/// Takes two patterns and searches them in matrix32 +/// \returns best match +uint8_t find_patterns(uint8_t *matrix32, uint16_t *pattern08, uint16_t *pattern10, uint8_t &col, uint8_t &row){ + uint8_t c08 = 0; + uint8_t r08 = 0; + uint8_t match08 = 0; + uint8_t c10 = 0; + uint8_t r10 = 0; + uint8_t match10 = 0; + + match08 = xyzcal_find_pattern_12x12_in_32x32(matrix32, pattern08, &c08, &r08); + match10 = xyzcal_find_pattern_12x12_in_32x32(matrix32, pattern10, &c10, &r10); + + if (match08 > match10){ + col = c08; + row = r08; + return match08; + } + + col = c10; + row = r10; + return match10; +} + /// scans area around the current head location and /// searches for the center of the calibration pin bool xyzcal_scan_and_process(void){ @@ -881,21 +911,25 @@ bool xyzcal_scan_and_process(void){ int16_t z = _Z; uint8_t *matrix32 = (uint8_t *)block_buffer; - uint16_t *pattern = (uint16_t *)(matrix32 + 32 * 32); + 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++){ - pattern[i] = pgm_read_word((uint16_t*)(xyzcal_point_pattern + i)); -// DBG(_n(" pattern[%d]=%d\n"), i, pattern[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)); + // DBG(_n(" pattern[%d]=%d\n"), i, pattern[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 (xyzcal_find_pattern_12x12_in_32x32(matrix32, pattern, &uc, &ur) >= 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; From 13fefaa1bff902c919ced316bee589de885be16d Mon Sep 17 00:00:00 2001 From: espr14 Date: Wed, 30 Dec 2020 15:01:32 +0100 Subject: [PATCH 11/15] Fix typo --- Firmware/xyzcal.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index 7e81602cb..636c6b9cd 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -385,11 +385,11 @@ int8_t xyzcal_meassure_pinda_hysterezis(int16_t min_z, int16_t max_z, uint16_t d } #endif //XYZCAL_MEASSURE_PINDA_HYSTEREZIS -void print_hysterezis(int16_t min_z, int16_t max_z, int16_t step){ +void print_hysteresis(int16_t min_z, int16_t max_z, int16_t step){ int16_t delay_us = 600; int16_t trigger = 0; int16_t untrigger = 0; - DBG(_n("Hysterezis\n")); + DBG(_n("Hysteresis\n")); xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 0); From a9fd09ca3f06b2f775c3ab35a9ccd02e6ed7b119 Mon Sep 17 00:00:00 2001 From: espr14 Date: Wed, 30 Dec 2020 15:15:17 +0100 Subject: [PATCH 12/15] Implement median --- Firmware/xyzcal.cpp | 31 +++++++++++++++++++++---------- 1 file changed, 21 insertions(+), 10 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index 636c6b9cd..89bb1cc52 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -800,6 +800,24 @@ float highest(float *points, const uint8_t num_points){ return max; } +/// slow bubble sort but short +void sort(float *points, const uint8_t num_points){ + /// one direction bubble sort + for (uint8_t i = 0; i < num_points; ++i){ + for (uint8_t j = 0; j < num_points - i - 1; ++j){ + if (points[j] > points[j + 1]) + SWAP(points[j], points[j + 1]); + } + } +} + +/// sort array and returns median value +/// don't send empty array or nullptr +float median(float *points, const uint8_t num_points){ + sort(points, num_points); + return points[num_points / 2]; +} + /// Searches for circle iteratively /// Uses points on the perimeter. If point is high it pushes circle out of the center (shift or change of radius), /// otherwise to the center. @@ -846,18 +864,11 @@ void dynamic_circle(uint8_t *matrix_32x32, float &x, float &y, float &r, uint8_t } } - /// remove extreme values (slow but simple) - for (uint8_t j = 0; j < blocks / 2; ++j){ - remove_highest(shifts_x, blocks); - remove_highest(shifts_y, blocks); - remove_highest(shifts_r, blocks); - } - /// median is the highest now norm = 1.f / (32.f * (num_points * 3 / 4)); - x += CLAMP(highest(shifts_x, blocks) * norm, -max_val, max_val); - y += CLAMP(highest(shifts_y, blocks) * norm, -max_val, max_val); - r += CLAMP(highest(shifts_r, blocks) * norm, -max_val, max_val); + x += CLAMP(median(shifts_x, blocks) * norm, -max_val, max_val); + y += CLAMP(median(shifts_y, blocks) * norm, -max_val, max_val); + r += CLAMP(median(shifts_r, blocks) * norm, -max_val, max_val); r = MAX(2, r); From 49d57ed6216748e68ac07711ba33c9d5090a993b Mon Sep 17 00:00:00 2001 From: espr14 Date: Wed, 30 Dec 2020 17:02:31 +0100 Subject: [PATCH 13/15] Median dynamic circle --- Firmware/xyzcal.cpp | 50 ++++++++++++++++++++++++++------------------- 1 file changed, 29 insertions(+), 21 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index 89bb1cc52..f5d6bc0ee 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -809,8 +809,14 @@ void sort(float *points, const uint8_t num_points){ SWAP(points[j], points[j + 1]); } } + + // DBG(_n("Sorted: ")); + // for (uint8_t i = 0; i < num_points; ++i) + // DBG(_n("%f "), points[i]); + // DBG(_n("\n")); } + /// sort array and returns median value /// don't send empty array or nullptr float median(float *points, const uint8_t num_points){ @@ -828,10 +834,9 @@ void dynamic_circle(uint8_t *matrix_32x32, float &x, float &y, float &r, uint8_t float points[num_points]; 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 norm; float angle; - float max_val = 0.5f; - const uint8_t blocks = 7; + float max_change = 0.5f; ///< avoids too fast changes (could cause oscillation) + const uint8_t blocks = num_points; float shifts_x[blocks]; float shifts_y[blocks]; float shifts_r[blocks]; @@ -840,35 +845,38 @@ void dynamic_circle(uint8_t *matrix_32x32, float &x, float &y, float &r, uint8_t for (int8_t i = iterations; i > 0; --i){ - // DBG(_n(" [%f, %f][%f] circle\n"), x, y, r); + DBG(_n(" [%f, %f][%f] circle\n"), x, y, r); /// read points on the circle for (uint8_t p = 0; p < num_points; ++p){ angle = p * pi_2_div_num_points; points[p] = get_value(matrix_32x32, r * cos(angle) + x, r * sin(angle) + y) - target_z; // DBG(_n("%f "), points[p]); + + shifts_x[p] = cos(angle) * points[p]; + shifts_y[p] = sin(angle) * points[p]; + shifts_r[p] = points[p]; } // DBG(_n(" points\n")); - /// sum blocks - for (uint8_t j = 0; j < blocks; ++j){ - shifts_x[j] = shifts_y[j] = shifts_r[j] = 0; - /// first part - for (uint8_t p = 0; p < num_points * 3 / 4; ++p){ - uint8_t idx = (p + j * num_points / blocks) % num_points; + // /// sum blocks + // for (uint8_t j = 0; j < blocks; ++j){ + // shifts_x[j] = shifts_y[j] = shifts_r[j] = 0; + // /// first part + // for (uint8_t p = 0; p < num_points * 3 / 4; ++p){ + // uint8_t idx = (p + j * num_points / blocks) % num_points; - angle = idx * pi_2_div_num_points; - shifts_x[j] += cos(angle) * points[idx]; - shifts_y[j] += sin(angle) * points[idx]; - shifts_r[j] += points[idx]; - } - } + // angle = idx * pi_2_div_num_points; + // shifts_x[j] += cos(angle) * points[idx]; + // shifts_y[j] += sin(angle) * points[idx]; + // shifts_r[j] += points[idx]; + // } + // } - /// median is the highest now - norm = 1.f / (32.f * (num_points * 3 / 4)); - x += CLAMP(median(shifts_x, blocks) * norm, -max_val, max_val); - y += CLAMP(median(shifts_y, blocks) * norm, -max_val, max_val); - r += CLAMP(median(shifts_r, blocks) * norm, -max_val, max_val); + const float norm = 1.f / 32.f; + 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); r = MAX(2, r); From cc9e7b9376dac9ef40bd4c1cbc42e419ed1ee549 Mon Sep 17 00:00:00 2001 From: espr14 Date: Mon, 4 Jan 2021 13:01:06 +0100 Subject: [PATCH 14/15] Fix comments, remove unused --- Firmware/xyzcal.cpp | 32 ++++++++------------------------ 1 file changed, 8 insertions(+), 24 deletions(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index f5d6bc0ee..d36244941 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -627,8 +627,8 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ 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); + /// !!! data reversed in X + // DBG(_n("%04x"), ((uint32_t)line_buffer[31 - c] + (z_trig - min_z)) / 2); /// save average of both directions (filters effect of hysteresis) pixels[(uint16_t)r * 32 + (31 - c)] = (uint8_t)MIN((uint32_t)255, ((uint32_t)line_buffer[31 - c] + (z_trig - min_z)) / 2); } @@ -831,11 +831,10 @@ 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 points[num_points]; 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 (could cause oscillation) + float max_change = 0.5f; ///< avoids too fast changes (avoid oscillation) const uint8_t blocks = num_points; float shifts_x[blocks]; float shifts_y[blocks]; @@ -850,29 +849,15 @@ 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; - points[p] = get_value(matrix_32x32, r * cos(angle) + x, r * sin(angle) + y) - target_z; - // DBG(_n("%f "), points[p]); + const float height = get_value(matrix_32x32, r * cos(angle) + x, r * sin(angle) + y) - target_z; + // DBG(_n("%f "), point); - shifts_x[p] = cos(angle) * points[p]; - shifts_y[p] = sin(angle) * points[p]; - shifts_r[p] = points[p]; + shifts_x[p] = cos(angle) * height; + shifts_y[p] = sin(angle) * height; + shifts_r[p] = height; } // DBG(_n(" points\n")); - // /// sum blocks - // for (uint8_t j = 0; j < blocks; ++j){ - // shifts_x[j] = shifts_y[j] = shifts_r[j] = 0; - // /// first part - // for (uint8_t p = 0; p < num_points * 3 / 4; ++p){ - // uint8_t idx = (p + j * num_points / blocks) % num_points; - - // angle = idx * pi_2_div_num_points; - // shifts_x[j] += cos(angle) * points[idx]; - // shifts_y[j] += sin(angle) * points[idx]; - // shifts_r[j] += points[idx]; - // } - // } - const float norm = 1.f / 32.f; x += CLAMP(median(shifts_x, blocks) * norm, -max_change, max_change); y += CLAMP(median(shifts_y, blocks) * norm, -max_change, max_change); @@ -939,7 +924,6 @@ bool xyzcal_scan_and_process(void){ 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)); - // DBG(_n(" pattern[%d]=%d\n"), i, pattern[i]); } /// SEARCH FOR BINARY CIRCLE From 2981b9e9c5d822a5e5ff4809f6f9eba818a25115 Mon Sep 17 00:00:00 2001 From: espr14 Date: Mon, 4 Jan 2021 14:59:12 +0100 Subject: [PATCH 15/15] OctoPrint needs new lines --- Firmware/xyzcal.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index d36244941..c68804890 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -559,7 +559,7 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_ go_manhattan((d & 1) ? (cx + 992) : (cx - 992), cy - 992 + r * 64, _Z, Z_ACCEL, Z_MIN_DELAY); xyzcal_lineXYZ_to((d & 1) ? (cx + 992) : (cx - 992), cy - 992 + r * 64, _Z, delay_us, 0); sm4_set_dir(X_AXIS, d); - DBG(_n("%d "), 64 - (r * 2 + d)); ///< to keep OctoPrint connection alive + DBG(_n("%d\n"), 64 - (r * 2 + d)); ///< to keep OctoPrint connection alive for (uint8_t c = 0; c < 32; c++){ ///< X axis /// move to the next point and move Z up diagonally (if needed)