New XYZ calibration algorithm
This commit is contained in:
espr14
parent
396ee0fde2
commit
a8f4207df9
@ -9,7 +9,6 @@
|
||||
#include "temperature.h"
|
||||
#include "sm4.h"
|
||||
|
||||
|
||||
#define XYZCAL_PINDA_HYST_MIN 20 //50um
|
||||
#define XYZCAL_PINDA_HYST_MAX 100 //250um
|
||||
#define XYZCAL_PINDA_HYST_DIF 5 //12.5um
|
||||
@ -30,13 +29,83 @@
|
||||
#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
|
||||
|
||||
#define _PI 3.14159265F
|
||||
|
||||
/// \returns positive value always
|
||||
#define ABS(a) \
|
||||
({ __typeof__ (a) _a = (a); \
|
||||
_a >= 0 ? _a : (-_a); })
|
||||
|
||||
/// \returns maximum of the two
|
||||
#define MAX(a, b) \
|
||||
({ __typeof__ (a) _a = (a); \
|
||||
__typeof__ (b) _b = (b); \
|
||||
_a >= _b ? _a : _b; })
|
||||
|
||||
/// \returns minimum of the two
|
||||
#define MIN(a, b) \
|
||||
({ __typeof__ (a) _a = (a); \
|
||||
__typeof__ (b) _b = (b); \
|
||||
_a <= _b ? _a : _b; })
|
||||
|
||||
/// swap values
|
||||
#define SWAP(a, b) \
|
||||
({ __typeof__ (a) c = (a); \
|
||||
a = (b); \
|
||||
b = c; })
|
||||
|
||||
/// Saturates value
|
||||
/// \returns min if value is less than min
|
||||
/// \returns max if value is more than min
|
||||
/// \returns value otherwise
|
||||
#define CLAMP(value, min, max) \
|
||||
({ __typeof__ (value) a_ = (value); \
|
||||
__typeof__ (min) min_ = (min); \
|
||||
__typeof__ (max) max_ = (max); \
|
||||
( a_ < min_ ? min_ : (a_ <= max_ ? a_ : max_)); })
|
||||
|
||||
/// position types
|
||||
typedef int16_t pos_i16_t;
|
||||
typedef long pos_i32_t;
|
||||
typedef float pos_mm_t;
|
||||
typedef int16_t usteps_t;
|
||||
|
||||
uint8_t check_pinda_0();
|
||||
uint8_t check_pinda_1();
|
||||
void xyzcal_update_pos(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de);
|
||||
uint16_t xyzcal_calc_delay(uint16_t nd, uint16_t dd);
|
||||
|
||||
uint8_t round_to_u8(float f){
|
||||
return (uint8_t)(f + .5f);
|
||||
}
|
||||
|
||||
uint16_t round_to_u16(float f){
|
||||
return (uint16_t)(f + .5f);
|
||||
}
|
||||
|
||||
int16_t round_to_i16(float f){
|
||||
return (int16_t)(f + .5f);
|
||||
}
|
||||
|
||||
/// converts millimeters to integer position
|
||||
pos_i16_t mm_2_pos(pos_mm_t mm){
|
||||
return (pos_i16_t)(0.5f + mm * 100);
|
||||
}
|
||||
|
||||
/// converts integer position to millimeters
|
||||
pos_mm_t pos_2_mm(pos_i16_t pos){
|
||||
return pos * 0.01f;
|
||||
}
|
||||
pos_mm_t pos_2_mm(float pos){
|
||||
return pos * 0.01f;
|
||||
}
|
||||
|
||||
void xyzcal_meassure_enter(void)
|
||||
{
|
||||
@ -142,6 +211,7 @@ 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
|
||||
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);
|
||||
@ -152,12 +222,17 @@ bool xyzcal_lineXYZ_to(int16_t x, int16_t y, int16_t z, uint16_t delay_us, int8_
|
||||
sm4_set_dir_bits(xyzcal_dm);
|
||||
sm4_stop_cb = check_pinda?((check_pinda<0)?check_pinda_0:check_pinda_1):0;
|
||||
xyzcal_sm4_delay = delay_us;
|
||||
// uint32_t u = _micros();
|
||||
bool ret = sm4_line_xyze_ui(abs(x), abs(y), abs(z), 0)?true:false;
|
||||
// u = _micros() - u;
|
||||
// uint32_t u = _micros();
|
||||
bool ret = sm4_line_xyze_ui(abs(x), abs(y), abs(z), 0) ? true : false;
|
||||
// u = _micros() - u;
|
||||
return ret;
|
||||
}
|
||||
|
||||
/// Moves printer to absolute position [x,y,z] defined in millimeters
|
||||
bool xyzcal_lineXYZ_to_float(pos_mm_t x, pos_mm_t y, pos_mm_t z, uint16_t delay_us, int8_t check_pinda){
|
||||
return xyzcal_lineXYZ_to(mm_2_pos(x), mm_2_pos(y), mm_2_pos(z), delay_us, check_pinda);
|
||||
}
|
||||
|
||||
bool xyzcal_spiral2(int16_t cx, int16_t cy, int16_t z0, int16_t dz, int16_t radius, int16_t rotation, uint16_t delay_us, int8_t check_pinda, uint16_t* pad)
|
||||
{
|
||||
bool ret = false;
|
||||
@ -171,7 +246,13 @@ bool xyzcal_spiral2(int16_t cx, int16_t cy, int16_t z0, int16_t dz, int16_t radi
|
||||
uint8_t k = 720 / (dad_max - dad_min); //delta calculation constant
|
||||
ad = 0;
|
||||
if (pad) ad = *pad % 720;
|
||||
|
||||
DBG(_n("xyzcal_spiral2 cx=%d cy=%d z0=%d dz=%d radius=%d ad=%d\n"), cx, cy, z0, dz, radius, ad);
|
||||
// lcd_set_cursor(0, 4);
|
||||
// char text[10];
|
||||
// snprintf(text, 10, "%4d", z0);
|
||||
// lcd_print(text);
|
||||
|
||||
for (; ad < 720; ad++)
|
||||
{
|
||||
if (radius > 0)
|
||||
@ -200,6 +281,10 @@ bool xyzcal_spiral2(int16_t cx, int16_t cy, int16_t z0, int16_t dz, int16_t radi
|
||||
ad += dad;
|
||||
}
|
||||
if (pad) *pad = ad;
|
||||
// if(ret){
|
||||
// lcd_set_cursor(0, 4);
|
||||
// lcd_print(" ");
|
||||
// }
|
||||
return ret;
|
||||
}
|
||||
|
||||
@ -275,426 +360,113 @@ int8_t xyzcal_meassure_pinda_hysterezis(int16_t min_z, int16_t max_z, uint16_t d
|
||||
}
|
||||
#endif //XYZCAL_MEASSURE_PINDA_HYSTEREZIS
|
||||
|
||||
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;
|
||||
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);
|
||||
|
||||
void xyzcal_scan_pixels_32x32(int16_t cx, int16_t cy, int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t* pixels)
|
||||
{
|
||||
DBG(_n("xyzcal_scan_pixels_32x32 cx=%d cy=%d min_z=%d max_z=%d\n"), cx, cy, min_z, max_z);
|
||||
// xyzcal_lineXYZ_to(cx - 1024, cy - 1024, max_z, 2*delay_us, 0);
|
||||
// xyzcal_lineXYZ_to(cx, cy, max_z, delay_us, 0);
|
||||
int16_t z = (int16_t)count_position[2];
|
||||
xyzcal_lineXYZ_to(cx, cy, z, 2*delay_us, 0);
|
||||
for (uint8_t r = 0; r < 32; r++)
|
||||
{
|
||||
// int8_t _pinda = _PINDA;
|
||||
xyzcal_lineXYZ_to((r&1)?(cx+1024):(cx-1024), cy - 1024 + r*64, 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);
|
||||
z = (int16_t)count_position[2];
|
||||
sm4_set_dir(X_AXIS, (r&1)?1:0);
|
||||
for (uint8_t c = 0; c < 32; c++)
|
||||
{
|
||||
uint16_t sum = 0;
|
||||
int16_t z_sum = 0;
|
||||
for (uint8_t i = 0; i < 64; i++)
|
||||
{
|
||||
int8_t pinda = _PINDA;
|
||||
int16_t pix = z - min_z;
|
||||
pix += (pinda)?23:-24;
|
||||
if (pix < 0) pix = 0;
|
||||
if (pix > 255) pix = 255;
|
||||
sum += pix;
|
||||
z_sum += z;
|
||||
// if (_pinda != pinda)
|
||||
// {
|
||||
// if (pinda)
|
||||
// DBG(_n("!1 x=%d z=%d\n"), c*64+i, z+23);
|
||||
// else
|
||||
// DBG(_n("!0 x=%d z=%d\n"), c*64+i, z-24);
|
||||
// }
|
||||
sm4_set_dir(Z_AXIS, !pinda);
|
||||
if (!pinda)
|
||||
{
|
||||
if (z > min_z)
|
||||
{
|
||||
sm4_do_step(Z_AXIS_MASK);
|
||||
z--;
|
||||
}
|
||||
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++;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (z < max_z)
|
||||
{
|
||||
sm4_do_step(Z_AXIS_MASK);
|
||||
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);
|
||||
}
|
||||
sm4_do_step(X_AXIS_MASK);
|
||||
delayMicroseconds(600);
|
||||
// _pinda = pinda;
|
||||
}
|
||||
sum >>= 6; //div 64
|
||||
if (z_sum < 0)
|
||||
{
|
||||
z_sum = -z_sum;
|
||||
z_sum >>= 6; //div 64
|
||||
z_sum = -z_sum;
|
||||
}
|
||||
else
|
||||
z_sum >>= 6; //div 64
|
||||
if (pixels) pixels[((uint16_t)r<<5) + ((r&1)?(31-c):c)] = sum;
|
||||
// DBG(_n("c=%d r=%d l=%d z=%d\n"), c, r, sum, z_sum);
|
||||
count_position[0] += (r&1)?-64:64;
|
||||
count_position[2] = z;
|
||||
}
|
||||
if (pixels)
|
||||
for (uint8_t c = 0; c < 32; c++)
|
||||
DBG(_n("%02x"), pixels[((uint16_t)r<<5) + c]);
|
||||
DBG(_n("\n"));
|
||||
}
|
||||
// xyzcal_lineXYZ_to(cx, cy, z, 2*delay_us, 0);
|
||||
}
|
||||
|
||||
void xyzcal_histo_pixels_32x32(uint8_t* pixels, uint16_t* histo)
|
||||
{
|
||||
for (uint8_t l = 0; l < 16; l++)
|
||||
histo[l] = 0;
|
||||
for (uint8_t r = 0; r < 32; r++)
|
||||
for (uint8_t c = 0; c < 32; c++)
|
||||
{
|
||||
uint8_t pix = pixels[((uint16_t)r<<5) + c];
|
||||
histo[pix >> 4]++;
|
||||
}
|
||||
for (uint8_t l = 0; l < 16; l++)
|
||||
DBG(_n(" %2d %d\n"), l, histo[l]);
|
||||
}
|
||||
|
||||
void xyzcal_adjust_pixels(uint8_t* pixels, uint16_t* histo)
|
||||
{
|
||||
uint8_t l;
|
||||
uint16_t max_c = histo[1];
|
||||
uint8_t max_l = 1;
|
||||
for (l = 1; l < 16; l++)
|
||||
{
|
||||
uint16_t c = histo[l];
|
||||
if (c > max_c)
|
||||
{
|
||||
max_c = c;
|
||||
max_l = l;
|
||||
}
|
||||
}
|
||||
DBG(_n("max_c=%2d max_l=%d\n"), max_c, max_l);
|
||||
for (l = 14; l > 8; l--)
|
||||
if (histo[l] >= 10)
|
||||
break;
|
||||
uint8_t pix_min = 0;
|
||||
uint8_t pix_max = l << 4;
|
||||
if (histo[0] < (32*32 - 144))
|
||||
{
|
||||
pix_min = (max_l << 4) / 2;
|
||||
}
|
||||
uint8_t pix_dif = pix_max - pix_min;
|
||||
DBG(_n(" min=%d max=%d dif=%d\n"), pix_min, pix_max, pix_dif);
|
||||
for (int16_t i = 0; i < 32*32; i++)
|
||||
{
|
||||
uint16_t pix = pixels[i];
|
||||
if (pix > pix_min) pix -= pix_min;
|
||||
else pix = 0;
|
||||
pix <<= 8;
|
||||
pix /= pix_dif;
|
||||
// if (pix < 0) pix = 0;
|
||||
if (pix > 255) pix = 255;
|
||||
pixels[i] = (uint8_t)pix;
|
||||
}
|
||||
for (uint8_t r = 0; r < 32; r++)
|
||||
{
|
||||
for (uint8_t c = 0; c < 32; c++)
|
||||
DBG(_n("%02x"), pixels[((uint16_t)r<<5) + c]);
|
||||
DBG(_n("\n"));
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
void xyzcal_draw_pattern_12x12_in_32x32(uint8_t* pattern, uint32_t* pixels, int w, int h, uint8_t x, uint8_t y, uint32_t and, uint32_t or)
|
||||
{
|
||||
for (int i = 0; i < 8; i++)
|
||||
for (int j = 0; j < 8; j++)
|
||||
{
|
||||
int idx = (x + j) + w * (y + i);
|
||||
if (pattern[i] & (1 << j))
|
||||
{
|
||||
pixels[idx] &= and;
|
||||
pixels[idx] |= or;
|
||||
/// 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;
|
||||
}
|
||||
}
|
||||
// DBG(_n("\n\n"));
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
int16_t xyzcal_match_pattern_12x12_in_32x32(uint16_t* pattern, uint8_t* pixels, uint8_t c, uint8_t r)
|
||||
{
|
||||
/// Returns rate of match
|
||||
/// max match = 132, min match = 0
|
||||
uint8_t xyzcal_match_pattern_12x12_in_32x32(uint16_t* pattern, uint8_t* pixels, uint8_t c, uint8_t r){
|
||||
uint8_t thr = 16;
|
||||
int16_t match = 0;
|
||||
for (uint8_t i = 0; i < 12; i++)
|
||||
for (uint8_t j = 0; j < 12; j++)
|
||||
{
|
||||
if (((i == 0) || (i == 11)) && ((j < 2) || (j >= 10))) continue; //skip corners
|
||||
uint8_t match = 0;
|
||||
for (uint8_t i = 0; i < 12; ++i){
|
||||
for (uint8_t j = 0; j < 12; ++j){
|
||||
/// skip corners (3 pixels in each)
|
||||
if (((i == 0) || (i == 11)) && ((j < 2) || (j >= 10))) continue;
|
||||
if (((j == 0) || (j == 11)) && ((i < 2) || (i >= 10))) continue;
|
||||
uint16_t idx = (c + j) + 32 * (r + i);
|
||||
uint8_t val = pixels[idx];
|
||||
if (pattern[i] & (1 << j))
|
||||
{
|
||||
if (val > thr) match ++;
|
||||
else match --;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (val <= thr) match ++;
|
||||
else match --;
|
||||
}
|
||||
const uint16_t idx = (c + j) + 32 * ((uint16_t)r + i);
|
||||
const bool high_pix = pixels[idx] > thr;
|
||||
const bool high_pat = pattern[i] & (1 << j);
|
||||
if (high_pix == high_pat)
|
||||
match++;
|
||||
}
|
||||
}
|
||||
return match;
|
||||
}
|
||||
|
||||
int16_t xyzcal_find_pattern_12x12_in_32x32(uint8_t* pixels, uint16_t* pattern, uint8_t* pc, uint8_t* pr)
|
||||
{
|
||||
/// Searches for best match of pattern by shifting it
|
||||
/// Returns rate of match and the best location
|
||||
/// max match = 132, min match = 0
|
||||
uint8_t xyzcal_find_pattern_12x12_in_32x32(uint8_t* pixels, uint16_t* pattern, uint8_t* pc, uint8_t* pr){
|
||||
if (!pixels || !pattern || !pc || !pr)
|
||||
return -1;
|
||||
uint8_t max_c = 0;
|
||||
uint8_t max_r = 0;
|
||||
int16_t max_match = 0;
|
||||
for (uint8_t r = 0; r < (32 - 12); r++)
|
||||
for (uint8_t c = 0; c < (32 - 12); c++)
|
||||
{
|
||||
int16_t match = xyzcal_match_pattern_12x12_in_32x32(pattern, pixels, c, r);
|
||||
if (max_match < match)
|
||||
{
|
||||
uint8_t max_match = 0;
|
||||
|
||||
// DBG(_n("Matching:\n"));
|
||||
/// pixel precision
|
||||
for (uint8_t r = 0; r < (32 - 12); ++r){
|
||||
for (uint8_t c = 0; c < (32 - 12); ++c){
|
||||
const uint8_t match = xyzcal_match_pattern_12x12_in_32x32(pattern, pixels, c, r);
|
||||
if (max_match < match){
|
||||
max_c = c;
|
||||
max_r = r;
|
||||
max_match = match;
|
||||
}
|
||||
// DBG(_n("%d "), match);
|
||||
}
|
||||
DBG(_n("max_c=%d max_r=%d max_match=%d\n"), max_c, max_r, max_match);
|
||||
if (pc) *pc = max_c;
|
||||
if (pr) *pr = max_r;
|
||||
// DBG(_n("\n"));
|
||||
}
|
||||
DBG(_n("max_c=%f max_r=%f max_match=%d pixel\n"), max_c, max_r, max_match);
|
||||
|
||||
*pc = max_c;
|
||||
*pr = max_r;
|
||||
return max_match;
|
||||
}
|
||||
|
||||
#define MAX_DIAMETR 600
|
||||
#define XYZCAL_FIND_CENTER_DIAGONAL
|
||||
int8_t xyzcal_find_point_center2A(int16_t x0, int16_t y0, int16_t z0, uint16_t delay_us);
|
||||
int8_t xyzcal_find_point_center2(uint16_t delay_us)
|
||||
{
|
||||
printf_P(PSTR("xyzcal_find_point_center2\n"));
|
||||
int16_t x0 = _X;
|
||||
int16_t y0 = _Y;
|
||||
int16_t z0 = _Z;
|
||||
printf_P(PSTR(" x0=%d\n"), x0);
|
||||
printf_P(PSTR(" y0=%d\n"), y0);
|
||||
printf_P(PSTR(" z0=%d\n"), z0);
|
||||
|
||||
xyzcal_lineXYZ_to(_X, _Y, z0 + 400, 500, -1);
|
||||
xyzcal_lineXYZ_to(_X, _Y, z0 - 400, 500, 1);
|
||||
xyzcal_lineXYZ_to(_X, _Y, z0 + 400, 500, -1);
|
||||
xyzcal_lineXYZ_to(_X, _Y, z0 - 400, 500, 1);
|
||||
|
||||
if (has_temperature_compensation()){
|
||||
z0 = _Z - 20; // normal PINDA
|
||||
return xyzcal_find_point_center2A(x0, y0, z0, delay_us);
|
||||
} else {
|
||||
// try searching harder, each PINDA is different
|
||||
for(z0 = _Z - 20; z0 <= _Z + 140; z0 += 20 ){ // alternate PINDA
|
||||
int8_t rv = xyzcal_find_point_center2A(x0, y0, z0, delay_us);
|
||||
printf_P(PSTR(" z0=%d"), z0);
|
||||
if( rv != 0 ){
|
||||
printf_P(PSTR("ok\n"));
|
||||
return rv;
|
||||
} else {
|
||||
printf_P(PSTR("fail\n"));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int8_t xyzcal_find_point_center2A(int16_t x0, int16_t y0, int16_t z0, uint16_t delay_us){
|
||||
xyzcal_lineXYZ_to(_X, _Y, z0, 500, 0);
|
||||
|
||||
// xyzcal_lineXYZ_to(x0, y0, z0 - 100, 500, 1);
|
||||
// z0 = _Z;
|
||||
// printf_P(PSTR(" z0=%d\n"), z0);
|
||||
// xyzcal_lineXYZ_to(x0, y0, z0 + 100, 500, -1);
|
||||
// z0 += _Z;
|
||||
// z0 /= 2;
|
||||
printf_P(PSTR(" z0=%d\n"), z0);
|
||||
// xyzcal_lineXYZ_to(x0, y0, z0 - 100, 500, 1);
|
||||
// z0 = _Z - 10;
|
||||
|
||||
int8_t ret = 1;
|
||||
|
||||
#ifdef XYZCAL_FIND_CENTER_DIAGONAL
|
||||
int32_t xc = 0;
|
||||
int32_t yc = 0;
|
||||
int16_t ad = 45;
|
||||
for (; ad < 360; ad += 90)
|
||||
{
|
||||
float ar = (float)ad * _PI / 180;
|
||||
int16_t x = x0 + MAX_DIAMETR * cos(ar);
|
||||
int16_t y = y0 + MAX_DIAMETR * sin(ar);
|
||||
if (!xyzcal_lineXYZ_to(x, y, z0, delay_us, -1))
|
||||
{
|
||||
printf_P(PSTR("ERROR ad=%d\n"), ad);
|
||||
ret = 0;
|
||||
break;
|
||||
}
|
||||
xc += _X;
|
||||
yc += _Y;
|
||||
xyzcal_lineXYZ_to(x0, y0, z0, delay_us, 0);
|
||||
}
|
||||
if (ret)
|
||||
{
|
||||
printf_P(PSTR("OK\n"), ad);
|
||||
x0 = xc / 4;
|
||||
y0 = yc / 4;
|
||||
printf_P(PSTR(" x0=%d\n"), x0);
|
||||
printf_P(PSTR(" y0=%d\n"), y0);
|
||||
}
|
||||
|
||||
#else //XYZCAL_FIND_CENTER_DIAGONAL
|
||||
xyzcal_lineXYZ_to(x0 - MAX_DIAMETR, y0, z0, delay_us, -1);
|
||||
int16_t dx1 = x0 - _X;
|
||||
if (dx1 >= MAX_DIAMETR)
|
||||
{
|
||||
printf_P(PSTR("!!! dx1 = %d\n"), dx1);
|
||||
return 0;
|
||||
}
|
||||
xyzcal_lineXYZ_to(x0, y0, z0, delay_us, 0);
|
||||
xyzcal_lineXYZ_to(x0 + MAX_DIAMETR, y0, z0, delay_us, -1);
|
||||
int16_t dx2 = _X - x0;
|
||||
if (dx2 >= MAX_DIAMETR)
|
||||
{
|
||||
printf_P(PSTR("!!! dx2 = %d\n"), dx2);
|
||||
return 0;
|
||||
}
|
||||
xyzcal_lineXYZ_to(x0, y0, z0, delay_us, 0);
|
||||
xyzcal_lineXYZ_to(x0 , y0 - MAX_DIAMETR, z0, delay_us, -1);
|
||||
int16_t dy1 = y0 - _Y;
|
||||
if (dy1 >= MAX_DIAMETR)
|
||||
{
|
||||
printf_P(PSTR("!!! dy1 = %d\n"), dy1);
|
||||
return 0;
|
||||
}
|
||||
xyzcal_lineXYZ_to(x0, y0, z0, delay_us, 0);
|
||||
xyzcal_lineXYZ_to(x0, y0 + MAX_DIAMETR, z0, delay_us, -1);
|
||||
int16_t dy2 = _Y - y0;
|
||||
if (dy2 >= MAX_DIAMETR)
|
||||
{
|
||||
printf_P(PSTR("!!! dy2 = %d\n"), dy2);
|
||||
return 0;
|
||||
}
|
||||
printf_P(PSTR("dx1=%d\n"), dx1);
|
||||
printf_P(PSTR("dx2=%d\n"), dx2);
|
||||
printf_P(PSTR("dy1=%d\n"), dy1);
|
||||
printf_P(PSTR("dy2=%d\n"), dy2);
|
||||
|
||||
x0 += (dx2 - dx1) / 2;
|
||||
y0 += (dy2 - dy1) / 2;
|
||||
|
||||
printf_P(PSTR(" x0=%d\n"), x0);
|
||||
printf_P(PSTR(" y0=%d\n"), y0);
|
||||
|
||||
#endif //XYZCAL_FIND_CENTER_DIAGONAL
|
||||
|
||||
xyzcal_lineXYZ_to(x0, y0, z0, delay_us, 0);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
#ifdef XYZCAL_FIND_POINT_CENTER
|
||||
int8_t xyzcal_find_point_center(int16_t x0, int16_t y0, int16_t z0, int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t turns)
|
||||
{
|
||||
uint8_t n;
|
||||
uint16_t ad;
|
||||
float ar;
|
||||
float _cos;
|
||||
float _sin;
|
||||
int16_t r_min = 0;
|
||||
int16_t r_max = 0;
|
||||
int16_t x_min = 0;
|
||||
int16_t x_max = 0;
|
||||
int16_t y_min = 0;
|
||||
int16_t y_max = 0;
|
||||
int16_t r = 10;
|
||||
int16_t x = x0;
|
||||
int16_t y = y0;
|
||||
int16_t z = z0;
|
||||
int8_t _pinda = _PINDA;
|
||||
for (n = 0; n < turns; n++)
|
||||
{
|
||||
uint32_t r_sum = 0;
|
||||
for (ad = 0; ad < 720; ad++)
|
||||
{
|
||||
ar = ad * _PI / 360;
|
||||
_cos = cos(ar);
|
||||
_sin = sin(ar);
|
||||
x = x0 + (int)(_cos * r);
|
||||
y = y0 + (int)(_sin * r);
|
||||
xyzcal_lineXYZ_to(x, y, z, 1000, 0);
|
||||
int8_t pinda = _PINDA;
|
||||
if (pinda)
|
||||
r += 1;
|
||||
else
|
||||
{
|
||||
r -= 1;
|
||||
ad--;
|
||||
r_sum -= r;
|
||||
}
|
||||
if (ad == 0)
|
||||
{
|
||||
x_min = x0;
|
||||
x_max = x0;
|
||||
y_min = y0;
|
||||
y_max = y0;
|
||||
r_min = r;
|
||||
r_max = r;
|
||||
}
|
||||
else if (pinda)
|
||||
{
|
||||
if (x_min > x) x_min = (2*x + x_min) / 3;
|
||||
if (x_max < x) x_max = (2*x + x_max) / 3;
|
||||
if (y_min > y) y_min = (2*y + y_min) / 3;
|
||||
if (y_max < y) y_max = (2*y + y_max) / 3;
|
||||
/* if (x_min > x) x_min = x;
|
||||
if (x_max < x) x_max = x;
|
||||
if (y_min > y) y_min = y;
|
||||
if (y_max < y) y_max = y;*/
|
||||
if (r_min > r) r_min = r;
|
||||
if (r_max < r) r_max = r;
|
||||
}
|
||||
r_sum += r;
|
||||
/* if (_pinda != pinda)
|
||||
{
|
||||
if (pinda)
|
||||
DBG(_n("!1 x=%d y=%d\n"), x, y);
|
||||
else
|
||||
DBG(_n("!0 x=%d y=%d\n"), x, y);
|
||||
}*/
|
||||
_pinda = pinda;
|
||||
// DBG(_n("x=%d y=%d rx=%d ry=%d\n"), x, y, rx, ry);
|
||||
}
|
||||
DBG(_n("x_min=%d x_max=%d y_min=%d y_max=%d r_min=%d r_max=%d r_avg=%d\n"), x_min, x_max, y_min, y_max, r_min, r_max, r_sum / 720);
|
||||
if ((n > 2) && (n & 1))
|
||||
{
|
||||
x0 += (x_min + x_max);
|
||||
y0 += (y_min + y_max);
|
||||
x0 /= 3;
|
||||
y0 /= 3;
|
||||
int rx = (x_max - x_min) / 2;
|
||||
int ry = (y_max - y_min) / 2;
|
||||
r = (rx + ry) / 3;//(rx < ry)?rx:ry;
|
||||
DBG(_n("x0=%d y0=%d r=%d\n"), x0, y0, r);
|
||||
}
|
||||
}
|
||||
xyzcal_lineXYZ_to(x0, y0, z, 200, 0);
|
||||
}
|
||||
#endif //XYZCAL_FIND_POINT_CENTER
|
||||
|
||||
|
||||
uint8_t xyzcal_xycoords2point(int16_t x, int16_t y)
|
||||
{
|
||||
uint8_t ix = (x > 10000)?1:0;
|
||||
@ -742,91 +514,221 @@ bool xyzcal_searchZ(void)
|
||||
return false;
|
||||
}
|
||||
|
||||
bool xyzcal_scan_and_process(void)
|
||||
{
|
||||
/// returns value of any location within data
|
||||
/// uses bilinear interpolation
|
||||
float get_value(uint8_t * matrix_32x32, float c, float r){
|
||||
if (c <= 0 || r <= 0 || c >= 31 || r >= 31)
|
||||
return 0;
|
||||
|
||||
/// calculate weights of nearby points
|
||||
const float wc1 = c - floor(c);
|
||||
const float wr1 = r - floor(r);
|
||||
const float wc0 = 1 - wc1;
|
||||
const float wr0 = 1 - wr1;
|
||||
|
||||
const float w00 = wc0 * wr0;
|
||||
const float w01 = wc0 * wr1;
|
||||
const float w10 = wc1 * wr0;
|
||||
const float w11 = wc1 * wr1;
|
||||
|
||||
const uint16_t c0 = c;
|
||||
const uint16_t c1 = c0 + 1;
|
||||
const uint16_t r0 = r;
|
||||
const uint16_t r1 = r0 + 1;
|
||||
|
||||
const uint16_t idx00 = c0 + 32 * r0;
|
||||
const uint16_t idx01 = c0 + 32 * r1;
|
||||
const uint16_t idx10 = c1 + 32 * r0;
|
||||
const uint16_t idx11 = c1 + 32 * r1;
|
||||
|
||||
/// bilinear resampling
|
||||
return w00 * matrix_32x32[idx00] + w01 * matrix_32x32[idx01] + w10 * matrix_32x32[idx10] + w11 * matrix_32x32[idx11];
|
||||
}
|
||||
|
||||
const constexpr float m_infinity = -1000.f;
|
||||
|
||||
/// replaces the highest number by m_infinity
|
||||
void remove_highest(float *points, const uint8_t num_points){
|
||||
if (num_points <= 0)
|
||||
return;
|
||||
|
||||
float max = points[0];
|
||||
uint8_t max_i = 0;
|
||||
for (uint8_t i = 0; i < num_points; ++i){
|
||||
if (max < points[i]){
|
||||
max = points[i];
|
||||
max_i = i;
|
||||
}
|
||||
}
|
||||
points[max_i] = m_infinity;
|
||||
}
|
||||
|
||||
/// return the highest number in the list
|
||||
float highest(float *points, const uint8_t num_points){
|
||||
if (num_points <= 0)
|
||||
return 0;
|
||||
|
||||
float max = points[0];
|
||||
for (uint8_t i = 0; i < num_points; ++i){
|
||||
if (max < points[i]){
|
||||
max = points[i];
|
||||
}
|
||||
}
|
||||
return max;
|
||||
}
|
||||
|
||||
/// 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.
|
||||
/// Algorithm is stopped after fixed number of iterations. Move is limited to 0.5 px per iteration.
|
||||
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 shifts_x[blocks];
|
||||
float shifts_y[blocks];
|
||||
float shifts_r[blocks];
|
||||
|
||||
for (int8_t i = iterations; i > 0; --i){
|
||||
|
||||
// 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]);
|
||||
}
|
||||
// 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);
|
||||
|
||||
r = MAX(2, r);
|
||||
|
||||
}
|
||||
DBG(_n(" [%f, %f][%f] final circle\n"), x, y, r);
|
||||
}
|
||||
|
||||
/// Prints matrix in hex to debug output (serial line)
|
||||
void print_image(uint8_t *matrix_32x32){
|
||||
for (uint8_t y = 0; y < 32; ++y){
|
||||
const uint16_t idx_y = y * 32;
|
||||
for (uint8_t x = 0; x < 32; ++x){
|
||||
DBG(_n("%02x"), matrix_32x32[idx_y + x]);
|
||||
}
|
||||
DBG(_n("\n"));
|
||||
}
|
||||
DBG(_n("\n"));
|
||||
}
|
||||
|
||||
/// scans area around the current head location and
|
||||
/// searches for the center of the calibration pin
|
||||
bool xyzcal_scan_and_process(void){
|
||||
DBG(_n("sizeof(block_buffer)=%d\n"), sizeof(block_t)*BLOCK_BUFFER_SIZE);
|
||||
// DBG(_n("sizeof(pixels)=%d\n"), 32*32);
|
||||
// DBG(_n("sizeof(histo)=%d\n"), 2*16);
|
||||
// DBG(_n("sizeof(pattern)=%d\n"), 2*12);
|
||||
DBG(_n("sizeof(total)=%d\n"), 32*32+2*16+2*12);
|
||||
bool ret = false;
|
||||
int16_t x = _X;
|
||||
int16_t y = _Y;
|
||||
int16_t z = _Z;
|
||||
|
||||
uint8_t* pixels = (uint8_t*)block_buffer;
|
||||
xyzcal_scan_pixels_32x32(x, y, z - 72, 2400, 200, pixels);
|
||||
uint8_t *matrix32 = (uint8_t *)block_buffer;
|
||||
uint16_t *pattern = (uint16_t *)(matrix32 + 32 * 32);
|
||||
|
||||
uint16_t* histo = (uint16_t*)(pixels + 32*32);
|
||||
xyzcal_histo_pixels_32x32(pixels, histo);
|
||||
xyzcal_scan_pixels_32x32_Zhop(x, y, z - 72, 2400, 300, matrix32);
|
||||
print_image(matrix32);
|
||||
|
||||
xyzcal_adjust_pixels(pixels, histo);
|
||||
|
||||
uint16_t* pattern = (uint16_t*)(histo + 2*16);
|
||||
for (uint8_t i = 0; i < 12; i++)
|
||||
{
|
||||
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]);
|
||||
}
|
||||
uint8_t c = 0;
|
||||
uint8_t r = 0;
|
||||
if (xyzcal_find_pattern_12x12_in_32x32(pixels, pattern, &c, &r) > 66) //total pixels=144, corner=12 (1/2 = 66)
|
||||
{
|
||||
DBG(_n(" pattern found at %d %d\n"), c, r);
|
||||
c += 6;
|
||||
r += 6;
|
||||
x += ((int16_t)c - 16) << 6;
|
||||
y += ((int16_t)r - 16) << 6;
|
||||
DBG(_n(" x=%d y=%d z=%d\n"), x, y, z);
|
||||
|
||||
/// 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){
|
||||
/// find precise circle
|
||||
/// move to the center of the pattern (+5.5)
|
||||
float xf = uc + 5.5f;
|
||||
float yf = ur + 5.5f;
|
||||
float radius = 5; ///< default radius
|
||||
const uint8_t iterations = 20;
|
||||
dynamic_circle(matrix32, xf, yf, radius, iterations);
|
||||
if (ABS(xf - uc + 5.5f) > 3 || ABS(yf - ur + 5.5f) > 3 || ABS(radius - 5) > 3){
|
||||
/// dynamic algorithm diverged, use original position instead
|
||||
xf = uc + 5.5f;
|
||||
yf = ur + 5.5f;
|
||||
}
|
||||
|
||||
/// move to the center of area and convert to position
|
||||
xf = (float)x + (xf - 15.5f) * 64;
|
||||
yf = (float)y + (yf - 15.5f) * 64;
|
||||
DBG(_n(" [%f %f] mm pattern center\n"), pos_2_mm(xf), pos_2_mm(yf));
|
||||
x = round_to_i16(xf);
|
||||
y = round_to_i16(yf);
|
||||
xyzcal_lineXYZ_to(x, y, z, 200, 0);
|
||||
ret = true;
|
||||
}
|
||||
|
||||
/// wipe buffer
|
||||
for (uint16_t i = 0; i < sizeof(block_t)*BLOCK_BUFFER_SIZE; i++)
|
||||
pixels[i] = 0;
|
||||
matrix32[i] = 0;
|
||||
return ret;
|
||||
}
|
||||
|
||||
bool xyzcal_find_bed_induction_sensor_point_xy(void)
|
||||
{
|
||||
DBG(_n("xyzcal_find_bed_induction_sensor_point_xy x=%ld y=%ld z=%ld\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS]);
|
||||
bool xyzcal_find_bed_induction_sensor_point_xy(void){
|
||||
bool ret = false;
|
||||
|
||||
DBG(_n("xyzcal_find_bed_induction_sensor_point_xy x=%ld y=%ld z=%ld\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS]);
|
||||
st_synchronize();
|
||||
int16_t x = _X;
|
||||
int16_t y = _Y;
|
||||
int16_t z = _Z;
|
||||
pos_i16_t x = _X;
|
||||
pos_i16_t y = _Y;
|
||||
pos_i16_t z = _Z;
|
||||
|
||||
uint8_t point = xyzcal_xycoords2point(x, y);
|
||||
x = pgm_read_word((uint16_t*)(xyzcal_point_xcoords + point));
|
||||
y = pgm_read_word((uint16_t*)(xyzcal_point_ycoords + point));
|
||||
x = pgm_read_word((uint16_t *)(xyzcal_point_xcoords + point));
|
||||
y = pgm_read_word((uint16_t *)(xyzcal_point_ycoords + point));
|
||||
DBG(_n("point=%d x=%d y=%d z=%d\n"), point, x, y, z);
|
||||
xyzcal_meassure_enter();
|
||||
xyzcal_lineXYZ_to(x, y, z, 200, 0);
|
||||
if (xyzcal_searchZ())
|
||||
{
|
||||
|
||||
if (xyzcal_searchZ()){
|
||||
int16_t z = _Z;
|
||||
xyzcal_lineXYZ_to(x, y, z, 200, 0);
|
||||
if (xyzcal_scan_and_process())
|
||||
{
|
||||
if (xyzcal_find_point_center2(500))
|
||||
{
|
||||
uint32_t x_avg = 0;
|
||||
uint32_t y_avg = 0;
|
||||
uint8_t n; for (n = 0; n < 4; n++)
|
||||
{
|
||||
if (!xyzcal_find_point_center2(1000)) break;
|
||||
x_avg += _X;
|
||||
y_avg += _Y;
|
||||
}
|
||||
if (n == 4)
|
||||
{
|
||||
xyzcal_lineXYZ_to(x_avg >> 2, y_avg >> 2, _Z, 200, 0);
|
||||
ret = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
ret = xyzcal_scan_and_process();
|
||||
}
|
||||
xyzcal_meassure_leave();
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
#endif //NEW_XYZCAL
|
||||
|
||||
@ -17,20 +17,6 @@ extern bool xyzcal_spiral8(int16_t cx, int16_t cy, int16_t z0, int16_t dz, int16
|
||||
|
||||
//extern int8_t xyzcal_meassure_pinda_hysterezis(int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t samples);
|
||||
|
||||
extern void xyzcal_scan_pixels_32x32(int16_t cx, int16_t cy, int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t* pixels);
|
||||
|
||||
extern void xyzcal_histo_pixels_32x32(uint8_t* pixels, uint16_t* histo);
|
||||
|
||||
extern void xyzcal_adjust_pixels(uint8_t* pixels, uint16_t* histo);
|
||||
|
||||
extern int16_t xyzcal_match_pattern_12x12_in_32x32(uint16_t* pattern, uint8_t* pixels, uint8_t x, uint8_t y);
|
||||
|
||||
extern int16_t xyzcal_find_pattern_12x12_in_32x32(uint8_t* pixels, uint16_t* pattern, uint8_t* pc, uint8_t* pr);
|
||||
|
||||
extern int8_t xyzcal_find_point_center2(uint16_t delay_us);
|
||||
|
||||
//extern int8_t xyzcal_find_point_center(int16_t x0, int16_t y0, int16_t z0, int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t turns);
|
||||
|
||||
extern bool xyzcal_searchZ(void);
|
||||
|
||||
extern bool xyzcal_scan_and_process(void);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user