diff --git a/Firmware/sm4.c b/Firmware/sm4.c index dab47042..cf5ac16e 100644 --- a/Firmware/sm4.c +++ b/Firmware/sm4.c @@ -191,5 +191,45 @@ uint16_t sm4_line_xyze_ui(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de) return nd; } +uint16_t sm4_line_xyz_ui(uint16_t dx, uint16_t dy, uint16_t dz){ + uint16_t dd = (uint16_t)(sqrt((float)(((uint32_t)dx)*dx + ((uint32_t)dy*dy) + ((uint32_t)dz*dz))) + 0.5); + uint16_t nd = dd; + uint16_t cx = dd; + uint16_t cy = dd; + uint16_t cz = dd; + uint16_t x = 0; + uint16_t y = 0; + uint16_t z = 0; + while (nd){ + if (sm4_stop_cb && (*sm4_stop_cb)()) break; + uint8_t sm = 0; //step mask + if (cx <= dx){ + sm |= 1; + cx += dd; + x++; + } + if (cy <= dy){ + sm |= 2; + cy += dd; + y++; + } + if (cz <= dz){ + sm |= 4; + cz += dd; + z++; + } + cx -= dx; + cy -= dy; + cz -= dz; + sm4_do_step(sm); + uint16_t delay = SM4_DEFDELAY; + if (sm4_calc_delay_cb) delay = (*sm4_calc_delay_cb)(nd, dd); + if (delay) delayMicroseconds(delay); + nd--; + } + if (sm4_update_pos_cb) + (*sm4_update_pos_cb)(x, y, z, 0); + return nd; +} #endif //NEW_XYZCAL diff --git a/Firmware/sm4.h b/Firmware/sm4.h index fc64f7a6..ce110caa 100644 --- a/Firmware/sm4.h +++ b/Firmware/sm4.h @@ -1,56 +1,57 @@ -//sm4.h - simple 4-axis stepper control -#ifndef _SM4_H -#define _SM4_H - -#include -#include "config.h" - - -#if defined(__cplusplus) -extern "C" { -#endif //defined(__cplusplus) - - -// callback prototype for stop condition (return 0 - continue, return 1 - stop) -typedef uint8_t (*sm4_stop_cb_t)(); - -// callback prototype for updating position counters -typedef void (*sm4_update_pos_cb_t)(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de); - -// callback prototype for calculating delay -typedef uint16_t (*sm4_calc_delay_cb_t)(uint16_t nd, uint16_t dd); - - -// callback pointer - stop -extern sm4_stop_cb_t sm4_stop_cb; - -// callback pointer - update_pos -extern sm4_update_pos_cb_t sm4_update_pos_cb; - -// callback pointer - calc_delay -extern sm4_calc_delay_cb_t sm4_calc_delay_cb; - - -// returns direction for single axis (0 - positive, 1 - negative) -extern uint8_t sm4_get_dir(uint8_t axis); - -// set direction for single axis (0 - positive, 1 - negative) -extern void sm4_set_dir(uint8_t axis, uint8_t dir); - -// returns direction of all axes as bitmask (0 - positive, 1 - negative) -extern uint8_t sm4_get_dir_bits(void); - -// set direction for all axes as bitmask (0 - positive, 1 - negative) -extern void sm4_set_dir_bits(uint8_t dir_bits); - -// step axes by bitmask -extern void sm4_do_step(uint8_t axes_mask); - -// xyze linear-interpolated relative move, returns remaining diagonal steps (>0 means stoped) -extern uint16_t sm4_line_xyze_ui(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de); - - -#if defined(__cplusplus) -} -#endif //defined(__cplusplus) -#endif //_SM4_H +//sm4.h - simple 4-axis stepper control +#ifndef _SM4_H +#define _SM4_H + +#include +#include "config.h" + + +#if defined(__cplusplus) +extern "C" { +#endif //defined(__cplusplus) + + +// callback prototype for stop condition (return 0 - continue, return 1 - stop) +typedef uint8_t (*sm4_stop_cb_t)(); + +// callback prototype for updating position counters +typedef void (*sm4_update_pos_cb_t)(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de); + +// callback prototype for calculating delay +typedef uint16_t (*sm4_calc_delay_cb_t)(uint16_t nd, uint16_t dd); + + +// callback pointer - stop +extern sm4_stop_cb_t sm4_stop_cb; + +// callback pointer - update_pos +extern sm4_update_pos_cb_t sm4_update_pos_cb; + +// callback pointer - calc_delay +extern sm4_calc_delay_cb_t sm4_calc_delay_cb; + + +// returns direction for single axis (0 - positive, 1 - negative) +extern uint8_t sm4_get_dir(uint8_t axis); + +// set direction for single axis (0 - positive, 1 - negative) +extern void sm4_set_dir(uint8_t axis, uint8_t dir); + +// returns direction of all axes as bitmask (0 - positive, 1 - negative) +extern uint8_t sm4_get_dir_bits(void); + +// set direction for all axes as bitmask (0 - positive, 1 - negative) +extern void sm4_set_dir_bits(uint8_t dir_bits); + +// step axes by bitmask +extern void sm4_do_step(uint8_t axes_mask); + +// xyze linear-interpolated relative move, returns remaining diagonal steps (>0 means stoped) +extern uint16_t sm4_line_xyze_ui(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de); +extern uint16_t sm4_line_xyz_ui(uint16_t dx, uint16_t dy, uint16_t dz); + + +#if defined(__cplusplus) +} +#endif //defined(__cplusplus) +#endif //_SM4_H diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp index 47eecb94..aed79073 100644 --- a/Firmware/xyzcal.cpp +++ b/Firmware/xyzcal.cpp @@ -252,7 +252,7 @@ bool xyzcal_lineXYZ_to(int16_t x, int16_t y, int16_t z, uint16_t delay_us, int8_ 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; + bool ret = sm4_line_xyz_ui(abs(x), abs(y), abs(z)) ? true : false; // u = _micros() - u; return ret; } @@ -689,25 +689,6 @@ uint8_t xyzcal_find_pattern_12x12_in_32x32(uint8_t* pixels, uint16_t* pattern, u return max_match; } -uint8_t xyzcal_xycoords2point(int16_t x, int16_t y) -{ - uint8_t ix = (x > 10000)?1:0; - uint8_t iy = (y > 10000)?1:0; - return iy?(3-ix):ix; -} - -//MK3 -#if ((MOTHERBOARD == BOARD_EINSY_1_0a)) -const int16_t xyzcal_point_xcoords[4] PROGMEM = {1200, 22000, 22000, 1200}; -const int16_t xyzcal_point_ycoords[4] PROGMEM = {600, 600, 19800, 19800}; -#endif //((MOTHERBOARD == BOARD_EINSY_1_0a)) - -//MK2.5 -#if ((MOTHERBOARD == BOARD_RAMBO_MINI_1_0) || (MOTHERBOARD == BOARD_RAMBO_MINI_1_3)) -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_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}; @@ -939,8 +920,7 @@ bool xyzcal_scan_and_process(void){ float radius = 4.5f; ///< default radius const uint8_t iterations = 20; dynamic_circle(matrix32, xf, yf, radius, iterations); - if (ABS(xf - (uc + 5.5f)) > 3 || ABS(yf - (ur + 5.5f)) > 3 || ABS(radius - 5) > 3) - { + if (fabs(xf - (uc + 5.5f)) > 3 || fabs(yf - (ur + 5.5f)) > 3 || fabs(radius - 5) > 3){ DBG(_n(" [%f %f][%f] mm divergence\n"), xf - (uc + 5.5f), yf - (ur + 5.5f), radius - 5); /// dynamic algorithm diverged, use original position instead xf = uc + 5.5f; @@ -974,13 +954,7 @@ bool xyzcal_find_bed_induction_sensor_point_xy(void){ ///< magic constant, lowers min_z after searchZ to obtain more dense data in scan const pos_i16_t lower_z = 72; - 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)); - 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()){ xyzcal_lineXYZ_to(_X, _Y, _Z - lower_z, 200, 0); ret = xyzcal_scan_and_process();