Merge pull request #2960 from espr14/PFW-1169-3
Calibration: more accelerations, fix scan, more robust circle search PFW-1169
This commit is contained in:
DRracer
GitHub
commit
85f2c29dfe
1 changed files with 246 additions and 113 deletions
|
|
@ -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);
|
||||
|
|
@ -226,6 +238,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,9 +385,48 @@ int8_t xyzcal_meassure_pinda_hysterezis(int16_t min_z, int16_t max_z, uint16_t d
|
|||
}
|
||||
#endif //XYZCAL_MEASSURE_PINDA_HYSTEREZIS
|
||||
|
||||
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("Hysteresis\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);
|
||||
}
|
||||
}
|
||||
|
||||
void update_position_1_step(uint8_t axis, uint8_t dir){
|
||||
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_axes_dir(uint8_t axes, uint8_t dir){
|
||||
if (axes & X_AXIS_MASK)
|
||||
sm4_set_dir(X_AXIS, dir & X_AXIS_MASK);
|
||||
if (axes & Y_AXIS_MASK)
|
||||
sm4_set_dir(Y_AXIS, dir & Y_AXIS_MASK);
|
||||
if (axes & Z_AXIS_MASK)
|
||||
sm4_set_dir(Z_AXIS, 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){
|
||||
sm4_do_step(axis);
|
||||
/// does not update global positions
|
||||
void accelerate_1_step(uint8_t axes, int16_t acc, uint16_t &delay_us, uint16_t min_delay_us){
|
||||
sm4_do_step(axes);
|
||||
|
||||
/// keep max speed (avoid extra computation)
|
||||
if (acc > 0 && delay_us == min_delay_us){
|
||||
|
|
@ -406,136 +460,182 @@ 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 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(axes, acc, delay_us, min_delay_us);
|
||||
update_position_1_step(axes, 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 axes, 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));
|
||||
if (steps > s){
|
||||
/// go steady
|
||||
sm4_do_step(axis);
|
||||
sm4_do_step(axes);
|
||||
delayMicroseconds(delay_us);
|
||||
} else {
|
||||
/// decelerate
|
||||
accelerate(axis, -dec, delay_us, delay_us);
|
||||
accelerate_1_step(axes, -dec, delay_us, delay_us);
|
||||
}
|
||||
--steps;
|
||||
return true;
|
||||
}
|
||||
|
||||
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){
|
||||
/// \param dir sets direction of movement
|
||||
/// updates global positions
|
||||
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 axes, uint8_t dir, int16_t dec, uint16_t &delay_us){
|
||||
if (dec <= 0)
|
||||
return;
|
||||
set_axes_dir(axes, dir);
|
||||
while (delay_us < MAX_DELAY){
|
||||
accelerate_1_step(axes, -dec, delay_us, delay_us);
|
||||
update_position_1_step(axes, dir);
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
go_and_stop(axes, 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, "), 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, "), 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));
|
||||
// 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){
|
||||
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
|
||||
xyzcal_lineXYZ_to(cx - 1024, cy - 1024, min_z, delay_us, 0);
|
||||
int16_t start_z;
|
||||
uint16_t steps_to_go;
|
||||
|
||||
DBG(_n("Scan countdown: "));
|
||||
|
||||
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;
|
||||
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);
|
||||
for (uint8_t c = 0; c < 32; c++){ ///< X axis
|
||||
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)
|
||||
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)
|
||||
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){
|
||||
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_1_step(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;
|
||||
if (_PINDA){
|
||||
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_;
|
||||
}
|
||||
}
|
||||
/// slow down to stop
|
||||
while (current_delay_us < MAX_DELAY){
|
||||
accelerate(Z_AXIS_MASK, -Z_ACCEL, current_delay_us, Z_MIN_DELAY);
|
||||
++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){
|
||||
for (start_z = _Z; _Z > (min_z + start_z) / 2; --_Z_){
|
||||
if (_PINDA){
|
||||
z_trig = z;
|
||||
z_trig = _Z;
|
||||
break;
|
||||
}
|
||||
accelerate(Z_AXIS_MASK, Z_ACCEL, current_delay_us, Z_MIN_DELAY);
|
||||
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(Z_AXIS_MASK, Z_ACCEL, current_delay_us, steps_to_go);
|
||||
--z;
|
||||
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;
|
||||
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;
|
||||
/// 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_;
|
||||
}
|
||||
|
||||
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
|
||||
/// !!! 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);
|
||||
}
|
||||
|
||||
/// 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"));
|
||||
}
|
||||
DBG(_n("\n"));
|
||||
}
|
||||
|
||||
/// Returns rate of match
|
||||
|
|
@ -608,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)
|
||||
{
|
||||
|
|
@ -699,6 +800,30 @@ 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]);
|
||||
}
|
||||
}
|
||||
|
||||
// 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){
|
||||
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.
|
||||
|
|
@ -706,55 +831,37 @@ float highest(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 norm;
|
||||
float angle;
|
||||
float max_val = 0.5f;
|
||||
const uint8_t blocks = 7;
|
||||
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];
|
||||
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);
|
||||
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]);
|
||||
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) * 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];
|
||||
}
|
||||
}
|
||||
|
||||
/// 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);
|
||||
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);
|
||||
|
||||
|
|
@ -774,6 +881,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){
|
||||
|
|
@ -784,21 +915,24 @@ 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));
|
||||
}
|
||||
|
||||
/// 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;
|
||||
|
|
@ -846,8 +980,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();
|
||||
|
|
|
|||
Loading…
Reference in a new issue