diff --git a/Firmware/Configuration_prusa.h b/Firmware/Configuration_prusa.h index 7cf413c3..c0658e68 100644 --- a/Firmware/Configuration_prusa.h +++ b/Firmware/Configuration_prusa.h @@ -95,6 +95,12 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o // Automatic recovery after crash is detected #define AUTOMATIC_RECOVERY_AFTER_CRASH +// New XYZ calibration +#define NEW_XYZCAL + +// Watchdog support +#define WATCHDOG + // Disable some commands #define _DISABLE_M42_M226 diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp index e2018ee1..f91bf32a 100644 --- a/Firmware/Marlin_main.cpp +++ b/Firmware/Marlin_main.cpp @@ -2286,6 +2286,7 @@ bool gcode_M45(bool onlyZ) current_position[Z_AXIS] = MESH_HOME_Z_SEARCH; plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[Z_AXIS] / 40, active_extruder); st_synchronize(); +#ifndef NEW_XYZCAL if (result >= 0) { point_too_far_mask = 0; @@ -2304,6 +2305,8 @@ bool gcode_M45(bool onlyZ) st_synchronize(); // if (result >= 0) babystep_apply(); } +#endif //NEW_XYZCAL + lcd_bed_calibration_show_result(result, point_too_far_mask); if (result >= 0) { @@ -3276,17 +3279,17 @@ void process_commands() enquecommand_front_P((PSTR("G28 W0"))); break; } - lcd_show_fullscreen_message_and_wait_P(MSG_TEMP_CAL_WARNING); - bool result = lcd_show_fullscreen_message_yes_no_and_wait_P(MSG_STEEL_SHEET_CHECK, false, false); - if (result) - { + lcd_show_fullscreen_message_and_wait_P(MSG_TEMP_CAL_WARNING); + bool result = lcd_show_fullscreen_message_yes_no_and_wait_P(MSG_STEEL_SHEET_CHECK, false, false); + if (result) + { current_position[Z_AXIS] = 50; current_position[Y_AXIS] = 190; plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000 / 60, active_extruder); st_synchronize(); - lcd_show_fullscreen_message_and_wait_P(MSG_REMOVE_STEEL_SHEET); - } - lcd_update_enable(true); + lcd_show_fullscreen_message_and_wait_P(MSG_REMOVE_STEEL_SHEET); + } + lcd_update_enable(true); KEEPALIVE_STATE(NOT_BUSY); //no need to print busy messages as we print current temperatures periodicaly SERIAL_ECHOLNPGM("PINDA probe calibration start"); diff --git a/Firmware/config.h b/Firmware/config.h index a1014ca3..8bd1a16e 100644 --- a/Firmware/config.h +++ b/Firmware/config.h @@ -8,5 +8,8 @@ #define ADC_OVRSAMPL 16 //oversampling multiplier #define ADC_CALLBACK adc_ready //callback function () +//SM4 configuration +#define SM4_DEFDELAY 500 //default step delay [us] + #endif //_CONFIG_H diff --git a/Firmware/mesh_bed_calibration.cpp b/Firmware/mesh_bed_calibration.cpp index 0d6cd87e..bcdfb0d2 100644 --- a/Firmware/mesh_bed_calibration.cpp +++ b/Firmware/mesh_bed_calibration.cpp @@ -898,6 +898,9 @@ error: return false; } +#ifdef NEW_XYZCAL +extern bool xyzcal_find_bed_induction_sensor_point_xy(); +#endif //NEW_XYZCAL // Search around the current_position[X,Y], // look for the induction sensor response. // Adjust the current_position[X,Y,Z] to the center of the target dot and its response Z coordinate. @@ -905,8 +908,11 @@ error: #define FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS (6.f) #define FIND_BED_INDUCTION_SENSOR_POINT_XY_STEP (1.f) #define FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP (0.2f) -inline bool find_bed_induction_sensor_point_xy(int verbosity_level) +/*inline */bool find_bed_induction_sensor_point_xy(int verbosity_level) { +#ifdef NEW_XYZCAL + return xyzcal_find_bed_induction_sensor_point_xy(); +#else //NEW_XYZCAL #ifdef SUPPORT_VERBOSITY if(verbosity_level >= 10) MYSERIAL.println("find bed induction sensor point xy"); #endif // SUPPORT_VERBOSITY @@ -1099,8 +1105,10 @@ inline bool find_bed_induction_sensor_point_xy(int verbosity_level) enable_z_endstop(false); return found; +#endif //NEW_XYZCAL } +#ifndef NEW_XYZCAL // Search around the current_position[X,Y,Z]. // It is expected, that the induction sensor is switched on at the current position. // Look around this center point by painting a star around the point. @@ -1190,7 +1198,9 @@ inline bool improve_bed_induction_sensor_point() enable_z_endstop(endstop_z_enabled); return found; } +#endif //NEW_XYZCAL +#ifndef NEW_XYZCAL static inline void debug_output_point(const char *type, const float &x, const float &y, const float &z) { SERIAL_ECHOPGM("Measured "); @@ -1203,7 +1213,9 @@ static inline void debug_output_point(const char *type, const float &x, const fl MYSERIAL.print(z, 5); SERIAL_ECHOLNPGM(""); } +#endif //NEW_XYZCAL +#ifndef NEW_XYZCAL // Search around the current_position[X,Y,Z]. // It is expected, that the induction sensor is switched on at the current position. // Look around this center point by painting a star around the point. @@ -1363,7 +1375,9 @@ canceled: go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f); return false; } +#endif //NEW_XYZCAL +#ifndef NEW_XYZCAL // Searching the front points, where one cannot move the sensor head in front of the sensor point. // Searching in a zig-zag movement in a plane for the maximum width of the response. // This function may set the current_position[Y_AXIS] below Y_MIN_POS, if the function succeeded. @@ -1684,7 +1698,9 @@ canceled: go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f); return false; } +#endif //NEW_XYZCAL +#ifndef NEW_XYZCAL // Scan the mesh bed induction points one by one by a left-right zig-zag movement, // write the trigger coordinates to the serial line. // Useful for visualizing the behavior of the bed induction detector. @@ -1729,6 +1745,7 @@ inline void scan_bed_induction_sensor_point() current_position[Y_AXIS] = center_old_y; go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f); } +#endif //NEW_XYZCAL #define MESH_BED_CALIBRATION_SHOW_LCD @@ -1855,7 +1872,7 @@ BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level #endif // SUPPORT_VERBOSITY if (!find_bed_induction_sensor_point_xy(verbosity_level)) return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND; -#if 1 +#ifndef NEW_XYZCAL if (k == 0 || k == 1) { // Improve the position of the 1st row sensor points by a zig-zag movement. @@ -2017,6 +2034,7 @@ BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level return result; } +#ifndef NEW_XYZCAL BedSkewOffsetDetectionResultType improve_bed_offset_and_skew(int8_t method, int8_t verbosity_level, uint8_t &too_far_mask) { // Don't let the manage_inactivity() function remove power from the motors. @@ -2322,6 +2340,7 @@ canceled: enable_z_endstop(endstop_z_enabled); return result; } +#endif //NEW_XYZCAL void go_home_with_z_lift() { @@ -2462,6 +2481,7 @@ bool sample_mesh_and_store_reference() return true; } +#ifndef NEW_XYZCAL bool scan_bed_induction_points(int8_t verbosity_level) { // Don't let the manage_inactivity() function remove power from the motors. @@ -2523,6 +2543,7 @@ bool scan_bed_induction_points(int8_t verbosity_level) enable_z_endstop(endstop_z_enabled); return true; } +#endif //NEW_XYZCAL // Shift a Z axis by a given delta. // To replace loading of the babystep correction. diff --git a/Firmware/mesh_bed_calibration.h b/Firmware/mesh_bed_calibration.h index 4f6ebd72..c5789fac 100644 --- a/Firmware/mesh_bed_calibration.h +++ b/Firmware/mesh_bed_calibration.h @@ -161,7 +161,9 @@ enum BedSkewOffsetDetectionResultType { }; extern BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level, uint8_t &too_far_mask); +#ifndef NEW_XYZCAL extern BedSkewOffsetDetectionResultType improve_bed_offset_and_skew(int8_t method, int8_t verbosity_level, uint8_t &too_far_mask); +#endif //NEW_XYZCAL extern bool sample_mesh_and_store_reference(); diff --git a/Firmware/sm4.c b/Firmware/sm4.c new file mode 100644 index 00000000..4ea47271 --- /dev/null +++ b/Firmware/sm4.c @@ -0,0 +1,194 @@ +//sm4.c - simple 4-axis stepper control + +#include "sm4.h" +#include +#include + +#include "boards.h" +#define bool int8_t +#define false 0 +#define true 1 +#include "Configuration_prusa.h" + + +#ifdef NEW_XYZCAL + + +// Signal pinouts + +// direction signal - MiniRambo +//#define X_DIR_PIN 48 //PL1 (-) +//#define Y_DIR_PIN 49 //PL0 (-) +//#define Z_DIR_PIN 47 //PL2 (-) +//#define E0_DIR_PIN 43 //PL6 (+) + +//direction signal - EinsyRambo +//#define X_DIR_PIN 49 //PL0 (+) +//#define Y_DIR_PIN 48 //PL1 (-) +//#define Z_DIR_PIN 47 //PL2 (+) +//#define E0_DIR_PIN 43 //PL6 (-) + +//step signal pinout - common for all rambo boards +//#define X_STEP_PIN 37 //PC0 (+) +//#define Y_STEP_PIN 36 //PC1 (+) +//#define Z_STEP_PIN 35 //PC2 (+) +//#define E0_STEP_PIN 34 //PC3 (+) + + +sm4_stop_cb_t sm4_stop_cb = 0; + +sm4_update_pos_cb_t sm4_update_pos_cb = 0; + +sm4_calc_delay_cb_t sm4_calc_delay_cb = 0; + +uint16_t sm4_cpu_time = 0; + + +uint8_t sm4_get_dir(uint8_t axis) +{ + switch (axis) + { +#if ((MOTHERBOARD == 200) || (MOTHERBOARD == 203)) + case 0: return (PORTL & 2)?0:1; + case 1: return (PORTL & 1)?0:1; + case 2: return (PORTL & 4)?0:1; + case 3: return (PORTL & 64)?1:0; +#else if ((MOTHERBOARD == 303) || (MOTHERBOARD == 304)) + case 0: return (PORTL & 1)?1:0; + case 1: return (PORTL & 2)?0:1; + case 2: return (PORTL & 4)?1:0; + case 3: return (PORTL & 64)?0:1; +#endif + } + return 0; +} + +void sm4_set_dir(uint8_t axis, uint8_t dir) +{ + switch (axis) + { +#if ((MOTHERBOARD == 200) || (MOTHERBOARD == 203)) + case 0: if (!dir) PORTL |= 2; else PORTL &= ~2; break; + case 1: if (!dir) PORTL |= 1; else PORTL &= ~1; break; + case 2: if (!dir) PORTL |= 4; else PORTL &= ~4; break; + case 3: if (dir) PORTL |= 64; else PORTL &= ~64; break; +#else if ((MOTHERBOARD == 303) || (MOTHERBOARD == 304)) + case 0: if (dir) PORTL |= 1; else PORTL &= ~1; break; + case 1: if (!dir) PORTL |= 2; else PORTL &= ~2; break; + case 2: if (dir) PORTL |= 4; else PORTL &= ~4; break; + case 3: if (!dir) PORTL |= 64; else PORTL &= ~64; break; +#endif + } + asm("nop"); +} + +uint8_t sm4_get_dir_bits(void) +{ + uint8_t register dir_bits = 0; + uint8_t register portL = PORTL; + //TODO -optimize in asm +#if ((MOTHERBOARD == 200) || (MOTHERBOARD == 203)) + if (portL & 2) dir_bits |= 1; + if (portL & 1) dir_bits |= 2; + if (portL & 4) dir_bits |= 4; + if (portL & 64) dir_bits |= 8; + dir_bits ^= 0x07; //invert XYZ, do not invert E +#else if ((MOTHERBOARD == 303) || (MOTHERBOARD == 304)) + if (portL & 1) dir_bits |= 1; + if (portL & 2) dir_bits |= 2; + if (portL & 4) dir_bits |= 4; + if (portL & 64) dir_bits |= 8; + dir_bits ^= 0x0a; //invert YE, do not invert XZ +#endif + return dir_bits; +} + +void sm4_set_dir_bits(uint8_t dir_bits) +{ + uint8_t register portL = PORTL; + portL &= 0xb8; //set direction bits to zero + //TODO -optimize in asm +#if ((MOTHERBOARD == 200) || (MOTHERBOARD == 203)) + dir_bits ^= 0x07; //invert XYZ, do not invert E + if (dir_bits & 1) portL |= 2; //set X direction bit + if (dir_bits & 2) portL |= 1; //set Y direction bit + if (dir_bits & 4) portL |= 4; //set Z direction bit + if (dir_bits & 8) portL |= 64; //set E direction bit +#else if ((MOTHERBOARD == 303) || (MOTHERBOARD == 304)) + dir_bits ^= 0x0a; //invert YE, do not invert XZ + if (dir_bits & 1) portL |= 1; //set X direction bit + if (dir_bits & 2) portL |= 2; //set Y direction bit + if (dir_bits & 4) portL |= 4; //set Z direction bit + if (dir_bits & 8) portL |= 64; //set E direction bit +#endif + PORTL = portL; + asm("nop"); +} + +void sm4_do_step(uint8_t axes_mask) +{ +#if ((MOTHERBOARD == 200) || (MOTHERBOARD == 203) || (MOTHERBOARD == 303) || (MOTHERBOARD == 304)) + uint8_t register portC = PORTC & 0xf0; + PORTC = portC | (axes_mask & 0x0f); //set step signals by mask + asm("nop"); + PORTC = portC; //set step signals to zero + asm("nop"); +#endif //((MOTHERBOARD == 200) || (MOTHERBOARD == 203) || (MOTHERBOARD == 303) || (MOTHERBOARD == 304)) +} + +uint16_t sm4_line_xyze_ui(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de) +{ + uint16_t dd = (uint16_t)(sqrt((float)(((uint32_t)dx)*dx + ((uint32_t)dy*dy) + ((uint32_t)dz*dz) + ((uint32_t)de*de))) + 0.5); + uint16_t nd = dd; + uint16_t cx = dd; + uint16_t cy = dd; + uint16_t cz = dd; + uint16_t ce = dd; + uint16_t x = 0; + uint16_t y = 0; + uint16_t z = 0; + uint16_t e = 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++; + } + if (ce <= de) + { + sm |= 4; + ce += dd; + e++; + } + cx -= dx; + cy -= dy; + cz -= dz; + ce -= de; + 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, e); + return nd; +} + + +#endif //NEW_XYZCAL \ No newline at end of file diff --git a/Firmware/sm4.h b/Firmware/sm4.h new file mode 100644 index 00000000..fc64f7a6 --- /dev/null +++ b/Firmware/sm4.h @@ -0,0 +1,56 @@ +//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 diff --git a/Firmware/temperature.h b/Firmware/temperature.h index 4414e762..a8b70b6a 100644 --- a/Firmware/temperature.h +++ b/Firmware/temperature.h @@ -27,6 +27,9 @@ #include "stepper.h" #endif +#define ENABLE_TEMPERATURE_INTERRUPT() TIMSK0 |= (1< +#include "stepper.h" +#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 + +#define ENABLE_FANCHECK_INTERRUPT() EIMSK |= (1<<7) +#define DISABLE_FANCHECK_INTERRUPT() EIMSK &= ~(1<<7) + +#define _PINDA ((READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING)?1:0) + +//#define DBG(args...) printf_P(args) +#define DBG(args...) +#define _n PSTR + +#define _X ((int16_t)count_position[X_AXIS]) +#define _Y ((int16_t)count_position[Y_AXIS]) +#define _Z ((int16_t)count_position[Z_AXIS]) +#define _E ((int16_t)count_position[E_AXIS]) + +#define _PI 3.14159265F + +extern long count_position[NUM_AXIS]; + +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); + + +void xyzcal_meassure_enter(void) +{ + DBG(_n("xyzcal_meassure_enter\n")); + disable_heater(); + DISABLE_TEMPERATURE_INTERRUPT(); +#if (defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1)) + DISABLE_FANCHECK_INTERRUPT(); +#endif //(defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1)) + DISABLE_STEPPER_DRIVER_INTERRUPT(); +#ifdef WATCHDOG + wdt_disable(); +#endif //WATCHDOG + sm4_stop_cb = 0; + sm4_update_pos_cb = xyzcal_update_pos; + sm4_calc_delay_cb = xyzcal_calc_delay; +} + +void xyzcal_meassure_leave(void) +{ + DBG(_n("xyzcal_meassure_leave\n")); + planner_abort_hard(); + ENABLE_TEMPERATURE_INTERRUPT(); +#if (defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1)) + ENABLE_FANCHECK_INTERRUPT(); +#endif //(defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1)) + ENABLE_STEPPER_DRIVER_INTERRUPT(); +#ifdef WATCHDOG + wdt_enable(WDTO_4S); +#endif //WATCHDOG + sm4_stop_cb = 0; + sm4_update_pos_cb = 0; + sm4_calc_delay_cb = 0; +} + + +uint8_t check_pinda_0() +{ + return _PINDA?0:1; +} + +uint8_t check_pinda_1() +{ + return _PINDA?1:0; +} + +uint8_t xyzcal_dm = 0; + +void xyzcal_update_pos(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de) +{ + DBG(_n("xyzcal_update_pos dx=%d dy=%d dz=%d dir=%02x\n"), dx, dy, dz, xyzcal_dm); + if (xyzcal_dm&1) count_position[0] -= dx; else count_position[0] += dx; + if (xyzcal_dm&2) count_position[1] -= dy; else count_position[1] += dy; + if (xyzcal_dm&4) count_position[2] -= dz; else count_position[2] += dz; + DBG(_n(" after xyzcal_update_pos x=%ld y=%ld z=%ld\n"), count_position[0], count_position[1], count_position[2]); +} + +uint16_t xyzcal_sm4_delay = 0; + +//#define SM4_ACCEL_TEST +#ifdef SM4_ACCEL_TEST +uint16_t xyzcal_sm4_v0 = 2000; +uint16_t xyzcal_sm4_vm = 45000; +uint16_t xyzcal_sm4_v = xyzcal_sm4_v0; +uint16_t xyzcal_sm4_ac = 2000; +uint16_t xyzcal_sm4_ac2 = (uint32_t)xyzcal_sm4_ac * 1024 / 10000; +//float xyzcal_sm4_vm = 10000; +#endif //SM4_ACCEL_TEST + +uint16_t xyzcal_calc_delay(uint16_t nd, uint16_t dd) +{ + return xyzcal_sm4_delay; +#ifdef SM4_ACCEL_TEST + + uint16_t del_us = 0; + if (xyzcal_sm4_v & 0xf000) //>=4096 + { + del_us = (uint16_t)62500 / (uint16_t)(xyzcal_sm4_v >> 4); + xyzcal_sm4_v += (xyzcal_sm4_ac2 * del_us + 512) >> 10; + if (xyzcal_sm4_v > xyzcal_sm4_vm) xyzcal_sm4_v = xyzcal_sm4_vm; + if (del_us > 25) return del_us - 25; + } + else + { + del_us = (uint32_t)1000000 / xyzcal_sm4_v; + xyzcal_sm4_v += ((uint32_t)xyzcal_sm4_ac2 * del_us + 512) >> 10; + if (xyzcal_sm4_v > xyzcal_sm4_vm) xyzcal_sm4_v = xyzcal_sm4_vm; + if (del_us > 50) return del_us - 50; + } + +// uint16_t del_us = (uint16_t)(((float)1000000 / xyzcal_sm4_v) + 0.5); +// uint16_t del_us = (uint32_t)1000000 / xyzcal_sm4_v; +// uint16_t del_us = 100; +// uint16_t del_us = (uint16_t)10000 / xyzcal_sm4_v; +// v += (ac * del_us + 500) / 1000; +// xyzcal_sm4_v += (xyzcal_sm4_ac * del_us) / 1000; +// return xyzcal_sm4_delay; +// DBG(_n("xyzcal_calc_delay nd=%d dd=%d v=%d del_us=%d\n"), nd, dd, xyzcal_sm4_v, del_us); + return 0; +#endif //SM4_ACCEL_TEST +} + + +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); + x -= (int16_t)count_position[0]; + y -= (int16_t)count_position[1]; + z -= (int16_t)count_position[2]; + xyzcal_dm = ((x<0)?1:0) | ((y<0)?2:0) | ((z<0)?4:0); + 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; + return ret; +} + +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; + float r = 0; //radius + uint8_t n = 0; //point number + uint16_t ad = 0; //angle [deg] + float ar; //angle [rad] + uint8_t dad = 0; //delta angle [deg] + uint8_t dad_min = 4; //delta angle min [deg] + uint8_t dad_max = 16; //delta angle max [deg] + 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); + for (; ad < 720; ad++) + { + if (radius > 0) + { + dad = dad_max - (ad / k); + r = (float)(((uint32_t)ad) * radius) / 720; + } + else + { + dad = dad_max - ((719 - ad) / k); + r = (float)(((uint32_t)(719 - ad)) * (-radius)) / 720; + } + ar = (ad + rotation)* (float)_PI / 180; + float _cos = cos(ar); + float _sin = sin(ar); + int x = (int)(cx + (_cos * r)); + int y = (int)(cy + (_sin * r)); + int z = (int)(z0 - ((float)((int32_t)dz * ad) / 720)); + if (xyzcal_lineXYZ_to(x, y, z, delay_us, check_pinda)) + { + ad += dad + 1; + ret = true; + break; + } + n++; + ad += dad; + } + if (pad) *pad = ad; + return ret; +} + +bool xyzcal_spiral8(int16_t cx, int16_t cy, int16_t z0, int16_t dz, int16_t radius, uint16_t delay_us, int8_t check_pinda, uint16_t* pad) +{ + bool ret = false; + uint16_t ad = 0; + if (pad) ad = *pad; + DBG(_n("xyzcal_spiral8 cx=%d cy=%d z0=%d dz=%d radius=%d ad=%d\n"), cx, cy, z0, dz, radius, ad); + if (!ret && (ad < 720)) + if (ret = xyzcal_spiral2(cx, cy, z0 - 0*dz, dz, radius, 0, delay_us, check_pinda, &ad)) + ad += 0; + if (!ret && (ad < 1440)) + if (ret = xyzcal_spiral2(cx, cy, z0 - 1*dz, dz, -radius, 0, delay_us, check_pinda, &ad)) + ad += 720; + if (!ret && (ad < 2160)) + if (ret = xyzcal_spiral2(cx, cy, z0 - 2*dz, dz, radius, 180, delay_us, check_pinda, &ad)) + ad += 1440; + if (!ret && (ad < 2880)) + if (ret = xyzcal_spiral2(cx, cy, z0 - 3*dz, dz, -radius, 180, delay_us, check_pinda, &ad)) + ad += 2160; + if (pad) *pad = ad; + return ret; +} + +#ifdef XYZCAL_MEASSURE_PINDA_HYSTEREZIS +int8_t xyzcal_meassure_pinda_hysterezis(int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t samples) +{ + DBG(_n("xyzcal_meassure_pinda_hysterezis\n")); + int8_t ret = -1; // PINDA signal error + int16_t z = _Z; + int16_t sum_up = 0; + int16_t sum_dn = 0; + int16_t up; + int16_t dn; + uint8_t sample; + xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 1); + xyzcal_lineXYZ_to(_X, _Y, max_z, delay_us, -1); + if (!_PINDA) + { + for (sample = 0; sample < samples; sample++) + { + dn = _Z; + if (!xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 1)) break; + dn = dn - _Z; + up = _Z; + if (!xyzcal_lineXYZ_to(_X, _Y, max_z, delay_us, -1)) break; + up = _Z - up; + DBG(_n("%d. up=%d dn=%d\n"), sample, up, dn); + sum_up += up; + sum_dn += dn; + if (abs(up - dn) > XYZCAL_PINDA_HYST_DIF) + { + ret = -2; // difference between up-dn to high + break; + } + } + if (sample == samples) + { + up = sum_up / samples; + dn = sum_dn / samples; + uint16_t hyst = (up + dn) / 2; + if (abs(up - dn) > XYZCAL_PINDA_HYST_DIF) + ret = -2; // difference between up-dn to high + else if ((hyst < XYZCAL_PINDA_HYST_MIN) || (hyst > XYZCAL_PINDA_HYST_MAX)) + ret = -3; // hysterezis out of range + else + ret = hyst; + } + } + xyzcal_lineXYZ_to(_X, _Y, z, delay_us, 0); + return ret; +} +#endif //XYZCAL_MEASSURE_PINDA_HYSTEREZIS + + +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--; + } + } + else + { + if (z < max_z) + { + sm4_do_step(Z_AXIS_MASK); + z++; + } + } + 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[0]; + uint8_t max_l = 0; + 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 = 15; l > 8; l--) + if (histo[l] >= 10) + break; + uint8_t pix_min = (max_l + 3) << 4; + uint8_t pix_max = l << 4; + 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; + } + } +} +*/ + +int16_t xyzcal_match_pattern_12x12_in_32x32(uint16_t* pattern, uint8_t* pixels, uint8_t c, uint8_t r) +{ + uint8_t thr = 64; + 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 + 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 --; + } + } + return match; +} + +int16_t xyzcal_find_pattern_12x12_in_32x32(uint8_t* pixels, uint16_t* pattern, uint8_t* pc, uint8_t* pr) +{ + 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) + { + max_c = c; + max_r = r; + max_match = 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; + return max_match; +} + +#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; + uint8_t iy = (y > 10000)?1:0; + return iy?(3-ix):ix; +} + +//MK3 +#if ((MOTHERBOARD == 303) || (MOTHERBOARD == 304)) +const int16_t PROGMEM xyzcal_point_xcoords[4] = {1200, 22000, 22000, 1200}; +const int16_t PROGMEM xyzcal_point_ycoords[4] = {600, 600, 19800, 19800}; +#endif //((MOTHERBOARD == 303) || (MOTHERBOARD == 304)) + +//MK2.5 +#if ((MOTHERBOARD == 200) || (MOTHERBOARD == 203)) +const int16_t PROGMEM xyzcal_point_xcoords[4] = {1200, 22000, 22000, 1200}; +const int16_t PROGMEM xyzcal_point_ycoords[4] = {700, 700, 19800, 19800}; +#endif //((MOTHERBOARD == 200) || (MOTHERBOARD == 203)) + +const uint16_t PROGMEM xyzcal_point_pattern[12] = {0x000, 0x0f0, 0x1f8, 0x3fc, 0x7fe, 0x7fe, 0x7fe, 0x7fe, 0x3fc, 0x1f8, 0x0f0, 0x000}; + +bool xyzcal_searchZ(void) +{ + DBG(_n("xyzcal_searchZ x=%ld y=%ld z=%ld\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS]); + int16_t x0 = _X; + int16_t y0 = _Y; + int16_t z0 = _Z; +// int16_t min_z = -6000; +// int16_t dz = 100; + int16_t z = z0; + while (z > -2300) //-6mm + 0.25mm + { + uint16_t ad = 0; + if (xyzcal_spiral8(x0, y0, z, 100, 900, 320, 1, &ad)) //dz=100 radius=900 delay=400 + { + int16_t x_on = _X; + int16_t y_on = _Y; + int16_t z_on = _Z; + DBG(_n(" ON-SIGNAL at x=%d y=%d z=%d ad=%d\n"), x_on, y_on, z_on, ad); + return true; + } + z -= 400; + } + DBG(_n("xyzcal_searchZ no signal\n x=%ld y=%ld z=%ld\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS]); + return false; +} + +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 - 128, 2400, 200, pixels); + + uint16_t* histo = (uint16_t*)(pixels + 32*32); + xyzcal_histo_pixels_32x32(pixels, histo); + + xyzcal_adjust_pixels(pixels, histo); + + uint16_t* pattern = (uint16_t*)(histo + 2*16); + for (uint8_t i = 0; i < 12; i++) + { + pattern[i] = pgm_read_word_far((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); + xyzcal_lineXYZ_to(x, y, z, 200, 0); + ret = true; + } + for (uint16_t i = 0; i < sizeof(block_t)*BLOCK_BUFFER_SIZE; i++) + pixels[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 ret = false; + st_synchronize(); + int16_t x = _X; + int16_t y = _Y; + int16_t z = _Z; + uint8_t point = xyzcal_xycoords2point(x, y); + x = pgm_read_word_far((uint16_t*)(xyzcal_point_xcoords + point)); + y = pgm_read_word_far((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()) + { + int16_t z = _Z; + xyzcal_lineXYZ_to(x, y, z, 200, 0); + if (xyzcal_scan_and_process()) + { + ret = true; + } + } + xyzcal_meassure_leave(); + return ret; +} + + +#endif //NEW_XYZCAL diff --git a/Firmware/xyzcal.h b/Firmware/xyzcal.h new file mode 100644 index 00000000..a91b1c18 --- /dev/null +++ b/Firmware/xyzcal.h @@ -0,0 +1,39 @@ +//xyzcal.h - xyz calibration with image processing +#ifndef _XYZCAL_H +#define _XYZCAL_H + +#include + + +extern void xyzcal_meassure_enter(void); + +extern void xyzcal_meassure_leave(void); + +extern bool xyzcal_lineXYZ_to(int16_t x, int16_t y, int16_t z, uint16_t delay_us, int8_t check_pinda); + +extern 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); + +extern bool xyzcal_spiral8(int16_t cx, int16_t cy, int16_t z0, int16_t dz, int16_t radius, uint16_t delay_us, int8_t check_pinda, uint16_t* pad); + +//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_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); + +extern bool xyzcal_find_bed_induction_sensor_point_xy(void); + + +#endif //_XYZCAL_H